From 200c4fade514b2f3ae0a8a7180da2cb65fcb6cb7 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Mon, 10 Mar 2025 15:19:35 +0100 Subject: [PATCH 1/5] Added msgs intended for gui --- mg_msgs/CMakeLists.txt | 2 ++ mg_msgs/msg/TEdit.msg | 4 ++++ mg_msgs/msg/TEditCycle.msg | 6 ++++++ 3 files changed, 12 insertions(+) create mode 100644 mg_msgs/msg/TEdit.msg create mode 100644 mg_msgs/msg/TEditCycle.msg diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt index 23c7e0d..7009768 100644 --- a/mg_msgs/CMakeLists.txt +++ b/mg_msgs/CMakeLists.txt @@ -11,6 +11,8 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/Point2d.msg" + "msg/TEdit.msg" + "msg/TEditCycle.msg" "action/MoveStraight.action" "action/MovePoint.action" "action/LookAt.action" diff --git a/mg_msgs/msg/TEdit.msg b/mg_msgs/msg/TEdit.msg new file mode 100644 index 0000000..4ef056d --- /dev/null +++ b/mg_msgs/msg/TEdit.msg @@ -0,0 +1,4 @@ + +uint8 type +uint16 idx +uint16 idy diff --git a/mg_msgs/msg/TEditCycle.msg b/mg_msgs/msg/TEditCycle.msg new file mode 100644 index 0000000..d394ade --- /dev/null +++ b/mg_msgs/msg/TEditCycle.msg @@ -0,0 +1,6 @@ + +mg_msgs/TEdit[<=100] modifications +mg_msgs/Point2d[<=100] new_points + +bool DELETE=1 +bool ADD=0 \ No newline at end of file From 0fffe915ca383f8d636b5164d859781eded9e09d Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Tue, 18 Mar 2025 19:37:26 +0100 Subject: [PATCH 2/5] Changed to fixed array due to message loaning --- mg_msgs/msg/TEditCycle.msg | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/mg_msgs/msg/TEditCycle.msg b/mg_msgs/msg/TEditCycle.msg index d394ade..e9af1cc 100644 --- a/mg_msgs/msg/TEditCycle.msg +++ b/mg_msgs/msg/TEditCycle.msg @@ -1,6 +1,8 @@ -mg_msgs/TEdit[<=100] modifications -mg_msgs/Point2d[<=100] new_points +mg_msgs/TEdit[100] modifications +mg_msgs/Point2d[100] new_points +uint8 cmodifications 0 +uint8 cnew_points 0 -bool DELETE=1 -bool ADD=0 \ No newline at end of file +uint8 DELETE=1 +uint8 ADD=0 \ No newline at end of file From ec831bd334835328a6764318dd5de15f4d82def0 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Thu, 17 Apr 2025 19:24:19 +0200 Subject: [PATCH 3/5] Add a-star based global planner --- mg_msgs/CMakeLists.txt | 3 + mg_msgs/action/MoveGlobal.action | 25 +++++ mg_msgs/msg/Path.msg | 1 + mg_msgs/srv/CalcPath.srv | 4 + mg_navigation/CMakeLists.txt | 1 + mg_navigation/include/handlers/dwa.hpp | 1 + mg_navigation/include/handlers/dwa_global.hpp | 89 ++++++++++++++++++ mg_navigation/include/mg_navigation.hpp | 8 ++ mg_navigation/include/path_buffer.hpp | 60 ++++++++++++ mg_navigation/src/mg_navigation.cpp | 10 ++ mg_navigation/src/path_buffer.cpp | 78 ++++++++++++++++ mg_planner/CMakeLists.txt | 58 ++++++++++++ .../include/mg_planner/a_star/a_star.hpp | 72 ++++++++++++++ mg_planner/include/mg_planner/config.hpp | 24 +++++ .../include/mg_planner/mg_planner_node.hpp | 41 ++++++++ .../include/mg_planner/obstacleManager.hpp | 16 ++++ mg_planner/package.xml | 25 +++++ mg_planner/src/a_star.cpp | 93 +++++++++++++++++++ mg_planner/src/mg_planner.cpp | 11 +++ mg_planner/src/mg_planner_node.cpp | 58 ++++++++++++ mg_planner/src/obstacleManager.cpp | 8 ++ 21 files changed, 686 insertions(+) create mode 100644 mg_msgs/action/MoveGlobal.action create mode 100644 mg_msgs/msg/Path.msg create mode 100644 mg_msgs/srv/CalcPath.srv create mode 100644 mg_navigation/include/handlers/dwa_global.hpp create mode 100644 mg_navigation/include/path_buffer.hpp create mode 100644 mg_navigation/src/path_buffer.cpp create mode 100644 mg_planner/CMakeLists.txt create mode 100644 mg_planner/include/mg_planner/a_star/a_star.hpp create mode 100644 mg_planner/include/mg_planner/config.hpp create mode 100644 mg_planner/include/mg_planner/mg_planner_node.hpp create mode 100644 mg_planner/include/mg_planner/obstacleManager.hpp create mode 100644 mg_planner/package.xml create mode 100644 mg_planner/src/a_star.cpp create mode 100644 mg_planner/src/mg_planner.cpp create mode 100644 mg_planner/src/mg_planner_node.cpp create mode 100644 mg_planner/src/obstacleManager.cpp diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt index 7009768..5bcd36f 100644 --- a/mg_msgs/CMakeLists.txt +++ b/mg_msgs/CMakeLists.txt @@ -10,13 +10,16 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Path.msg" "msg/Point2d.msg" "msg/TEdit.msg" "msg/TEditCycle.msg" + "action/MoveGlobal.action" "action/MoveStraight.action" "action/MovePoint.action" "action/LookAt.action" "action/Rotate.action" + "srv/CalcPath.srv" "srv/SendDouble.srv" ) diff --git a/mg_msgs/action/MoveGlobal.action b/mg_msgs/action/MoveGlobal.action new file mode 100644 index 0000000..32080d0 --- /dev/null +++ b/mg_msgs/action/MoveGlobal.action @@ -0,0 +1,25 @@ +float64[] x +float64[] y + +float64 step 0.01 +float64 tolerance 0.5 +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/Path.msg b/mg_msgs/msg/Path.msg new file mode 100644 index 0000000..8f47b8d --- /dev/null +++ b/mg_msgs/msg/Path.msg @@ -0,0 +1 @@ +int32[] indicies \ No newline at end of file diff --git a/mg_msgs/srv/CalcPath.srv b/mg_msgs/srv/CalcPath.srv new file mode 100644 index 0000000..4169ee6 --- /dev/null +++ b/mg_msgs/srv/CalcPath.srv @@ -0,0 +1,4 @@ +float64[] x +float64[] y +--- +int32[] indicies \ No newline at end of file diff --git a/mg_navigation/CMakeLists.txt b/mg_navigation/CMakeLists.txt index 3d61058..b1005ce 100644 --- a/mg_navigation/CMakeLists.txt +++ b/mg_navigation/CMakeLists.txt @@ -30,6 +30,7 @@ set(PACKAGE_DEPS add_executable(mg_nav_server src/mg_navigation.cpp + src/path_buffer.cpp ) ament_target_dependencies(mg_nav_server ${PACKAGE_DEPS}) diff --git a/mg_navigation/include/handlers/dwa.hpp b/mg_navigation/include/handlers/dwa.hpp index 321d3a6..97d9520 100644 --- a/mg_navigation/include/handlers/dwa.hpp +++ b/mg_navigation/include/handlers/dwa.hpp @@ -2,6 +2,7 @@ #include "handlers/dwa_core.hpp" #include "handlers/dwa_point.hpp" +#include "handlers/dwa_global.hpp" #include "handlers/dwa_forward.hpp" #include "handlers/dwa_lookat.hpp" #include "handlers/dwa_rotate.hpp" diff --git a/mg_navigation/include/handlers/dwa_global.hpp b/mg_navigation/include/handlers/dwa_global.hpp new file mode 100644 index 0000000..5b1bce1 --- /dev/null +++ b/mg_navigation/include/handlers/dwa_global.hpp @@ -0,0 +1,89 @@ +#pragma once +#include "handlers/dwa_core.hpp" + +#include "glm/gtx/norm.hpp" +#include "glm/gtx/rotate_vector.hpp" +#include "glm/gtx/vector_angle.hpp" + +#define LOOKAHEAD 0.2 + +namespace mg { + + template <> + inline bool DWA::target_check() { + return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance; + } + + template <> + inline void DWA::target_init() { + baseNode.path_buffer()->update_path_block(goal); + target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD); + } + + template <> + inline void DWA::target_update() { + baseNode.path_buffer()->update_path(goal); + target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD); + } + + template <> + inline bool DWA::condition_check() { + return false; + } + + template <> + inline double DWA::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 DWA::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 index 7c92935..e535620 100644 --- a/mg_navigation/include/mg_navigation.hpp +++ b/mg_navigation/include/mg_navigation.hpp @@ -17,6 +17,8 @@ #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" +#include "path_buffer.hpp" + namespace mg { namespace Geometry = geometry_msgs::msg; @@ -25,12 +27,14 @@ namespace mg { public: RCLCPP_SMART_PTR_ALIASES_ONLY(MgNavigationServer) using MovePoint = mg_msgs::action::MovePoint; + using MoveGlobal = mg_msgs::action::MoveGlobal; using MoveStraight = mg_msgs::action::MoveStraight; using LookAt = mg_msgs::action::LookAt; using Rotate = mg_msgs::action::Rotate; rclcpp::Publisher::SharedPtr pub_twist; + rclcpp_action::Server::SharedPtr sv_move_global; rclcpp_action::Server::SharedPtr sv_move_point; rclcpp_action::Server::SharedPtr sv_move_straight; rclcpp_action::Server::SharedPtr sv_look_at; @@ -43,9 +47,13 @@ namespace mg { MgNavigationServer(rclcpp::NodeOptions& opts); + std::shared_ptr path_buffer() { return path_buffer_; } + private: bool is_processing = false; + std::shared_ptr path_buffer_; + template rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&, typename T::Goal::ConstSharedPtr, diff --git a/mg_navigation/include/path_buffer.hpp b/mg_navigation/include/path_buffer.hpp new file mode 100644 index 0000000..3f7dcb5 --- /dev/null +++ b/mg_navigation/include/path_buffer.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include +#include + +#include "mg_msgs/srv/calc_path.hpp" +#include "mg_msgs/action/move_global.hpp" +#include "rclcpp/rclcpp.hpp" + +#define AREAX 3000 +#define AREAY 2000 +#define GRID_Y 66 +#define GRID_X 106 + +#define MIN_X 175 +#define MIN_Y 175 + +#define MAX_X 2825 +#define MAX_Y 1825 + +#define GRID_X_SIZE ((MAX_X - MIN_X) / GRID_X) +#define GRID_Y_SIZE ((MAX_Y - MIN_Y) / GRID_Y) + +namespace mg { + inline glm::vec2 GridToCoords(const glm::ivec2 grid) { + return glm::vec2{ grid.x, grid.y } * glm::vec2{ GRID_X_SIZE, GRID_Y_SIZE } + glm::vec2{ MIN_X, MIN_Y }; + } + inline glm::ivec2 IdToGrid(const int id) { return { id % GRID_X, id / GRID_X }; } + inline glm::vec2 IdToCoords(const int id) { return GridToCoords(IdToGrid(id)); } + + class PathBuffer { + using PathService = mg_msgs::srv::CalcPath; + using PathGoal = mg_msgs::action::MoveGlobal::Goal::ConstSharedPtr; + using PathFuture = rclcpp::Client::SharedFuture; + + public: + PathBuffer(rclcpp::Node* node); + // ~PathBuffer(); + + glm::vec2 get_next(glm::vec2 pos, const double lookahead); + + bool update_path(PathGoal goal = nullptr); + void update_path_block(PathGoal goal = nullptr); + + private: + int current; + + rclcpp::Node* node_; + + PathGoal goal_; + PathService::Response::ConstSharedPtr path_msg_; + + rclcpp::Client::Client::SharedPtr client_; + + PathFuture resp_; + }; + +} \ No newline at end of file diff --git a/mg_navigation/src/mg_navigation.cpp b/mg_navigation/src/mg_navigation.cpp index ff925b0..9251e7f 100644 --- a/mg_navigation/src/mg_navigation.cpp +++ b/mg_navigation/src/mg_navigation.cpp @@ -50,9 +50,19 @@ namespace mg { tf2_buffer = std::make_shared(get_clock()); tf2_listener = std::make_shared(*tf2_buffer); + path_buffer_ = std::make_shared(this); + pub_twist = create_publisher("diffdrive_controller/cmd_vel", 2); using namespace std::placeholders; + + sv_move_global = rclcpp_action::create_server( + this, + "MoveGlobal", + std::bind(&MgNavigationServer::handle_goal, this, _1, _2, "MoveGlobal"), + std::bind(&MgNavigationServer::handle_cancel, this, _1, "MoveGlobal"), + std::bind(&MgNavigationServer::handle_accepted, this, _1, "MoveGlobal")); + sv_move_point = rclcpp_action::create_server( this, "MovePoint", diff --git a/mg_navigation/src/path_buffer.cpp b/mg_navigation/src/path_buffer.cpp new file mode 100644 index 0000000..b0991d9 --- /dev/null +++ b/mg_navigation/src/path_buffer.cpp @@ -0,0 +1,78 @@ +#include "path_buffer.hpp" + +#include + +namespace mg { + + PathBuffer::PathBuffer(rclcpp::Node* node) : current(-1), node_(node) { + client_ = node_->create_client("GeneratePath"); + } + + glm::vec2 PathBuffer::get_next(glm::vec2 pos, const double lookahead) { + if (current < 0) { + current++; + while (current < (int)path_msg_->indicies.size() + && glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) > lookahead * lookahead) { + current++; + } + } + while (current < (int)path_msg_->indicies.size() + && glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) < lookahead * lookahead) { + current++; + } + + if (current == (int)path_msg_->indicies.size()) { + double best = 10000000; + int id = 0; + + for (int i = 0; i < (int)goal_->x.size(); i++) { + const double dist2 = glm::distance2({ goal_->x[i], goal_->y[i] }, pos); + if (dist2 < best) { + best = dist2; + id = i; + } + } + return { goal_->x[id], goal_->y[id] }; + } + return IdToCoords(path_msg_->indicies[current]); + } + + bool PathBuffer::update_path(PathGoal goal) { + if (goal) { + goal_ = goal; + client_->prune_pending_requests(); + auto req = std::make_shared(); + req->x = goal_->x; + req->y = goal_->y; + resp_ = client_->async_send_request(req).share(); + } + if (resp_.valid()) { + path_msg_ = resp_.get(); + auto req = std::make_shared(); + req->x = goal_->x; + req->y = goal_->y; + resp_ = client_->async_send_request(req).share(); + current = -1; + return true; + } + return false; + } + + void PathBuffer::update_path_block(PathGoal goal) { + if (goal) { + goal_ = goal; + client_->prune_pending_requests(); + auto req = std::make_shared(); + req->x = goal_->x; + req->y = goal_->y; + resp_ = client_->async_send_request(req).share(); + } + + path_msg_ = resp_.get(); + auto req = std::make_shared(); + req->x = goal_->x; + req->y = goal_->y; + resp_ = client_->async_send_request(req).share(); + current = -1; + } +} \ No newline at end of file diff --git a/mg_planner/CMakeLists.txt b/mg_planner/CMakeLists.txt new file mode 100644 index 0000000..dca5646 --- /dev/null +++ b/mg_planner/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.8) +project(mg_planner) + +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_planner + src/mg_planner.cpp + src/mg_planner_node.cpp + src/obstacleManager.cpp + src/a_star.cpp +) + +ament_target_dependencies(mg_planner ${PACKAGE_DEPS}) + +target_include_directories( + mg_planner + PRIVATE + ${LIBGLM_INCLUDE_DIRS} + include +) + +target_link_libraries( + mg_planner + ${LIBGLM_LIBRARIES} +) + +install( TARGETS + mg_planner + DESTINATION lib/${PROJECT_NAME} +) + +target_compile_features(mg_planner PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +ament_package() + diff --git a/mg_planner/include/mg_planner/a_star/a_star.hpp b/mg_planner/include/mg_planner/a_star/a_star.hpp new file mode 100644 index 0000000..547f913 --- /dev/null +++ b/mg_planner/include/mg_planner/a_star/a_star.hpp @@ -0,0 +1,72 @@ +#pragma once + +#include "mg_planner/config.hpp" +#include "rclcpp/node.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#define GRID_X_SIZE ((MAX_X - MIN_X) / GRID_X) +#define GRID_Y_SIZE ((MAX_Y - MIN_Y) / GRID_Y) + +namespace mg { + + class PlannerNode; + + constexpr std::array neighbors{ glm::ivec2(1, 0), glm::ivec2(1, 1), glm::ivec2(1, 2), + glm::ivec2(1, 3), glm::ivec2(2, 1), glm::ivec2(2, 3), + glm::ivec2(3, 1), glm::ivec2(3, 2), glm::ivec2(0, 1) }; + + constexpr std::array directions{ + glm::ivec2(1, 1), glm::ivec2(1, -1), glm::ivec2(-1, -1), glm::ivec2(-1, 1) + }; + + static inline std::array precalc_Distances() { + std::array dist{}; + for (int i = 0; i < 9; i++) { dist[i] = glm::fastLength((glm::vec2)neighbors[i]); } + return dist; + } + + const std::array distances = precalc_Distances(); + + class AStar { + public: + friend mg::PlannerNode; + AStar() = delete; + AStar(AStar&&) = delete; + AStar(AStar&) = delete; + + static glm::ivec2 CoordsToGrid(const glm::ivec2 pos) { + return (pos - glm::ivec2{ MIN_X, MIN_Y }) / glm::ivec2{ GRID_X_SIZE, GRID_Y_SIZE }; + } + + static glm::ivec2 GridToCoords(const glm::ivec2 grid) { + return grid * glm::ivec2{ GRID_X_SIZE, GRID_Y_SIZE } + glm::ivec2{ MIN_X, MIN_Y }; + } + + static glm::ivec2 IdToGrid(const int id) { return { id % GRID_X, id / GRID_X }; } + static int GridToId(const glm::ivec2 grid) { return grid.x + GRID_X * grid.y; } + + static glm::ivec2 IdToCoords(const int id) { return GridToCoords(IdToGrid(id)); } + static int CoordsToId(const glm::ivec2 pos) { return GridToId(CoordsToGrid(pos)); } + + static void gen_path(int start, std::array& parents, int end, std::vector& path); + float dist_to_goal(glm::ivec2 pos); + + void execute(glm::ivec2 pos, std::vector&& goals, std::vector& path); + + bool isRunning() const { return running; } + + private: + AStar(PlannerNode* node) : node_(node) { } + + bool running; + PlannerNode* node_; + std::vector targets_; + }; +} \ No newline at end of file diff --git a/mg_planner/include/mg_planner/config.hpp b/mg_planner/include/mg_planner/config.hpp new file mode 100644 index 0000000..f4aa3f5 --- /dev/null +++ b/mg_planner/include/mg_planner/config.hpp @@ -0,0 +1,24 @@ +#pragma once + +#define AREAX 3000 +#define AREAY 2000 +#define GRID_Y 66ul +#define GRID_X 106ul + +#define MIN_X 175 +#define MIN_Y 175 + +#define MAX_X 2825 +#define MAX_Y 1825 + +#define SEARCH_DISTANCE 5 + +#define CLOSE 100000 +#define MAXOBSTC 50 +#define CLOSEDIST 110 +#define GOALDELTA2 2500 +#define ROBOTRADIUS 165 + +#define ELIPSEPERCENT 0.5 +#define GOALPERCENT 0.1 +#define MAXDTIME 0.1 \ No newline at end of file diff --git a/mg_planner/include/mg_planner/mg_planner_node.hpp b/mg_planner/include/mg_planner/mg_planner_node.hpp new file mode 100644 index 0000000..67fa278 --- /dev/null +++ b/mg_planner/include/mg_planner/mg_planner_node.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "mg_planner/a_star/a_star.hpp" +#include "mg_planner/obstacleManager.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "mg_msgs/srv/calc_path.hpp" + +#define TREE_SIZE 9 + +namespace mg { + + class PlannerNode : public rclcpp::Node { + public: + PlannerNode(); + + void fill_obstacles() { } + + ObstacleManager& getObstacleManager() { return obstacle_manager_; } + + glm::ivec2 get_pos(); + + private: + rclcpp::Service::SharedPtr astar_service; + + AStar astar_; + ObstacleManager obstacle_manager_; + + std::shared_ptr tf_buf_; + std::shared_ptr tf_listener; + }; + +} \ No newline at end of file diff --git a/mg_planner/include/mg_planner/obstacleManager.hpp b/mg_planner/include/mg_planner/obstacleManager.hpp new file mode 100644 index 0000000..863f3b6 --- /dev/null +++ b/mg_planner/include/mg_planner/obstacleManager.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include +#include + +namespace mg { + class ObstacleManager { + const std::vector static_obstacles_; + const std::vector permanent_obstacles_; + + glm::vec2 dynamic_obstacle_; + + public: + bool check_collision(glm::ivec2 pos, float size); + }; +} \ No newline at end of file diff --git a/mg_planner/package.xml b/mg_planner/package.xml new file mode 100644 index 0000000..b3820f5 --- /dev/null +++ b/mg_planner/package.xml @@ -0,0 +1,25 @@ + + + + mg_planner + 0.0.0 + TODO: Package description + Pimpest + Apache 2.0 + + ament_cmake + + rclcpp + rclcpp_action + mg_msgs + tf2 + tf2_ros + tf2_geometry_msgs + + + libglm-dev + + + ament_cmake + + \ No newline at end of file diff --git a/mg_planner/src/a_star.cpp b/mg_planner/src/a_star.cpp new file mode 100644 index 0000000..e187397 --- /dev/null +++ b/mg_planner/src/a_star.cpp @@ -0,0 +1,93 @@ +#include "mg_planner/a_star/a_star.hpp" +#include "mg_planner/config.hpp" +#include "mg_planner/mg_planner_node.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mg { + + void AStar::execute(glm::ivec2 pos, std::vector&& goals, std::vector& path) { + std:: + priority_queue, std::vector>, std::greater>> + pq_tasks; + std::array visited{}; + std::array parent{}; + std::array total_distance{}; + + targets_ = std::move(goals); + const int posIdx = CoordsToId(pos); + + RCLCPP_INFO(node_->get_logger(), "Got pos: %d %d", pos.x, pos.y); + + pq_tasks.emplace(0, posIdx); + visited.at(posIdx) = true; + node_->fill_obstacles(); + + while (!pq_tasks.empty()) { + const auto [score, idx] = pq_tasks.top(); + const glm::ivec2 grid_loc = IdToGrid(idx); + pq_tasks.pop(); + + RCLCPP_INFO(node_->get_logger(), "Popped grid_loc: %d %d", grid_loc.x, grid_loc.y); + + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 9; j++) { + const glm::ivec2 gridy = IdToGrid(idx) + neighbors[j] * directions[i]; + const int idy = GridToId(gridy); + //RCLCPP_INFO(node_->get_logger(), "Checking [idx, pos]: %d, %d;%d", idy, gridy.x, gridy.y); + + if (node_->getObstacleManager().check_collision(GridToCoords(gridy), 320)) { + if (!visited[idy]) { + parent[idy] = idx; + visited[idy] = true; + total_distance[idy] = distances[j] + total_distance[idx]; + + RCLCPP_DEBUG(node_->get_logger(), "Gridy: %d %d", gridy.x, gridy.y); + RCLCPP_DEBUG(node_->get_logger(), "idy: %d", idy); + RCLCPP_DEBUG(node_->get_logger(), "Distance: %f", total_distance[idy]); + pq_tasks.emplace(total_distance[idy] + dist_to_goal(gridy), idy); + for (const auto& goal : targets_) { + if (goal == gridy) { + gen_path(posIdx, parent, idy, path); + return; + } + } + } + } + } + } + } + } + + void AStar::gen_path(int start, std::array& parents, int end, std::vector& path) { + + int curr = end; + + while (curr != start) { + path.push_back(curr); + curr = parents[curr]; + } + + path.push_back(start); + } + + float AStar::dist_to_goal(glm::ivec2 pos) { + float mini = INFINITY; + for (const auto& goal : targets_) { + // Find closest of multiple targets + mini = glm::min(glm::distance((glm::vec2)goal, (glm::vec2)pos), mini); + } + return mini; + } +} \ No newline at end of file diff --git a/mg_planner/src/mg_planner.cpp b/mg_planner/src/mg_planner.cpp new file mode 100644 index 0000000..ad0457f --- /dev/null +++ b/mg_planner/src/mg_planner.cpp @@ -0,0 +1,11 @@ +#include "rclcpp/rclcpp.hpp" + +#include "mg_planner/mg_planner_node.hpp" +#include + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + + return 0; +} \ No newline at end of file diff --git a/mg_planner/src/mg_planner_node.cpp b/mg_planner/src/mg_planner_node.cpp new file mode 100644 index 0000000..3ed6774 --- /dev/null +++ b/mg_planner/src/mg_planner_node.cpp @@ -0,0 +1,58 @@ +#include "mg_planner/mg_planner_node.hpp" +#include "mg_msgs/msg/t_edit_cycle.hpp" +#include "mg_msgs/srv/calc_path.hpp" +#include "mg_planner/a_star/a_star.hpp" +#include "mg_planner/config.hpp" +#include +#include +#include +#include +#include +#include + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/convert.hpp" + +mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) { + astar_service = create_service( + "GeneratePath", + [this](mg_msgs::srv::CalcPath::Request::ConstSharedPtr req, + mg_msgs::srv::CalcPath::Response::SharedPtr resp) -> void { + std::cout << "Bro recieved request\n"; + auto elapsed = get_clock()->now(); + std::vector goals; + std::transform(req->x.begin(), + req->x.end(), + req->y.begin(), + std::back_inserter(goals), + [](const auto& aa, const auto& bb) { + std::cout << aa << " " << bb << "\n"; + return AStar::CoordsToGrid(glm::ivec2(aa * 1000, bb * 1000)); + }); + for (const auto& goal : goals) { + if (goal.x < 0 || GRID_X < goal.x || goal.y < 0 || GRID_Y < goal.y) { + return; + } + } + astar_.execute(get_pos(), std::move(goals), resp->indicies); + RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds()); + }); + + tf_buf_ = std::make_shared(get_clock()); + tf_listener = std::make_shared(*tf_buf_); +} + +glm::ivec2 mg::PlannerNode::get_pos() { + try { + auto tmsg = tf_buf_->lookupTransform("odom", "base-link", tf2::TimePointZero); + + tf2::Transform t; + tf2::convert(tmsg.transform, t); + auto vec3 = tmsg.transform.translation; + + return glm::clamp( + glm::ivec2{ vec3.x * 1000, vec3.y * 1000 }, glm::ivec2(MIN_X, MIN_Y), glm::ivec2(MAX_X, MAX_Y)); + + } catch (const std::exception& e) { RCLCPP_DEBUG(get_logger(), "Failed to find transform, ohno"); } + return { 1000, 1000 }; +} \ No newline at end of file diff --git a/mg_planner/src/obstacleManager.cpp b/mg_planner/src/obstacleManager.cpp new file mode 100644 index 0000000..adb78c0 --- /dev/null +++ b/mg_planner/src/obstacleManager.cpp @@ -0,0 +1,8 @@ +#include "mg_planner/obstacleManager.hpp" +#include "mg_planner/config.hpp" + +namespace mg { + bool ObstacleManager::check_collision(glm::ivec2 pos, float size) { + return (pos.x >= MIN_X && pos.x < MAX_X) && (pos.y >= MIN_Y && pos.y < MAX_Y); + } +} \ No newline at end of file From aeaf36fc2bbae9309e634b8b9be57a1d3ca36d08 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Fri, 18 Apr 2025 22:04:04 +0200 Subject: [PATCH 4/5] Publish generated path to use for visualisation --- mg_navigation/include/handlers/dwa_core.hpp | 1 + mg_navigation/include/handlers/dwa_global.hpp | 2 +- mg_navigation/src/path_buffer.cpp | 2 ++ mg_planner/include/mg_planner/mg_planner_node.hpp | 2 ++ mg_planner/src/a_star.cpp | 2 +- mg_planner/src/mg_planner_node.cpp | 5 +++++ 6 files changed, 12 insertions(+), 2 deletions(-) diff --git a/mg_navigation/include/handlers/dwa_core.hpp b/mg_navigation/include/handlers/dwa_core.hpp index 2bf599c..dacf90d 100644 --- a/mg_navigation/include/handlers/dwa_core.hpp +++ b/mg_navigation/include/handlers/dwa_core.hpp @@ -109,6 +109,7 @@ namespace mg { rclcpp::Rate rate(UPDATE_RATE); while (target_check() && rclcpp::ok()) { + target_update(); if (hgoal->is_canceling()) { cancel(); return; diff --git a/mg_navigation/include/handlers/dwa_global.hpp b/mg_navigation/include/handlers/dwa_global.hpp index 5b1bce1..6169787 100644 --- a/mg_navigation/include/handlers/dwa_global.hpp +++ b/mg_navigation/include/handlers/dwa_global.hpp @@ -22,7 +22,7 @@ namespace mg { template <> inline void DWA::target_update() { - baseNode.path_buffer()->update_path(goal); + baseNode.path_buffer()->update_path(); target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD); } diff --git a/mg_navigation/src/path_buffer.cpp b/mg_navigation/src/path_buffer.cpp index b0991d9..09776a1 100644 --- a/mg_navigation/src/path_buffer.cpp +++ b/mg_navigation/src/path_buffer.cpp @@ -47,6 +47,7 @@ namespace mg { resp_ = client_->async_send_request(req).share(); } if (resp_.valid()) { + path_msg_ = resp_.get(); auto req = std::make_shared(); req->x = goal_->x; @@ -68,6 +69,7 @@ namespace mg { resp_ = client_->async_send_request(req).share(); } + resp_.wait(); path_msg_ = resp_.get(); auto req = std::make_shared(); req->x = goal_->x; diff --git a/mg_planner/include/mg_planner/mg_planner_node.hpp b/mg_planner/include/mg_planner/mg_planner_node.hpp index 67fa278..429b90d 100644 --- a/mg_planner/include/mg_planner/mg_planner_node.hpp +++ b/mg_planner/include/mg_planner/mg_planner_node.hpp @@ -12,6 +12,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "mg_msgs/msg/path.hpp" #include "mg_msgs/srv/calc_path.hpp" #define TREE_SIZE 9 @@ -30,6 +31,7 @@ namespace mg { private: rclcpp::Service::SharedPtr astar_service; + rclcpp::Publisher::SharedPtr path_pub_; AStar astar_; ObstacleManager obstacle_manager_; diff --git a/mg_planner/src/a_star.cpp b/mg_planner/src/a_star.cpp index e187397..59a25af 100644 --- a/mg_planner/src/a_star.cpp +++ b/mg_planner/src/a_star.cpp @@ -39,7 +39,7 @@ namespace mg { const glm::ivec2 grid_loc = IdToGrid(idx); pq_tasks.pop(); - RCLCPP_INFO(node_->get_logger(), "Popped grid_loc: %d %d", grid_loc.x, grid_loc.y); + RCLCPP_DEBUG(node_->get_logger(), "Popped grid_loc: %d %d", grid_loc.x, grid_loc.y); for (int i = 0; i < 4; i++) { for (int j = 0; j < 9; j++) { diff --git a/mg_planner/src/mg_planner_node.cpp b/mg_planner/src/mg_planner_node.cpp index 3ed6774..14d270a 100644 --- a/mg_planner/src/mg_planner_node.cpp +++ b/mg_planner/src/mg_planner_node.cpp @@ -35,9 +35,14 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) { } } astar_.execute(get_pos(), std::move(goals), resp->indicies); + mg_msgs::msg::Path path; + path.indicies = resp->indicies; + path_pub_->publish(path); RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds()); }); + path_pub_ = create_publisher("/GeneratedPath", rclcpp::QoS(4).keep_last(2)); + tf_buf_ = std::make_shared(get_clock()); tf_listener = std::make_shared(*tf_buf_); } From d171734de69d88db357f05f42d8b7f1e97693c73 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Sun, 27 Apr 2025 17:34:18 +0200 Subject: [PATCH 5/5] Removed unused interfaces from mg_msgs --- mg_msgs/CMakeLists.txt | 2 -- mg_msgs/msg/TEdit.msg | 4 ---- mg_msgs/msg/TEditCycle.msg | 8 -------- 3 files changed, 14 deletions(-) delete mode 100644 mg_msgs/msg/TEdit.msg delete mode 100644 mg_msgs/msg/TEditCycle.msg diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt index 5bcd36f..d062b16 100644 --- a/mg_msgs/CMakeLists.txt +++ b/mg_msgs/CMakeLists.txt @@ -12,8 +12,6 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/Path.msg" "msg/Point2d.msg" - "msg/TEdit.msg" - "msg/TEditCycle.msg" "action/MoveGlobal.action" "action/MoveStraight.action" "action/MovePoint.action" diff --git a/mg_msgs/msg/TEdit.msg b/mg_msgs/msg/TEdit.msg deleted file mode 100644 index 4ef056d..0000000 --- a/mg_msgs/msg/TEdit.msg +++ /dev/null @@ -1,4 +0,0 @@ - -uint8 type -uint16 idx -uint16 idy diff --git a/mg_msgs/msg/TEditCycle.msg b/mg_msgs/msg/TEditCycle.msg deleted file mode 100644 index e9af1cc..0000000 --- a/mg_msgs/msg/TEditCycle.msg +++ /dev/null @@ -1,8 +0,0 @@ - -mg_msgs/TEdit[100] modifications -mg_msgs/Point2d[100] new_points -uint8 cmodifications 0 -uint8 cnew_points 0 - -uint8 DELETE=1 -uint8 ADD=0 \ No newline at end of file