Publish generated path to use for visualisation

2025-04-18 22:04:04 +02:00
parent ec831bd334
commit aeaf36fc2b
6 changed files with 12 additions and 2 deletions
--- a/mg_planner/src/mg_planner_node.cpp
+++ b/mg_planner/src/mg_planner_node.cpp
@@ -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_);
 }