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

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