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

Publish generated path to use for visualisation

Browse Source
This commit is contained in:
Pimpest
2025-04-18 22:04:04 +02:00
parent ec831bd334
commit aeaf36fc2b
6 changed files with 12 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
mg_planner/src/a_star.cpp
View File

@ -39,7 +39,7 @@ namespace mg {
const glm::ivec2 grid_loc = IdToGrid(idx);
pq_tasks.pop();
RCLCPP_INFO(node_->get_logger(), "Popped grid_loc: %d %d", grid_loc.x, grid_loc.y);
RCLCPP_DEBUG(node_->get_logger(), "Popped grid_loc: %d %d", grid_loc.x, grid_loc.y);
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 9; j++) {

5
mg_planner/src/mg_planner_node.cpp
View File

@ -35,9 +35,14 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) {
}
}
astar_.execute(get_pos(), std::move(goals), resp->indicies);
mg_msgs::msg::Path path;
path.indicies = resp->indicies;
path_pub_->publish(path);
RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds());
});
path_pub_ = create_publisher<mg_msgs::msg::Path>("/GeneratedPath", rclcpp::QoS(4).keep_last(2));
tf_buf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buf_);
}