Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Added obstacle avoidance (#16)

Browse Source
This commit is contained in:
Pimpest
2025-05-05 10:26:00 +00:00
parent ed9c5bab45 11441ab3cb
commit 39066fabd4
31 changed files with 801 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.clang-tidy
View File

@ -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: ''

18
mg_msgs/action/MoveGlobal.action
View File

@ -1,17 +1,17 @@
float64[] x
float64[] y
float64 step 0.01
float64 tolerance 0.5
float64 max_wheel_speed 3
float64 step 0.1
float64 tolerance 0.05
float64 max_wheel_speed 6.0
float64 max_angular 3.14
float64 max_vel 2
float64 pos_mult 15
float64 ornt_mult 4
float64 max_vel 4
float64 pos_mult 14.0
float64 ornt_mult 3.0
float64 t_ornt_mult 0.1
float64 obst_mult_a -5
float64 obst_mult_b -30
float64 obst_mult_c -9
float64 obst_mult_a 160
float64 obst_mult_b 4.0
float64 obst_mult_c 0
string[] ignore_tags []
---
uint8 SUCCESS=0

2
mg_navigation/CMakeLists.txt
View File

@ -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

4
mg_navigation/include/handlers/dwa_core.hpp
View File

@ -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();

25
mg_navigation/include/handlers/dwa_global.hpp
View File

@ -18,6 +18,9 @@ namespace mg {
inline void DWA<MgNavigationServer::MoveGlobal>::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<MgNavigationServer::MoveGlobal>::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;
}

3
mg_navigation/include/handlers/dwa_point.hpp
View File

@ -15,6 +15,7 @@ namespace mg {
template <>
inline void DWA<MgNavigationServer::MovePoint>::target_update() {
target_pos = glm::dvec2(goal->x, goal->y);
dimy = 8;
}
template <>
@ -25,7 +26,7 @@ namespace mg {
template <>
inline double DWA<MgNavigationServer::MovePoint>::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;

8
mg_navigation/include/mg_navigation.hpp
View File

@ -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<PathBuffer> path_buffer() { return path_buffer_; }
std::shared_ptr<PathBuffer> path_buffer() { return path_buffer_; }
std::shared_ptr<ObstacleManager>& obstacle_manager() { return obstacle_manager_; }
private:
bool is_processing = false;
std::shared_ptr<PathBuffer> path_buffer_;
std::shared_ptr<PathBuffer> path_buffer_;
std::shared_ptr<ObstacleManager> obstacle_manager_;
template <typename T>
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,

2
mg_navigation/include/path_buffer.hpp
View File

@ -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;

1
mg_navigation/package.xml
View File

@ -17,6 +17,7 @@
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>mg_obstacles</depend>
<build_depend>libglm-dev</build_depend>

3
mg_navigation/src/mg_navigation.cpp
View File

@ -5,7 +5,6 @@
#include "rclcpp/rclcpp.hpp"
#include "mg_navigation.hpp"
#include "handlers/dwa.hpp"
namespace mg {
template <typename T>
@ -54,6 +53,8 @@ namespace mg {
pub_twist = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);
obstacle_manager_ = std::make_shared<ObstacleManager>(this, tf2_buffer);
using namespace std::placeholders;
sv_move_global = rclcpp_action::create_server<MoveGlobal>(

6
mg_navigation/src/path_buffer.cpp
View File

@ -2,6 +2,9 @@
#include <glm/gtx/norm.hpp>
#include <chrono>
using namespace std::chrono_literals;
namespace mg {
PathBuffer::PathBuffer(rclcpp::Node* node) : current(-1), node_(node) {
@ -20,7 +23,6 @@ namespace mg {
&& glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) < lookahead * lookahead) {
current++;
}
if (current == (int)path_msg_->indicies.size()) {
double best = 10000000;
int id = 0;
@ -46,7 +48,7 @@ namespace mg {
req->y = goal_->y;
resp_ = client_->async_send_request(req).share();
}
if (resp_.valid()) {
if (resp_.wait_for(0s) == std::future_status::ready) {
path_msg_ = resp_.get();
auto req = std::make_shared<PathService::Request>();

84
mg_obstacles/CMakeLists.txt Normal file
View File

@ -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
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
)
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()

60
mg_obstacles/include/mg_obstacles/mg_obstacles.hpp Normal file
View File

@ -0,0 +1,60 @@
#pragma once
#include <string>
#include <glm/glm.hpp>
#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<StaticObstacle> static_obstacles_;
std::vector<PermanentObstacle> permanent_obstacles_;
glm::dvec3 oponent_position_ = { 0, 0, 0.20 };
bool openent_active_ = false;
rclcpp::CallbackGroup::SharedPtr no_exec_cbg;
rclcpp::Subscription<StaticListMsg>::SharedPtr static_obst_sub_;
rclcpp::Subscription<DynamicPosMsg>::SharedPtr dynamic_obst_sub_;
void load_permanent(Json::Value& json);
void load_static(Json::Value& json);
};
}

20
mg_obstacles/include/mg_obstacles/permanent_obstacle.hpp Normal file
View File

@ -0,0 +1,20 @@
#pragma once
#include <glm/glm.hpp>
#include <jsoncpp/json/json.h>
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;
};
}

16
mg_obstacles/include/mg_obstacles/sdf.hpp Normal file
View File

@ -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);
}

26
mg_obstacles/include/mg_obstacles/static_obstacle.hpp Normal file
View File

@ -0,0 +1,26 @@
#pragma once
#include <glm/glm.hpp>
#include <jsoncpp/json/json.h>
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;
};
}

56
mg_obstacles/obstacles/obstacles-static.json Normal file
View File

@ -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
}
]
}

94
mg_obstacles/obstacles/obstacles.json Normal file
View File

@ -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
}
]
}

29
mg_obstacles/package.xml Normal file
View File

@ -0,0 +1,29 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>mg_obstacles</name>
<version>0.0.0</version>
<description>Library for collision detection</description>
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
<license>Apache 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>rclcpp_components</depend>
<depend>mg_msgs</depend>
<depend>geometry_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>libjsoncpp</depend>
<build_depend>libjsoncpp-dev</build_depend>
<build_depend>libglm-dev</build_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

204
mg_obstacles/src/mg_obstacles.cpp Normal file
View File

@ -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 <fstream>
#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<StaticListMsg>(
"/staticObstacles", rclcpp::QoS(1).keep_last(1), static_cb, sub_opts);
dynamic_obst_sub_ = node_->create_subscription<DynamicPosMsg>(
"/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<std::string>("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);
}
}
}

23
mg_obstacles/src/permanent_obstacle.cpp Normal file
View File

@ -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);
}
}

53
mg_obstacles/src/sdf.cpp Normal file
View File

@ -0,0 +1,53 @@
#include "mg_obstacles/sdf.hpp"
#include <glm/gtx/norm.hpp>
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;
}
}

46
mg_obstacles/src/static_obstacle.cpp Normal file
View File

@ -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);
}
}
}

2
mg_planner/CMakeLists.txt
View File

@ -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

8
mg_planner/include/mg_planner/mg_planner_node.hpp
View File

@ -6,7 +6,7 @@
#include <rclcpp/client.hpp>
#include <rclcpp/service.hpp>
#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<mg_msgs::srv::CalcPath>::SharedPtr astar_service;
rclcpp::Publisher<mg_msgs::msg::Path>::SharedPtr path_pub_;
AStar astar_;
ObstacleManager obstacle_manager_;
AStar astar_;
ObstacleManager::SharedPtr obstacle_manager_;
std::shared_ptr<tf2_ros::Buffer> tf_buf_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener;

10
mg_planner/include/mg_planner/obstacleManager.hpp
View File

@ -4,13 +4,5 @@
#include <vector>
namespace mg {
class ObstacleManager {
const std::vector<glm::vec2> static_obstacles_;
const std::vector<glm::vec2> permanent_obstacles_;
glm::vec2 dynamic_obstacle_;
public:
bool check_collision(glm::ivec2 pos, float size);
};
bool check_collision(glm::ivec2 pos, float size);
}

1
mg_planner/package.xml
View File

@ -15,6 +15,7 @@
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>mg_obstacles</depend>
<build_depend>libglm-dev</build_depend>

21
mg_planner/src/a_star.cpp
View File

@ -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 <array>
#include <cmath>
#include <codecvt>
@ -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<int, (GRID_X * GRID_Y)>& parents, int end, std::vector<int>& path) {

5
mg_planner/src/mg_planner.cpp
View File

@ -5,7 +5,10 @@
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<mg::PlannerNode>());
rclcpp::executors::MultiThreadedExecutor executor;
rclcpp::Node::SharedPtr node = std::make_shared<mg::PlannerNode>();
executor.add_node(node);
executor.spin();
return 0;
}

5
mg_planner/src/mg_planner_node.cpp
View File

@ -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<int>(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<tf2_ros::Buffer>(get_clock());
tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buf_);
obstacle_manager_ = std::make_shared<ObstacleManager>(this, tf_buf_);
}
glm::ivec2 mg::PlannerNode::get_pos() {

2
mg_planner/src/obstacleManager.cpp
View File

@ -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);
}
}