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

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