From 6df52b7c8118129f9394f9b9b2b4291622d1c78b Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Fri, 7 Feb 2025 15:55:03 +0100 Subject: [PATCH] Implented point to point DWM action server --- .vscode/c_cpp_properties.json | 17 ++ .vscode/launch.json | 17 ++ .vscode/settings.json | 86 ++++++++ colcon_defaults.yaml | 6 + mg_msgs/CMakeLists.txt | 19 ++ mg_msgs/action/LookAt.action | 25 +++ mg_msgs/action/MovePoint.action | 25 +++ mg_msgs/action/MoveStraight.action | 24 +++ mg_msgs/msg/Point2d.msg | 2 + mg_msgs/package.xml | 22 ++ mg_navigation/CMakeLists.txt | 53 +++++ mg_navigation/include/handlers/dwm.hpp | 6 + mg_navigation/include/handlers/dwm_core.hpp | 203 ++++++++++++++++++ .../include/handlers/dwm_forward.hpp | 57 +++++ mg_navigation/include/handlers/dwm_lookat.hpp | 83 +++++++ mg_navigation/include/handlers/dwm_point.hpp | 80 +++++++ mg_navigation/include/mg_navigation.hpp | 60 ++++++ mg_navigation/package.xml | 27 +++ mg_navigation/src/mg_navigation.cpp | 86 ++++++++ 19 files changed, 898 insertions(+) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json create mode 100644 colcon_defaults.yaml create mode 100644 mg_msgs/CMakeLists.txt create mode 100644 mg_msgs/action/LookAt.action create mode 100644 mg_msgs/action/MovePoint.action create mode 100644 mg_msgs/action/MoveStraight.action create mode 100644 mg_msgs/msg/Point2d.msg create mode 100644 mg_msgs/package.xml create mode 100644 mg_navigation/CMakeLists.txt create mode 100644 mg_navigation/include/handlers/dwm.hpp create mode 100644 mg_navigation/include/handlers/dwm_core.hpp create mode 100644 mg_navigation/include/handlers/dwm_forward.hpp create mode 100644 mg_navigation/include/handlers/dwm_lookat.hpp create mode 100644 mg_navigation/include/handlers/dwm_point.hpp create mode 100644 mg_navigation/include/mg_navigation.hpp create mode 100644 mg_navigation/package.xml create mode 100644 mg_navigation/src/mg_navigation.cpp diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..7a54b71 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,17 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": true + }, + "compileCommands": "${workspaceFolder}/build/compile_commands.json", + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/g++", + "cStandard": "gnu11", + "cppStandard": "c++17" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..b406e4b --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,17 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "C++ Debugger", + "request": "launch", + "type": "cppdbg", + "miDebuggerServerAddress": "localhost:3000", + "miDebuggerPath": "/usr/bin/gdb", + "cwd": "${workspaceFolder}", + "program": "${workspaceFolder}/install/mg_navigation/lib/mg_navigation/mg_nav_server" + } + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..5aea314 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,86 @@ +{ + "files.associations": { + "bitset": "cpp", + "forward_list": "cpp", + "fstream": "cpp", + "ostream": "cpp", + "tuple": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstddef": "cpp", + "cstring": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "cctype": "cpp", + "cfenv": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "clocale": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdarg": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "expected": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "source_location": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "format": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ranges": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "span": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stdfloat": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "valarray": "cpp", + "variant": "cpp" + } +} \ No newline at end of file diff --git a/colcon_defaults.yaml b/colcon_defaults.yaml new file mode 100644 index 0000000..ef401fa --- /dev/null +++ b/colcon_defaults.yaml @@ -0,0 +1,6 @@ +{ + "build": { + "cmake-args": ["-DCMAKE_EXPORT_COMPILE_COMMANDS=1"], + "symlink-install": true + } +} \ No newline at end of file diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt new file mode 100644 index 0000000..b0c0eb2 --- /dev/null +++ b/mg_msgs/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.8) +project(mg_msgs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Point2d.msg" + "action/MoveStraight.action" + "action/MovePoint.action" + "action/LookAt.action" +) + +ament_package() \ No newline at end of file diff --git a/mg_msgs/action/LookAt.action b/mg_msgs/action/LookAt.action new file mode 100644 index 0000000..3537aef --- /dev/null +++ b/mg_msgs/action/LookAt.action @@ -0,0 +1,25 @@ +float64 x +float64 y + +float64 step 0.01 +float64 tolerance 0.001 +float64 max_wheel_speed 3 +float64 max_angular 3.14 +float64 max_vel 2 +float64 pos_mult 15 +float64 ornt_mult 4 +float64 t_ornt_mult 0.1 +float64 obst_mult_a -5 +float64 obst_mult_b -30 +float64 obst_mult_c -9 +string[] ignore_tags [] +--- +uint8 SUCCESS=0 +uint8 BLOCKED=1 +uint8 TIMEOUT=2 +uint8 MISALIGNED=3 +uint8 UNKNOWN=254 + +uint8 error +--- +float64 distance_passed \ No newline at end of file diff --git a/mg_msgs/action/MovePoint.action b/mg_msgs/action/MovePoint.action new file mode 100644 index 0000000..9912d7b --- /dev/null +++ b/mg_msgs/action/MovePoint.action @@ -0,0 +1,25 @@ +float64 x +float64 y + +float64 step 0.01 +float64 tolerance 0.001 +float64 max_wheel_speed 3 +float64 max_angular 3.14 +float64 max_vel 2 +float64 pos_mult 15 +float64 ornt_mult 4 +float64 t_ornt_mult 0.1 +float64 obst_mult_a -5 +float64 obst_mult_b -30 +float64 obst_mult_c -9 +string[] ignore_tags [] +--- +uint8 SUCCESS=0 +uint8 BLOCKED=1 +uint8 TIMEOUT=2 +uint8 MISALIGNED=3 +uint8 UNKNOWN=254 + +uint8 error +--- +float64 distance_passed \ No newline at end of file diff --git a/mg_msgs/action/MoveStraight.action b/mg_msgs/action/MoveStraight.action new file mode 100644 index 0000000..1921291 --- /dev/null +++ b/mg_msgs/action/MoveStraight.action @@ -0,0 +1,24 @@ +float64 distance + +float64 step 0.01 +float64 tolerance 0.001 +float64 max_wheel_speed 3 +float64 max_angular 3.14 +float64 max_vel 2 +float64 pos_mult 15 +float64 ornt_mult 4 +float64 t_ornt_mult 0.1 +float64 obst_mult_a -5 +float64 obst_mult_b -30 +float64 obst_mult_c -9 +string[] ignore_tags [] +--- +uint8 SUCCESS=0 +uint8 BLOCKED=1 +uint8 TIMEOUT=2 +uint8 MISALIGNED=3 +uint8 UNKNOWN=254 + +uint8 error +--- +float64 distance_passed \ No newline at end of file diff --git a/mg_msgs/msg/Point2d.msg b/mg_msgs/msg/Point2d.msg new file mode 100644 index 0000000..125c004 --- /dev/null +++ b/mg_msgs/msg/Point2d.msg @@ -0,0 +1,2 @@ +float64 x +float64 y \ No newline at end of file diff --git a/mg_msgs/package.xml b/mg_msgs/package.xml new file mode 100644 index 0000000..80ce02e --- /dev/null +++ b/mg_msgs/package.xml @@ -0,0 +1,22 @@ + + + + mg_msgs + 0.0.0 + This contains various msg/action used within the project + Pimpest + MIT + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mg_navigation/CMakeLists.txt b/mg_navigation/CMakeLists.txt new file mode 100644 index 0000000..2cab310 --- /dev/null +++ b/mg_navigation/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.8) +project(mg_navigation) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(mg_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +include(FindPkgConfig) +pkg_search_module(LIBGLM REQUIRED glm) + + + +set(PACKAGE_DEPS + rclcpp + mg_msgs + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs +) + +add_executable(mg_nav_server + src/mg_navigation.cpp +) +ament_target_dependencies(mg_nav_server ${PACKAGE_DEPS}) + +target_include_directories( + mg_nav_server + PRIVATE + ${LIBGLM_INCLUDE_DIRS} + include +) + +target_link_libraries( + mg_nav_server + ${LIBGLM_LIBRARIES} +) + +install( TARGETS + mg_nav_server + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/mg_navigation/include/handlers/dwm.hpp b/mg_navigation/include/handlers/dwm.hpp new file mode 100644 index 0000000..6788d46 --- /dev/null +++ b/mg_navigation/include/handlers/dwm.hpp @@ -0,0 +1,6 @@ +#pragma once + +#include "handlers/dwm_core.hpp" +#include "handlers/dwm_point.hpp" +#include "handlers/dwm_forward.hpp" +#include "handlers/dwm_lookat.hpp" diff --git a/mg_navigation/include/handlers/dwm_core.hpp b/mg_navigation/include/handlers/dwm_core.hpp new file mode 100644 index 0000000..eee0618 --- /dev/null +++ b/mg_navigation/include/handlers/dwm_core.hpp @@ -0,0 +1,203 @@ +#pragma once + +#include "glm/glm.hpp" +#include "mg_navigation.hpp" + +#include "tf2/LinearMath/Transform.h" +#include "tf2/convert.h" +#include + +#ifndef WHEEL_SEP +#define WHEEL_SEP 0.258 +#endif + +#ifndef WHEEL_RAD +#define WHEEL_RAD ((0.075 / 2)) +#endif + +#ifndef UPDATE_RATE +#define UPDATE_RATE 100 +#endif + +#ifndef DIM_X +#define DIM_X 8 +#endif + +#ifndef DIM_Y +#define DIM_Y 8 +#endif + +namespace mg { + + template + class DWM { + public: + using Goal = typename T::Goal; + using Result = typename T::Result; + using Feedback = typename T::Feedback; + using GoalHandle = typename rclcpp_action::ServerGoalHandle; + using TwistPub = rclcpp::Publisher; + + enum DwmError { + SUCCESS, + BLOCKED, + TIMEOUT, + MISALIGNED, + + UNKNOWN = 254 + }; + + MgNavigationServer& baseNode; // NOLINT + std::shared_ptr hgoal; + TwistPub::SharedPtr pub_twist; + typename Goal::ConstSharedPtr goal; + tf2_ros::Buffer::SharedPtr tfbuf; + ulong dimx = DIM_X, dimy = DIM_Y; + int cvl = 0, cvr = 0; + double step; + + glm::dvec2 target_pos; + double target_ornt = 0; + + glm::dvec2 pos; + double theta = 0; + + DWM(std::shared_ptr, tf2_ros::Buffer::SharedPtr&, MgNavigationServer&); + + void execute(); + + protected: + void target_init() { target_update(); } + bool target_check() { return false; } + void target_update() { } + bool condition_check(); + void populate_candidates(std::vector& vl, std::vector& vr, int dimx, int dimy); + double calc_score(int vl, int vr); + + void abort(int error); + void succeed(); + void cancel(); + + void pos_query(); + void send_speed(double vl, double vr); + + static std::pair to_vel(double vl, double vr) { + return std::make_pair(WHEEL_RAD * (vl + vr) / 2.0, // NOLINT + WHEEL_RAD * (vr - vl) / WHEEL_SEP); + } + }; + + template + DWM::DWM(std::shared_ptr g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) : + baseNode(mns), + hgoal(g), + pub_twist(mns.pub_twist), + goal(g->get_goal()), + tfbuf(buf), + step(g->get_goal()->step), + target_pos(0, 0), + pos(0, 0) { } + + template + void DWM::execute() { + std::vector spacevl(dimx * dimy); + std::vector spacevr(dimx * dimy); + + pos_query(); + target_init(); + + rclcpp::Rate rate(UPDATE_RATE); + + while (target_check() && rclcpp::ok()) { + if (hgoal->is_canceling()) { + cancel(); + return; + } + + if (condition_check()) { + return; + } + + pos_query(); + populate_candidates(spacevl, spacevr, dimx, dimy); + + double b_score = calc_score(spacevl[0], spacevr[0]); + uint b_ind = 0; + + for (uint i = 1; i < spacevl.size(); i++) { + const double score = calc_score(spacevl[i], spacevr[i]); + if (score > b_score) { + b_score = score; + b_ind = i; + } + } + + calc_score(spacevl[b_ind], spacevr[b_ind]); // Called here again so it sticks out in logs + + cvl = spacevl[b_ind]; + cvr = spacevr[b_ind]; + send_speed(step * cvl, step * cvr); + + rate.sleep(); + } + succeed(); + } + + template + void DWM::pos_query() { + try { + double x = NAN; + double y = NAN; + + auto tmsg = tfbuf->lookupTransform("odom", "base-link", tf2::TimePointZero); + + tf2::Transform t; + tf2::convert(tmsg.transform, t); + t.getBasis().getRPY(x, y, theta); + auto vec3 = tmsg.transform.translation; + + pos = glm::dvec2(vec3.x, vec3.y); + + } catch (const tf2::TransformException& e) { + RCLCPP_ERROR(baseNode.get_logger(), "Got following error when looking up transform:\n\t%s", e.what()); + } + } + + template + void DWM::send_speed(double vl, double vr) { + auto [v, w] = to_vel(vl, vr); + Geometry::TwistStamped twist; + twist.twist.angular.z = w; + twist.twist.linear.x = v; + + pub_twist->publish(twist); + } + + template + void DWM::abort(int error) { + baseNode.mtx.lock(); + send_speed(0, 0); + auto x = std::make_shared(); + x->error = error; + hgoal->abort(x); + } + + template + void DWM::succeed() { + baseNode.mtx.lock(); + send_speed(0, 0); + auto x = std::make_shared(); + x->error = SUCCESS; + hgoal->succeed(x); + } + + template + void DWM::cancel() { + baseNode.mtx.lock(); + send_speed(0, 0); + auto x = std::make_shared(); + x->error = UNKNOWN; + hgoal->canceled(x); + } + +}; // namespace mg \ No newline at end of file diff --git a/mg_navigation/include/handlers/dwm_forward.hpp b/mg_navigation/include/handlers/dwm_forward.hpp new file mode 100644 index 0000000..5b32674 --- /dev/null +++ b/mg_navigation/include/handlers/dwm_forward.hpp @@ -0,0 +1,57 @@ +#pragma once +#include "handlers/dwm_core.hpp" + +#include "glm/gtx/norm.hpp" +#include "glm/gtx/rotate_vector.hpp" +#include "glm/gtx/vector_angle.hpp" + +namespace mg { + + template <> + inline bool DWM::target_check() { + return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance; + } + + template <> + inline void DWM::target_init() { + target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos; + } + + template <> + inline bool DWM::condition_check() { + return false; + } + + template <> + inline double DWM::calc_score(int vl, int vr) { + constexpr double delta = 0.8; + + auto [v, w] = to_vel(step * vl, step * vr); + + const glm::dvec2 dp = glm::rotate(glm::dvec2(1, 0), theta) * delta * v; + const glm::dvec2 n_pos = dp + pos; + + const double score = goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos)); + + return score; + } + + template <> + inline void DWM::populate_candidates(std::vector& vl, + std::vector& vr, + int dimx, + int) { + vl.clear(); + vr.clear(); + + for (int i = -dimx / 2; i <= dimx / 2; i++) { + auto [v, w] = to_vel(step * (cvl + i), step * (cvr + i)); + + if (step * abs(cvl + i) <= goal->max_wheel_speed && glm::abs(v) < goal->max_vel) { + vl.push_back(cvl + i); + vr.push_back(cvr + i); + } + } + } + +} \ No newline at end of file diff --git a/mg_navigation/include/handlers/dwm_lookat.hpp b/mg_navigation/include/handlers/dwm_lookat.hpp new file mode 100644 index 0000000..8bc3fe7 --- /dev/null +++ b/mg_navigation/include/handlers/dwm_lookat.hpp @@ -0,0 +1,83 @@ +#pragma once +#include "handlers/dwm_core.hpp" + +#include "glm/gtx/norm.hpp" +#include "glm/gtx/rotate_vector.hpp" +#include "glm/gtx/vector_angle.hpp" +#include + +namespace mg { + + template <> + inline bool DWM::target_check() { + + const double a = glm::abs(theta - target_ornt); + const double b = (a > glm::pi()) ? glm::two_pi() - a : a; + + return b > goal->tolerance; + } + + template <> + inline void DWM::target_init() { + + target_pos = glm::vec2(goal->x, goal->y); + + const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos)); + const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z; + const double sign = (cross < 0) ? -1 : 1; + + target_ornt = angle * sign; + } + + template <> + inline void DWM::target_update() { + + const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos)); + const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z; + const double sign = (cross < 0) ? -1 : 1; + + target_ornt = angle * sign; + } + + template <> + inline bool DWM::condition_check() { + return false; + } + + template <> + inline double DWM::calc_score(int vl, int vr) { + constexpr double delta = 0.5; + const auto [v, w] = to_vel(step * vl, step * vr); + const double n_theta = theta + w * delta; + + double dist_old = glm::abs(target_ornt - theta); + double dist_new = glm::abs(target_ornt - n_theta); + + dist_old = (dist_old > glm::pi()) ? glm::two_pi() - dist_old : dist_old; + dist_new = (dist_new > glm::pi()) ? glm::two_pi() - dist_new : dist_new; + + const double score = goal->ornt_mult * (dist_old - dist_new); + + return score; + } + + template <> + inline void DWM::populate_candidates(std::vector& vl, + std::vector& vr, + int dimx, + int) { + vl.clear(); + vr.clear(); + + for (int i = -dimx / 2; i <= dimx / 2; i++) { + auto [v, w] = to_vel(step * (cvl + i), step * (cvr + i)); + + if (step * abs(cvl - i) <= goal->max_wheel_speed && step * abs(cvr + i) <= goal->max_wheel_speed + && glm::abs(w) < goal->max_angular) { + vl.push_back(cvl - i); + vr.push_back(cvr + i); + } + } + } + +} \ No newline at end of file diff --git a/mg_navigation/include/handlers/dwm_point.hpp b/mg_navigation/include/handlers/dwm_point.hpp new file mode 100644 index 0000000..a6f69cc --- /dev/null +++ b/mg_navigation/include/handlers/dwm_point.hpp @@ -0,0 +1,80 @@ +#pragma once +#include "handlers/dwm_core.hpp" + +#include "glm/gtx/norm.hpp" +#include "glm/gtx/rotate_vector.hpp" +#include "glm/gtx/vector_angle.hpp" + +namespace mg { + + template <> + inline bool DWM::target_check() { + return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance; + } + + template <> + inline void DWM::target_update() { + target_pos = glm::dvec2(goal->x, goal->y); + } + + template <> + inline bool DWM::condition_check() { + return false; + } + + template <> + inline double DWM::calc_score(int vl, int vr) { + auto [v, w] = to_vel(step * vl, step * vr); + const double delta = 0.5; + glm::dvec2 n_pos; + double n_theta = NAN; + double score = 0; + + n_theta = w * delta; + + if (n_theta <= 1e-6) { //NOLINT + n_theta += theta; + const glm::dvec2 dp = glm::rotate(glm::dvec2(1, 0), theta) * delta * v; + n_pos = dp + pos; + } else { + n_theta += theta; + const double r = v / w; + n_pos.x = r * (glm::sin(n_theta) - glm::sin(theta)); + n_pos.y = -r * (glm::cos(n_theta) - glm::cos(theta)); + n_pos += pos; + } + + const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta); + const glm::dvec2 face = glm::rotate(glm::dvec2(1, 0), theta); + + const double angl = glm::angle(face, glm::normalize(target_pos - pos)); + const double n_angl = glm::angle(n_face, glm::normalize(target_pos - n_pos)); + + score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos)); + score += goal->ornt_mult * (angl - n_angl); + + return score; + } + + template <> + inline void DWM::populate_candidates(std::vector& vl, + std::vector& vr, + int dimx, + int dimy) { + vl.clear(); + vr.clear(); + + for (int i = -dimx / 2; i <= dimx / 2; i++) { + for (int j = -dimy / 2; j <= dimy / 2; j++) { + auto [v, w] = to_vel(step * (cvl + i), step * (cvr + j)); + + if (step * abs(cvl + i) <= goal->max_wheel_speed && step * abs(cvr + j) <= goal->max_wheel_speed + && glm::abs(v) < goal->max_vel && glm::abs(w) < goal->max_angular) { + vl.push_back(cvl + i); + vr.push_back(cvr + j); + } + } + } + } + +} \ No newline at end of file diff --git a/mg_navigation/include/mg_navigation.hpp b/mg_navigation/include/mg_navigation.hpp new file mode 100644 index 0000000..c8787f7 --- /dev/null +++ b/mg_navigation/include/mg_navigation.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "mg_msgs/action/move_point.hpp" +#include "mg_msgs/action/move_straight.hpp" +#include "mg_msgs/action/look_at.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + +namespace mg { + + namespace Geometry = geometry_msgs::msg; + + class MgNavigationServer : public rclcpp::Node { + public: + RCLCPP_SMART_PTR_ALIASES_ONLY(MgNavigationServer) + using MovePoint = mg_msgs::action::MovePoint; + using MoveStraight = mg_msgs::action::MoveStraight; + using LookAt = mg_msgs::action::LookAt; + + rclcpp::Publisher::SharedPtr pub_twist; + + rclcpp_action::Server::SharedPtr sv_look_at; + rclcpp_action::Server::SharedPtr sv_move_point; + rclcpp_action::Server::SharedPtr sv_move_straight; + + rclcpp::Subscription::SharedPtr tf2_subscription_; + tf2_ros::Buffer::SharedPtr tf2_buffer; + std::shared_ptr tf2_listener; + std::mutex mtx; + + MgNavigationServer(rclcpp::NodeOptions& opts); + + private: + bool is_processing = false; + + template + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&, + typename T::Goal::ConstSharedPtr, + const char*); + template + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr >, + const char*); + template + void handle_accepted(const std::shared_ptr >, const char*); + + template + void execute(const std::shared_ptr >); + }; + +}; \ No newline at end of file diff --git a/mg_navigation/package.xml b/mg_navigation/package.xml new file mode 100644 index 0000000..7cee11e --- /dev/null +++ b/mg_navigation/package.xml @@ -0,0 +1,27 @@ + + + + mg_navigation + 0.0.0 + TODO: Package description + Pimpest + Apache 2.0 + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_components + mg_msgs + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + + + libglm-dev + + + ament_cmake + + \ No newline at end of file diff --git a/mg_navigation/src/mg_navigation.cpp b/mg_navigation/src/mg_navigation.cpp new file mode 100644 index 0000000..c1da57b --- /dev/null +++ b/mg_navigation/src/mg_navigation.cpp @@ -0,0 +1,86 @@ +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "mg_navigation.hpp" +#include "handlers/dwm.hpp" + +namespace mg { + + template + rclcpp_action::GoalResponse MgNavigationServer::handle_goal(const rclcpp_action::GoalUUID& /*unused*/, + typename T::Goal::ConstSharedPtr /*unused*/, + const char* N) { + RCLCPP_INFO(get_logger(), "Recieved goal for action: [%s]", N); + const std::lock_guard lock(mtx); + if (is_processing) { + return rclcpp_action::GoalResponse::REJECT; + } else { + is_processing = true; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + } + + template + rclcpp_action::CancelResponse MgNavigationServer::handle_cancel( + const std::shared_ptr >, const char* N) { + RCLCPP_INFO(this->get_logger(), "Recieved cancel request for action: [%s]", N); + return rclcpp_action::CancelResponse::ACCEPT; + } + + template + void MgNavigationServer::handle_accepted(const std::shared_ptr > gh, + const char* N) { + RCLCPP_INFO(this->get_logger(), "Goal accepted for action: [%s]", N); + using namespace std::placeholders; + auto t = std::bind(&MgNavigationServer::execute, this, _1); + std::thread{ t, gh }.detach(); + } + + template + void MgNavigationServer::execute(const std::shared_ptr > gh) { + DWM dwm = DWM(gh, tf2_buffer, *this); + dwm.execute(); + is_processing = false; + mtx.unlock(); + } + + MgNavigationServer::MgNavigationServer(rclcpp::NodeOptions& opts) : rclcpp::Node("MgNavigationServer", opts) { + tf2_buffer = std::make_shared(get_clock()); + tf2_listener = std::make_shared(*tf2_buffer); + + pub_twist = create_publisher("diffdrive_controller/cmd_vel", 2); + + using namespace std::placeholders; + sv_move_point = rclcpp_action::create_server( + this, + "MovePoint", + std::bind(&MgNavigationServer::handle_goal, this, _1, _2, "MovePoint"), + std::bind(&MgNavigationServer::handle_cancel, this, _1, "MovePoint"), + std::bind(&MgNavigationServer::handle_accepted, this, _1, "MovePoint")); + + sv_look_at = rclcpp_action::create_server( + this, + "LookAt", + std::bind(&MgNavigationServer::handle_goal, this, _1, _2, "LookAt"), + std::bind(&MgNavigationServer::handle_cancel, this, _1, "LookAt"), + std::bind(&MgNavigationServer::handle_accepted, this, _1, "LookAt")); + + sv_move_straight = rclcpp_action::create_server( + this, + "MoveStraight", + std::bind(&MgNavigationServer::handle_goal, this, _1, _2, "MoveStraight"), + std::bind(&MgNavigationServer::handle_cancel, this, _1, "MoveStraight"), + std::bind(&MgNavigationServer::handle_accepted, this, _1, "MoveStraight")); + } + +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions opts; + rclcpp::spin(std::make_shared(opts)); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file