Added collision avoidance to MoveGlobal routine
This commit is contained in:
204
mg_obstacles/src/mg_obstacles.cpp
Normal file
204
mg_obstacles/src/mg_obstacles.cpp
Normal 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
23
mg_obstacles/src/permanent_obstacle.cpp
Normal 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
53
mg_obstacles/src/sdf.cpp
Normal 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
46
mg_obstacles/src/static_obstacle.cpp
Normal 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user