Publish generated path to use for visualisation

mg_navigation/include/handlers/dwa_core.hpp

@@ -109,6 +109,7 @@ namespace mg {
         rclcpp::Rate rate(UPDATE_RATE);
 
         while (target_check() && rclcpp::ok()) {
+            target_update();
             if (hgoal->is_canceling()) {
                 cancel();
                 return;

mg_navigation/include/handlers/dwa_global.hpp

@@ -22,7 +22,7 @@ namespace mg {
 
     template <>
     inline void DWA<MgNavigationServer::MoveGlobal>::target_update() {
-        baseNode.path_buffer()->update_path(goal);
+        baseNode.path_buffer()->update_path();
         target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);
     }
 

mg_navigation/src/path_buffer.cpp

@@ -47,6 +47,7 @@ namespace mg {
             resp_    = client_->async_send_request(req).share();
         }
         if (resp_.valid()) {
+
             path_msg_ = resp_.get();
             auto req  = std::make_shared<PathService::Request>();
             req->x    = goal_->x;
@@ -68,6 +69,7 @@ namespace mg {
             resp_    = client_->async_send_request(req).share();
         }
 
+        resp_.wait();
         path_msg_ = resp_.get();
         auto req  = std::make_shared<PathService::Request>();
         req->x    = goal_->x;

mg_planner/include/mg_planner/mg_planner_node.hpp

@@ -12,6 +12,7 @@
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 
+#include "mg_msgs/msg/path.hpp"
 #include "mg_msgs/srv/calc_path.hpp"
 
 #define TREE_SIZE 9
@@ -30,6 +31,7 @@ namespace mg {
 
       private:
         rclcpp::Service<mg_msgs::srv::CalcPath>::SharedPtr astar_service;
+        rclcpp::Publisher<mg_msgs::msg::Path>::SharedPtr   path_pub_;
 
         AStar           astar_;
         ObstacleManager obstacle_manager_;

mg_planner/src/a_star.cpp

@@ -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++) {

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