From ccfd1189c2fbede6410359d393cacb0d50f7ad65 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Mon, 5 May 2025 01:34:53 +0200 Subject: [PATCH] Added collision avoidance to MoveGlobal routine --- .clang-tidy | 4 + mg_msgs/action/MoveGlobal.action | 8 +- mg_navigation/CMakeLists.txt | 2 + mg_navigation/include/handlers/dwa_core.hpp | 4 +- mg_navigation/include/handlers/dwa_global.hpp | 25 ++- mg_navigation/include/handlers/dwa_point.hpp | 3 +- mg_navigation/include/mg_navigation.hpp | 8 +- mg_navigation/include/path_buffer.hpp | 2 +- mg_navigation/package.xml | 1 + mg_navigation/src/mg_navigation.cpp | 3 +- mg_obstacles/CMakeLists.txt | 84 ++++++++ .../include/mg_obstacles/mg_obstacles.hpp | 60 ++++++ .../mg_obstacles/permanent_obstacle.hpp | 20 ++ mg_obstacles/include/mg_obstacles/sdf.hpp | 16 ++ .../include/mg_obstacles/static_obstacle.hpp | 26 +++ mg_obstacles/obstacles/obstacles-static.json | 56 +++++ mg_obstacles/obstacles/obstacles.json | 94 ++++++++ mg_obstacles/package.xml | 29 +++ mg_obstacles/src/mg_obstacles.cpp | 204 ++++++++++++++++++ mg_obstacles/src/permanent_obstacle.cpp | 23 ++ mg_obstacles/src/sdf.cpp | 53 +++++ mg_obstacles/src/static_obstacle.cpp | 46 ++++ mg_planner/CMakeLists.txt | 2 + .../include/mg_planner/mg_planner_node.hpp | 8 +- .../include/mg_planner/obstacleManager.hpp | 10 +- mg_planner/package.xml | 1 + mg_planner/src/a_star.cpp | 21 +- mg_planner/src/mg_planner.cpp | 5 +- mg_planner/src/mg_planner_node.cpp | 5 +- mg_planner/src/obstacleManager.cpp | 2 +- 30 files changed, 792 insertions(+), 33 deletions(-) create mode 100644 mg_obstacles/CMakeLists.txt create mode 100644 mg_obstacles/include/mg_obstacles/mg_obstacles.hpp create mode 100644 mg_obstacles/include/mg_obstacles/permanent_obstacle.hpp create mode 100644 mg_obstacles/include/mg_obstacles/sdf.hpp create mode 100644 mg_obstacles/include/mg_obstacles/static_obstacle.hpp create mode 100644 mg_obstacles/obstacles/obstacles-static.json create mode 100644 mg_obstacles/obstacles/obstacles.json create mode 100644 mg_obstacles/package.xml create mode 100644 mg_obstacles/src/mg_obstacles.cpp create mode 100644 mg_obstacles/src/permanent_obstacle.cpp create mode 100644 mg_obstacles/src/sdf.cpp create mode 100644 mg_obstacles/src/static_obstacle.cpp diff --git a/.clang-tidy b/.clang-tidy index 79a5255..58aad6d 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -25,6 +25,10 @@ Checks: " -cppcoreguidelines-pro-type-union-access, -cppcoreguidelines-macro-usage, -performance-unnecessary-value-param, + -cppcoreguidelines-pro-bounds-constant-array-index, + -readability-implicit-bool-conversion, + -cppcoreguidelines-avoid-magic-numbers, + -readability-magic-numbers, " WarningsAsErrors: '' HeaderFilterRegex: '' diff --git a/mg_msgs/action/MoveGlobal.action b/mg_msgs/action/MoveGlobal.action index 32080d0..e710c3b 100644 --- a/mg_msgs/action/MoveGlobal.action +++ b/mg_msgs/action/MoveGlobal.action @@ -1,15 +1,15 @@ float64[] x float64[] y -float64 step 0.01 -float64 tolerance 0.5 +float64 step 0.1 +float64 tolerance 0.05 float64 max_wheel_speed 3 float64 max_angular 3.14 -float64 max_vel 2 +float64 max_vel 4 float64 pos_mult 15 float64 ornt_mult 4 float64 t_ornt_mult 0.1 -float64 obst_mult_a -5 +float64 obst_mult_a 50 float64 obst_mult_b -30 float64 obst_mult_c -9 string[] ignore_tags [] diff --git a/mg_navigation/CMakeLists.txt b/mg_navigation/CMakeLists.txt index b1005ce..45555cd 100644 --- a/mg_navigation/CMakeLists.txt +++ b/mg_navigation/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(mg_obstacles REQUIRED) include(FindPkgConfig) pkg_search_module(LIBGLM REQUIRED glm) @@ -26,6 +27,7 @@ set(PACKAGE_DEPS tf2 tf2_ros tf2_geometry_msgs + mg_obstacles ) add_executable(mg_nav_server diff --git a/mg_navigation/include/handlers/dwa_core.hpp b/mg_navigation/include/handlers/dwa_core.hpp index dacf90d..81edbe2 100644 --- a/mg_navigation/include/handlers/dwa_core.hpp +++ b/mg_navigation/include/handlers/dwa_core.hpp @@ -106,10 +106,13 @@ namespace mg { pos_query(); target_init(); + rclcpp::Time elapsed = baseNode.get_clock()->now(); rclcpp::Rate rate(UPDATE_RATE); while (target_check() && rclcpp::ok()) { target_update(); + baseNode.obstacle_manager()->update_dynamic(); + baseNode.obstacle_manager()->update_static(); if (hgoal->is_canceling()) { cancel(); return; @@ -138,7 +141,6 @@ namespace mg { cvl = spacevl[b_ind]; cvr = spacevr[b_ind]; send_speed(step * cvl, step * cvr); - rate.sleep(); } succeed(); diff --git a/mg_navigation/include/handlers/dwa_global.hpp b/mg_navigation/include/handlers/dwa_global.hpp index 6169787..4e57ca4 100644 --- a/mg_navigation/include/handlers/dwa_global.hpp +++ b/mg_navigation/include/handlers/dwa_global.hpp @@ -18,6 +18,9 @@ namespace mg { inline void DWA::target_init() { baseNode.path_buffer()->update_path_block(goal); target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD); + + dimy = 4; + dimx = 11; } template <> @@ -34,7 +37,7 @@ namespace mg { template <> inline double DWA::calc_score(int vl, int vr) { auto [v, w] = to_vel(step * vl, step * vr); - const double delta = 0.5; + const double delta = 0.3; glm::dvec2 n_pos; double n_theta = NAN; double score = 0; @@ -56,11 +59,25 @@ namespace mg { 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)); + // 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)); + + const double angl = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) })); + const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) })); + + const double dist = baseNode.obstacle_manager()->box_colide(n_pos, { 0.29, 0.33 }, n_theta); + const double dist2 = baseNode.obstacle_manager()->dist_to_nearest(n_pos) - 0.22; + const double obstacle_scoreA = glm::max(0.0, 0.02 - dist); + const double obstacle_scoreB = glm::max(0.0, 0.03 - dist2); + + RCLCPP_INFO(baseNode.get_logger(), "Door Stuck %lf", dist); + RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2); score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos)); - score += goal->ornt_mult * (angl - n_angl); + // score += goal->ornt_mult * (angl - n_angl); + score += goal->ornt_mult * (glm::abs(angl) - glm::abs(n_angl)); + score -= goal->obst_mult_a * obstacle_scoreA; + score -= goal->obst_mult_b * obstacle_scoreB; return score; } diff --git a/mg_navigation/include/handlers/dwa_point.hpp b/mg_navigation/include/handlers/dwa_point.hpp index 42024db..996337f 100644 --- a/mg_navigation/include/handlers/dwa_point.hpp +++ b/mg_navigation/include/handlers/dwa_point.hpp @@ -15,6 +15,7 @@ namespace mg { template <> inline void DWA::target_update() { target_pos = glm::dvec2(goal->x, goal->y); + dimy = 8; } template <> @@ -25,7 +26,7 @@ namespace mg { template <> inline double DWA::calc_score(int vl, int vr) { auto [v, w] = to_vel(step * vl, step * vr); - const double delta = 0.5; + const double delta = 0.3; glm::dvec2 n_pos; double n_theta = NAN; double score = 0; diff --git a/mg_navigation/include/mg_navigation.hpp b/mg_navigation/include/mg_navigation.hpp index e535620..2215552 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 "mg_obstacles/mg_obstacles.hpp" + #include "path_buffer.hpp" namespace mg { @@ -47,12 +49,14 @@ namespace mg { MgNavigationServer(rclcpp::NodeOptions& opts); - std::shared_ptr path_buffer() { return path_buffer_; } + std::shared_ptr path_buffer() { return path_buffer_; } + std::shared_ptr& obstacle_manager() { return obstacle_manager_; } private: bool is_processing = false; - std::shared_ptr path_buffer_; + std::shared_ptr path_buffer_; + std::shared_ptr obstacle_manager_; template rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&, diff --git a/mg_navigation/include/path_buffer.hpp b/mg_navigation/include/path_buffer.hpp index 3f7dcb5..de49f13 100644 --- a/mg_navigation/include/path_buffer.hpp +++ b/mg_navigation/include/path_buffer.hpp @@ -28,7 +28,7 @@ namespace mg { 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)); } + inline glm::vec2 IdToCoords(const int id) { return GridToCoords(IdToGrid(id)) / 1000.0F; } class PathBuffer { using PathService = mg_msgs::srv::CalcPath; diff --git a/mg_navigation/package.xml b/mg_navigation/package.xml index 7cee11e..708e747 100644 --- a/mg_navigation/package.xml +++ b/mg_navigation/package.xml @@ -17,6 +17,7 @@ tf2 tf2_ros tf2_geometry_msgs + mg_obstacles libglm-dev diff --git a/mg_navigation/src/mg_navigation.cpp b/mg_navigation/src/mg_navigation.cpp index 9251e7f..beaaa3f 100644 --- a/mg_navigation/src/mg_navigation.cpp +++ b/mg_navigation/src/mg_navigation.cpp @@ -5,7 +5,6 @@ #include "rclcpp/rclcpp.hpp" #include "mg_navigation.hpp" #include "handlers/dwa.hpp" - namespace mg { template @@ -54,6 +53,8 @@ namespace mg { pub_twist = create_publisher("diffdrive_controller/cmd_vel", 2); + obstacle_manager_ = std::make_shared(this, tf2_buffer); + using namespace std::placeholders; sv_move_global = rclcpp_action::create_server( diff --git a/mg_obstacles/CMakeLists.txt b/mg_obstacles/CMakeLists.txt new file mode 100644 index 0000000..c0bf339 --- /dev/null +++ b/mg_obstacles/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.12) + +project(mg_obstacles) + +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) + +set(PACKAGE_DEPS + rclcpp + ament_index_cpp + mg_msgs + geometry_msgs + std_msgs + tf2 + tf2_ros + tf2_geometry_msgs + jsoncpp +) + +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach(PACKAGE ${PACKAGE_DEPS}) + +include(FindPkgConfig) +pkg_check_modules(JSONCPP jsoncpp) +# pkg_search_module(LIBGLM REQUIRED glm) + +set(SOURCES + src/mg_obstacles.cpp + src/sdf.cpp + src/static_obstacle.cpp + src/permanent_obstacle.cpp +) + +add_library( + mg_obstacles + SHARED + ${SOURCES} +) + +target_include_directories( + mg_obstacles + PRIVATE + ${LIBGLM_INCLUDE_DIRS} + ${JSONCPP_INCLUDE_DIRS} +) + +target_link_libraries(mg_obstacles ${JSONCPP_LINK_LIBRARIES}) + +target_include_directories( + mg_obstacles + PUBLIC + "$" + "$" +) + +ament_target_dependencies( + mg_obstacles + ${PACKAGE_DEPS} +) + + +install( + TARGETS mg_obstacles + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +install(DIRECTORY obstacles DESTINATION share/${PROJECT_NAME}/) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_include_directories(include) + +target_compile_features(mg_obstacles PUBLIC c_std_99 cxx_std_17) + +ament_package() + diff --git a/mg_obstacles/include/mg_obstacles/mg_obstacles.hpp b/mg_obstacles/include/mg_obstacles/mg_obstacles.hpp new file mode 100644 index 0000000..9088596 --- /dev/null +++ b/mg_obstacles/include/mg_obstacles/mg_obstacles.hpp @@ -0,0 +1,60 @@ +#pragma once +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "mg_obstacles/permanent_obstacle.hpp" +#include "mg_obstacles/static_obstacle.hpp" + +#include "std_msgs/msg/u_int64.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" + +#include "tf2_ros/buffer.h" + +namespace mg { + class ObstacleManager { + using StaticListMsg = std_msgs::msg::UInt64; + using DynamicPosMsg = geometry_msgs::msg::PointStamped; + + public: + RCLCPP_SMART_PTR_DEFINITIONS(ObstacleManager) + + enum ObstacleMask { // NOLINT + DYNAMIC = 1, + STATIC = 2, + MOVABLE = 3, + PERMANENT = 4, + IMPORTANT = 5, + ALL = 7, + }; + + ObstacleManager(rclcpp::Node* node, tf2_ros::Buffer::SharedPtr& buf); + bool contains(glm::dvec2 pos, double radius, uint mask = ObstacleMask::ALL); + double dist_to_nearest(glm::dvec2 pos, int mask = ObstacleMask::ALL); + double box_colide(glm::dvec2 xy, glm::dvec2 size, double rot, int mask = ObstacleMask::ALL); + + static double dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, const glm::dmat2 rot_mat); + static double dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, double rotation); + + void update_dynamic(); + void update_static(); + + private: + rclcpp::Node* node_; + + tf2_ros::Buffer::SharedPtr tf_buffer; + + std::vector static_obstacles_; + std::vector permanent_obstacles_; + + glm::dvec3 oponent_position_ = { 0, 0, 0.20 }; + bool openent_active_ = false; + + rclcpp::CallbackGroup::SharedPtr no_exec_cbg; + + rclcpp::Subscription::SharedPtr static_obst_sub_; + rclcpp::Subscription::SharedPtr dynamic_obst_sub_; + + void load_permanent(Json::Value& json); + void load_static(Json::Value& json); + }; +} \ No newline at end of file diff --git a/mg_obstacles/include/mg_obstacles/permanent_obstacle.hpp b/mg_obstacles/include/mg_obstacles/permanent_obstacle.hpp new file mode 100644 index 0000000..9b9aef3 --- /dev/null +++ b/mg_obstacles/include/mg_obstacles/permanent_obstacle.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include +#include + +namespace mg { + class PermanentObstacle { + public: + /* Create a permanent obstacle from a json value */ + PermanentObstacle(const Json::Value& json); + + glm::dvec2 pos; + glm::dvec2 size; + + double distance(glm::dvec2 position) const; + bool contains(glm::dvec2 position, double radius) const; + double distanceBox(const glm::dvec2 position, const glm::dvec2 size, const glm::dmat2 rot_mat) const; + }; + +} \ No newline at end of file diff --git a/mg_obstacles/include/mg_obstacles/sdf.hpp b/mg_obstacles/include/mg_obstacles/sdf.hpp new file mode 100644 index 0000000..4bde130 --- /dev/null +++ b/mg_obstacles/include/mg_obstacles/sdf.hpp @@ -0,0 +1,16 @@ +#include "glm/glm.hpp" + +namespace mg { + double boxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t); + + double circleSdf(glm::dvec2 pos, glm::dvec2 t); + + bool inCircleSdf(glm::dvec2 pos, double rad, glm::dvec2 t, double radius); + bool inBoxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t, double radius); + + double boxToBox(const glm::dvec2 pos, + const glm::dvec2 size, + const glm::dvec2 t, + const glm::dvec2 sizet, + const glm::dmat2 rot_mat); +} \ No newline at end of file diff --git a/mg_obstacles/include/mg_obstacles/static_obstacle.hpp b/mg_obstacles/include/mg_obstacles/static_obstacle.hpp new file mode 100644 index 0000000..fd27052 --- /dev/null +++ b/mg_obstacles/include/mg_obstacles/static_obstacle.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include + +namespace mg { + class StaticObstacle { + public: + /* Create a static obstacle from a json value */ + StaticObstacle(const Json::Value& json); + + glm::dvec2 pos; + + bool active = true; + + bool horizontal; + + static double width; + static double height; + + double distance(glm::dvec2 position) const; + bool contains(glm::dvec2 position, double radius) const; + double distanceBox(glm::dvec2 position, glm::dvec2 size, glm::dmat2 rot_mat) const; + }; + +} \ No newline at end of file diff --git a/mg_obstacles/obstacles/obstacles-static.json b/mg_obstacles/obstacles/obstacles-static.json new file mode 100644 index 0000000..6e86710 --- /dev/null +++ b/mg_obstacles/obstacles/obstacles-static.json @@ -0,0 +1,56 @@ +{ + "staticObsacleHeight": 0.4, + "staticObsacleWidth": 0.1, + "staticObstacles": [ + { + "horizontal": true, + "x": 0.625, + "y": 1.775 + }, + { + "horizontal": false, + "x": 0, + "y": 0.6 + }, + { + "horizontal": false, + "x": 0, + "y": 1.525 + }, + { + "horizontal": true, + "x": 0.575, + "y": 0.3 + }, + { + "horizontal": true, + "x": 2.025, + "y": 0.3 + }, + { + "horizontal": false, + "x": 2.9, + "y": 0.6 + }, + { + "horizontal": false, + "x": 2.9, + "y": 1.525 + }, + { + "horizontal": true, + "x": 1.975, + "y": 1.775 + }, + { + "horizontal": true, + "x": 0.9, + "y": 1 + }, + { + "horizontal": true, + "x": 1.7, + "y": 1 + } + ] +} diff --git a/mg_obstacles/obstacles/obstacles.json b/mg_obstacles/obstacles/obstacles.json new file mode 100644 index 0000000..48ce579 --- /dev/null +++ b/mg_obstacles/obstacles/obstacles.json @@ -0,0 +1,94 @@ +{ + "staticObstacleHeight": 0.4, + "staticObstacleWidth": 0.1, + "staticObstacles": [ + { + "horizontal": true, + "x": 0.625, + "y": 1.775 + }, + { + "horizontal": false, + "x": 0, + "y": 0.6 + }, + { + "horizontal": false, + "x": 0, + "y": 1.525 + }, + { + "horizontal": true, + "x": 0.575, + "y": 0.3 + }, + { + "horizontal": true, + "x": 2.025, + "y": 0.3 + }, + { + "horizontal": false, + "x": 2.9, + "y": 0.6 + }, + { + "horizontal": false, + "x": 2.9, + "y": 1.525 + }, + { + "horizontal": true, + "x": 1.975, + "y": 1.775 + }, + { + "horizontal": true, + "x": 0.9, + "y": 1 + }, + { + "horizontal": true, + "x": 1.7, + "y": 1 + } + ], + "obstacles": [ + { + "height": 0.45, + "width": 1.95, + "x": 1.05, + "y": 2 + }, + { + "height": 0.45, + "width": 0.45, + "x": 1.55, + "y": 0.45 + }, + { + "height": 0.2, + "width": 0.4, + "x": 0.65, + "y": 2 + }, + { + "height": 0.15, + "width": 0.45, + "x": 2, + "y": 0.15 + }, + { + "height": 0.15, + "width": 0.45, + "x": 0, + "y": 0.15 + }, + { + "height": 0.45, + "width": 0.45, + "x": 0, + "y": 1.1 + } + ] +} \ No newline at end of file diff --git a/mg_obstacles/package.xml b/mg_obstacles/package.xml new file mode 100644 index 0000000..703df0a --- /dev/null +++ b/mg_obstacles/package.xml @@ -0,0 +1,29 @@ + + + + mg_obstacles + 0.0.0 + Library for collision detection + Pimpest + Apache 2.0 + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_components + mg_msgs + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + libjsoncpp + + + libjsoncpp-dev + libglm-dev + + + ament_cmake + + \ No newline at end of file diff --git a/mg_obstacles/src/mg_obstacles.cpp b/mg_obstacles/src/mg_obstacles.cpp new file mode 100644 index 0000000..9e564e8 --- /dev/null +++ b/mg_obstacles/src/mg_obstacles.cpp @@ -0,0 +1,204 @@ +#include "mg_obstacles/mg_obstacles.hpp" +#include "mg_obstacles/sdf.hpp" + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "glm/glm.hpp" +#include "glm/gtx/matrix_transform_2d.hpp" + +#include + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace mg { + ObstacleManager::ObstacleManager(rclcpp::Node* node, tf2_ros::Buffer::SharedPtr& buf) : + node_(node), tf_buffer(buf) { + no_exec_cbg = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::SubscriptionOptions sub_opts; + sub_opts.callback_group = no_exec_cbg; + + auto static_cb = [](StaticListMsg::ConstSharedPtr) {}; + auto dynamic_cb = [](DynamicPosMsg::ConstSharedPtr) {}; + + static_obst_sub_ = node_->create_subscription( + "/staticObstacles", rclcpp::QoS(1).keep_last(1), static_cb, sub_opts); + dynamic_obst_sub_ = node_->create_subscription( + "/dynamicObstacle", rclcpp::QoS(1).keep_last(1), dynamic_cb, sub_opts); + + std::string obstacle_pkg; + std::string file_suffix = "/obstacles/obstacles.json"; + node_->declare_parameter("obstacles", rclcpp::PARAMETER_STRING); + node_->get_parameter_or("obstacles", obstacle_pkg, "mg_obstacles/obstacles/obstacles.json"); + ulong n = obstacle_pkg.find_first_of('/'); + if (n < obstacle_pkg.size()) { + file_suffix = obstacle_pkg.substr(n); + obstacle_pkg = obstacle_pkg.substr(0, n); + } + Json::Value json; + RCLCPP_INFO(node_->get_logger(), + "Read file %s", + (ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix).c_str()); + std::ifstream json_document(ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix); + + json_document >> json; + + load_permanent(json); + load_static(json); + } + + bool ObstacleManager::contains(glm::dvec2 pos, double radius, uint mask) { + bool contained = false; + if (mask & ObstacleMask::DYNAMIC && openent_active_) { + contained |= inCircleSdf({ oponent_position_.x, oponent_position_.y }, oponent_position_.z, pos, radius); + } + if (mask & ObstacleMask::STATIC) { + for (const auto& obstacle : static_obstacles_) { + if (!obstacle.active) { + continue; + } + // Check Static obstacles + contained |= obstacle.contains(pos, radius); + } + } + if (mask & ObstacleMask::PERMANENT) { + for (const auto& obstacle : permanent_obstacles_) { + // Check Permanant obstacles + contained |= obstacle.contains(pos, radius); + } + } + // contained |= dist_to_bounds(pos, { radius, radius }, 0) < 0; + return contained; + } + + double ObstacleManager::dist_to_nearest(glm::dvec2 pos, int mask) { + double nearest = INFINITY; + + if (mask & ObstacleMask::DYNAMIC && openent_active_) { + const double x = circleSdf(pos, glm::dvec2(oponent_position_.x, oponent_position_.y)); + + nearest = glm::min(nearest, x - oponent_position_.z); + } + + if (mask & ObstacleMask::STATIC) { + for (const auto& obstacle : static_obstacles_) { + if (!obstacle.active) { + continue; + } + // Check Static obstacles + nearest = glm::min(nearest, obstacle.distance(pos)); + } + } + + if (mask & ObstacleMask::PERMANENT) { + for (const auto& obstacle : permanent_obstacles_) { + // Check Permanant obstacles + nearest = glm::min(nearest, obstacle.distance(pos)); + } + } + + return nearest; + } + + double ObstacleManager::box_colide(glm::dvec2 xy, glm::dvec2 size, double rot, int mask) { + const glm::dmat2 rot_mat{ { glm::cos(rot), glm::sin(rot) }, { -glm::sin(rot), glm::cos(rot) } }; + + double nearest = INFINITY; + + if (mask & ObstacleMask::DYNAMIC && openent_active_) { + const double x = circleSdf(xy, glm::dvec2(oponent_position_.x, oponent_position_.y)); + + nearest = glm::min(nearest, x - oponent_position_.z - glm::length(size) / 4); + } + if (mask & ObstacleMask::STATIC) { + for (const auto& obstacle : static_obstacles_) { + if (!obstacle.active) { + continue; + } + // Check Static obstacles + nearest = glm::min(nearest, obstacle.distanceBox(xy, size, rot_mat)); + } + } + + if (mask & ObstacleMask::PERMANENT) { + for (const auto& obstacle : permanent_obstacles_) { + // Check Permanant obstacles + nearest = glm::min(nearest, obstacle.distanceBox(xy, size, rot_mat)); + } + } + + return nearest; + } // NOLINT + + double ObstacleManager::dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, const glm::dmat2 rot_mat) { + glm::dvec2 point1 = glm::dvec2{ -size.x, -size.y } * 0.5; + glm::dvec2 point2 = glm::dvec2{ -size.x, size.y } * 0.5; + glm::dvec2 point3 = glm::dvec2{ size.x, -size.y } * 0.5; + glm::dvec2 point4 = glm::dvec2{ size.x, size.y } * 0.5; + + point1 = point1 * rot_mat + pos; + point2 = point2 * rot_mat + pos; + point3 = point3 * rot_mat + pos; + point4 = point4 * rot_mat + pos; + + glm::dvec2 mini = glm::min(point1, glm::dvec2(3.0, 2.0) - point1); + + mini = glm::min(glm::min(point2, glm::dvec2(3.0, 2.0) - point2), mini); + mini = glm::min(glm::min(point3, glm::dvec2(3.0, 2.0) - point3), mini); + mini = glm::min(glm::min(point4, glm::dvec2(3.0, 2.0) - point4), mini); + + return glm::min(mini.x, mini.y); + } + + double ObstacleManager::dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, double rot) { + // Find me + const glm::dmat2 rot_mat{ { glm::cos(rot), -glm::sin(rot) }, { glm::sin(rot), glm::cos(rot) } }; + return ObstacleManager::dist_to_bounds(pos, size, rot_mat); + } + + void ObstacleManager::update_dynamic() { + DynamicPosMsg msg; + rclcpp::MessageInfo info; + if (dynamic_obst_sub_->take(msg, info)) { + auto point = tf_buffer->transform(msg, "odom"); + + oponent_position_.x = point.point.x; + oponent_position_.y = point.point.y; + + openent_active_ = oponent_position_.x >= 0 && oponent_position_.y >= 0; + } + } + + void ObstacleManager::update_static() { + StaticListMsg msg; + rclcpp::MessageInfo info; + if (static_obst_sub_->take(msg, info)) { + uint ind = 1; + for (int i = (int)static_obstacles_.size() - 1; i >= 0; i--) { + static_obstacles_[i].active = ind & msg.data; + ind <<= 1; + } + } + } + + void ObstacleManager::load_permanent(Json::Value& json) { + + Json::Value& obstacles = json["obstacles"]; + + for (const Json::Value& obstacle : obstacles) { + // Add obstacle + permanent_obstacles_.emplace_back(obstacle); + } + } + + void ObstacleManager::load_static(Json::Value& json) { + + StaticObstacle::width = json["staticObstacleWidth"].asDouble(); + StaticObstacle::height = json["staticObstacleHeight"].asDouble(); + for (const Json::Value& obstacle : json["staticObstacles"]) { static_obstacles_.emplace_back(obstacle); } + for (const auto& obstacle : static_obstacles_) { + RCLCPP_INFO(node_->get_logger(), "Loaded static obstacle at %lf %lf", obstacle.pos.x, obstacle.pos.y); + } + } +} \ No newline at end of file diff --git a/mg_obstacles/src/permanent_obstacle.cpp b/mg_obstacles/src/permanent_obstacle.cpp new file mode 100644 index 0000000..b6f24ca --- /dev/null +++ b/mg_obstacles/src/permanent_obstacle.cpp @@ -0,0 +1,23 @@ +#include "mg_obstacles/permanent_obstacle.hpp" +#include "mg_obstacles/sdf.hpp" + +namespace mg { + PermanentObstacle::PermanentObstacle(const Json::Value& json) : + pos(json.get("x", 1.0).asDouble(), json.get("y", 1.0).asDouble()), + size(json.get("width", 0.0).asDouble(), json.get("height", 0.0).asDouble()) { } + + double PermanentObstacle::distance(glm::dvec2 position) const { + return boxSdf(pos + 0.5 * glm::dvec2{ size.x, -size.y }, size * 0.5, position); + } + + bool PermanentObstacle::contains(glm::dvec2 position, double radius) const { + return inBoxSdf(pos + 0.5 * glm::dvec2{ size.x, -size.y }, size * 0.5, position, radius); + } + + double PermanentObstacle::distanceBox(const glm::dvec2 position, + const glm::dvec2 size, + const glm::dmat2 rot_mat) const { + + return boxToBox(pos + 0.5 * glm::dvec2{ this->size.x, -this->size.y }, this->size, position, size, rot_mat); + } +} \ No newline at end of file diff --git a/mg_obstacles/src/sdf.cpp b/mg_obstacles/src/sdf.cpp new file mode 100644 index 0000000..209f13e --- /dev/null +++ b/mg_obstacles/src/sdf.cpp @@ -0,0 +1,53 @@ +#include "mg_obstacles/sdf.hpp" +#include + +namespace mg { + double boxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t) { + glm::dvec2 d = glm::abs(t - pos) - size; + return glm::length(glm::max(d, 0.0)) + glm::min(glm::max(d.x, d.y), 0.0); + } + + double circleSdf(glm::dvec2 pos, glm::dvec2 t) { return glm::length(pos - t); } + + bool inCircleSdf(glm::dvec2 pos, double rad, glm::dvec2 t, double radius) { + return glm::length2(pos - t) < glm::pow(rad + radius, 2); + } + + bool inBoxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t, double radius) { + const glm::dvec2 d = glm::abs(t - pos) - size; + // return (glm::length2(glm::max(d, 0.0))) < glm::pow(radius - glm::min(glm::max(d.x, d.y), 0.0), 2); + return boxSdf(pos, size, t) - radius < 0; + } + + double boxToBox(const glm::dvec2 pos, + const glm::dvec2 size, + const glm::dvec2 t, + const glm::dvec2 sizet, + const glm::dmat2 rot_mat) { + + const glm::dmat2 irot_mat = glm::transpose(rot_mat); + + glm::dvec2 pointA1 = rot_mat * (glm::dvec2{ -sizet.x, -sizet.y } * 0.5) + t; + glm::dvec2 pointA2 = rot_mat * (glm::dvec2{ sizet.x, -sizet.y } * 0.5) + t; + glm::dvec2 pointA3 = rot_mat * (glm::dvec2{ -sizet.x, sizet.y } * 0.5) + t; + glm::dvec2 pointA4 = rot_mat * (glm::dvec2{ sizet.x, sizet.y } * 0.5) + t; + + double distance = boxSdf(pos, size * 0.5, pointA1); + + distance = glm::min(boxSdf(pos, size * 0.5, pointA2), distance); + distance = glm::min(boxSdf(pos, size * 0.5, pointA3), distance); + distance = glm::min(boxSdf(pos, size * 0.5, pointA4), distance); + + glm::dvec2 pointB1 = irot_mat * (glm::dvec2{ -size.x, -size.y } * 0.5 + pos - t); + glm::dvec2 pointB2 = irot_mat * (glm::dvec2{ size.x, -size.y } * 0.5 + pos - t); + glm::dvec2 pointB3 = irot_mat * (glm::dvec2{ -size.x, size.y } * 0.5 + pos - t); + glm::dvec2 pointB4 = irot_mat * (glm::dvec2{ size.x, size.y } * 0.5 + pos - t); + + distance = glm::min(boxSdf({}, sizet * 0.5, pointB1), distance); + distance = glm::min(boxSdf({}, sizet * 0.5, pointB2), distance); + distance = glm::min(boxSdf({}, sizet * 0.5, pointB3), distance); + distance = glm::min(boxSdf({}, sizet * 0.5, pointB4), distance); + + return distance; + } +} \ No newline at end of file diff --git a/mg_obstacles/src/static_obstacle.cpp b/mg_obstacles/src/static_obstacle.cpp new file mode 100644 index 0000000..d4f0974 --- /dev/null +++ b/mg_obstacles/src/static_obstacle.cpp @@ -0,0 +1,46 @@ +#include "mg_obstacles/static_obstacle.hpp" +#include "mg_obstacles/sdf.hpp" + +namespace mg { + + double StaticObstacle::width = 0.1; + double StaticObstacle::height = 0.4; + + StaticObstacle::StaticObstacle(const Json::Value& json) : + pos(json.get("x", 1.0).asDouble(), json.get("y", 1.0).asDouble()), + horizontal(json.get("horizontal", true).asBool()) { } + + double StaticObstacle::distance(glm::dvec2 position) const { + const double width = StaticObstacle::width; + const double height = StaticObstacle::height; + if (!horizontal) { + return boxSdf(pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height } * 0.5, position); + } else { + return boxSdf(pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width } * 0.5, position); + } + } + + double StaticObstacle::distanceBox(glm::dvec2 position, glm::dvec2 size, glm::dmat2 rot_mat) const { + const double width = StaticObstacle::width; + const double height = StaticObstacle::height; + if (!horizontal) { + return boxToBox( + pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height }, position, size, rot_mat); + } else { + return boxToBox( + pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width }, position, size, rot_mat); + } + } + + bool StaticObstacle::contains(glm::dvec2 position, double radius) const { + const double width = StaticObstacle::width; + const double height = StaticObstacle::height; + if (!horizontal) { + return inBoxSdf( + pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height } * 0.5, position, radius); + } else { + return inBoxSdf( + pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width } * 0.5, position, radius); + } + } +} \ No newline at end of file diff --git a/mg_planner/CMakeLists.txt b/mg_planner/CMakeLists.txt index dca5646..43badb6 100644 --- a/mg_planner/CMakeLists.txt +++ b/mg_planner/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(mg_obstacles REQUIRED) include(FindPkgConfig) pkg_search_module(LIBGLM REQUIRED glm) @@ -24,6 +25,7 @@ set(PACKAGE_DEPS tf2 tf2_ros tf2_geometry_msgs + mg_obstacles ) add_executable(mg_planner diff --git a/mg_planner/include/mg_planner/mg_planner_node.hpp b/mg_planner/include/mg_planner/mg_planner_node.hpp index 429b90d..fc931b3 100644 --- a/mg_planner/include/mg_planner/mg_planner_node.hpp +++ b/mg_planner/include/mg_planner/mg_planner_node.hpp @@ -6,7 +6,7 @@ #include #include #include "mg_planner/a_star/a_star.hpp" -#include "mg_planner/obstacleManager.hpp" +#include "mg_obstacles/mg_obstacles.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" @@ -25,7 +25,7 @@ namespace mg { void fill_obstacles() { } - ObstacleManager& getObstacleManager() { return obstacle_manager_; } + ObstacleManager::SharedPtr getObstacleManager() { return obstacle_manager_; } glm::ivec2 get_pos(); @@ -33,8 +33,8 @@ namespace mg { rclcpp::Service::SharedPtr astar_service; rclcpp::Publisher::SharedPtr path_pub_; - AStar astar_; - ObstacleManager obstacle_manager_; + AStar astar_; + ObstacleManager::SharedPtr obstacle_manager_; std::shared_ptr tf_buf_; std::shared_ptr tf_listener; diff --git a/mg_planner/include/mg_planner/obstacleManager.hpp b/mg_planner/include/mg_planner/obstacleManager.hpp index 863f3b6..298a10c 100644 --- a/mg_planner/include/mg_planner/obstacleManager.hpp +++ b/mg_planner/include/mg_planner/obstacleManager.hpp @@ -4,13 +4,5 @@ #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); - }; + 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 index b3820f5..928b40f 100644 --- a/mg_planner/package.xml +++ b/mg_planner/package.xml @@ -15,6 +15,7 @@ tf2 tf2_ros tf2_geometry_msgs + mg_obstacles libglm-dev diff --git a/mg_planner/src/a_star.cpp b/mg_planner/src/a_star.cpp index 59a25af..af31d05 100644 --- a/mg_planner/src/a_star.cpp +++ b/mg_planner/src/a_star.cpp @@ -1,6 +1,7 @@ #include "mg_planner/a_star/a_star.hpp" #include "mg_planner/config.hpp" #include "mg_planner/mg_planner_node.hpp" +#include "mg_planner/obstacleManager.hpp" #include #include #include @@ -33,8 +34,14 @@ namespace mg { pq_tasks.emplace(0, posIdx); visited.at(posIdx) = true; node_->fill_obstacles(); + int id_close = posIdx; + float dist_close = INFINITY; + rclcpp::Time time = node_->get_clock()->now(); - while (!pq_tasks.empty()) { + node_->getObstacleManager()->update_static(); + node_->getObstacleManager()->update_dynamic(); + + while (!pq_tasks.empty() && (node_->get_clock()->now() - time).seconds() < 0.1) { const auto [score, idx] = pq_tasks.top(); const glm::ivec2 grid_loc = IdToGrid(idx); pq_tasks.pop(); @@ -47,7 +54,9 @@ namespace mg { 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 (check_collision(GridToCoords(gridy), 0) + && !node_->getObstacleManager()->contains( + glm::dvec2(GridToCoords(gridy)) / 1000.0, 0.195, ObstacleManager::ObstacleMask::ALL)) { if (!visited[idy]) { parent[idy] = idx; visited[idy] = true; @@ -56,7 +65,12 @@ namespace mg { 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); + const float distance = dist_to_goal(gridy); + if (distance < dist_close) { + id_close = idy; + dist_close = distance; + } + pq_tasks.emplace(total_distance[idy] + distance, idy); for (const auto& goal : targets_) { if (goal == gridy) { gen_path(posIdx, parent, idy, path); @@ -68,6 +82,7 @@ namespace mg { } } } + gen_path(posIdx, parent, id_close, path); } void AStar::gen_path(int start, std::array& parents, int end, std::vector& path) { diff --git a/mg_planner/src/mg_planner.cpp b/mg_planner/src/mg_planner.cpp index ad0457f..0a4924e 100644 --- a/mg_planner/src/mg_planner.cpp +++ b/mg_planner/src/mg_planner.cpp @@ -5,7 +5,10 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::executors::MultiThreadedExecutor executor; + rclcpp::Node::SharedPtr node = std::make_shared(); + executor.add_node(node); + executor.spin(); 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 index b9cd913..c9e95df 100644 --- a/mg_planner/src/mg_planner_node.cpp +++ b/mg_planner/src/mg_planner_node.cpp @@ -34,8 +34,9 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) { } } astar_.execute(get_pos(), std::move(goals), resp->indicies); + std::reverse(resp->indicies.begin(), resp->indicies.end()); mg_msgs::msg::Path path; - path.indicies = resp->indicies; + path.indicies = std::vector(resp->indicies); path_pub_->publish(path); RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds()); }); @@ -44,6 +45,8 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) { tf_buf_ = std::make_shared(get_clock()); tf_listener = std::make_shared(*tf_buf_); + + obstacle_manager_ = std::make_shared(this, tf_buf_); } glm::ivec2 mg::PlannerNode::get_pos() { diff --git a/mg_planner/src/obstacleManager.cpp b/mg_planner/src/obstacleManager.cpp index adb78c0..5cd8576 100644 --- a/mg_planner/src/obstacleManager.cpp +++ b/mg_planner/src/obstacleManager.cpp @@ -2,7 +2,7 @@ #include "mg_planner/config.hpp" namespace mg { - bool ObstacleManager::check_collision(glm::ivec2 pos, float size) { + bool 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