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

Added collision avoidance to MoveGlobal routine

Browse Source
This commit is contained in:
Pimpest
2025-05-05 01:34:53 +02:00
parent a160af8de3
commit ccfd1189c2
30 changed files with 792 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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