update

Controllers/Packages/amr_control/include/amr_control/amr_publiser.h

@@ -3,8 +3,17 @@
 
 #include <ros/ros.h>
 #include <robot/robot.h>
-#include <nav_msgs/OccupancyGrid.h>
+
 #include <geometry_msgs/PolygonStamped.h>
+#include <geometry_msgs/Twist.h>
+#include <robot_nav_2d_msgs/Twist2DStamped.h>
+#include <robot_nav_2d_msgs/Path2D.h>
+#include <robot_nav_2d_utils/conversions.h>
+#include <nav_2d_utils/conversions.h>
+
+#include <nav_2d_msgs/Path2D.h>
+#include <nav_msgs/Path.h>
+#include <nav_msgs/OccupancyGrid.h>
 #include <robot_nav_msgs/OccupancyGrid.h>
 #include <robot_map_msgs/OccupancyGridUpdate.h>
 #include <map_msgs/OccupancyGridUpdate.h>
@@ -14,12 +23,13 @@
 
 namespace amr_control
 {
-  struct CostmapObject
+  struct PlannerObject
   {
     // ROS publishers
     ros::Publisher pub_;
     ros::Publisher update_pub_;
     ros::Publisher footprint_pub_;
+    ros::Publisher plan_pub_;
     ros::Timer timer_;
     std::string topic_;
     bool active_;
@@ -47,6 +57,16 @@ namespace amr_control
      */
     virtual ~AmrPublisher();
 
+    /**
+     * @brief Publish global plan to RViz
+     */
+    void publishGlobalPlan();
+
+    /**
+     * @brief Publish local plan to RViz
+     */
+    void publishLocalPlan();
+
     /**
      * @brief Publish global costmap to RViz
      * @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
@@ -96,7 +116,7 @@ namespace amr_control
      * @brief Check if publishing is active
      * @return True if publishing is active (either global or local)
      */
-    bool isPublishing() const { return global_costmap_obj_.active_ || local_costmap_obj_.active_ || cmd_vel_publishing_active_; }
+    bool isPublishing() const { return global_planner_obj_.active_ || local_planner_obj_.active_ || cmd_vel_publishing_active_; }
 
     /**
      * @brief Start publishing cmd_vel at a fixed rate using ros::WallTimer
@@ -109,7 +129,28 @@ namespace amr_control
       */
      void stopCmdVelPublishing();
 
+    /**
+     * @brief Start publishing plans at a fixed rate
+     * @param global_rate Publishing rate for global plan (Hz). If 0, don't publish global.
+     * @param local_rate Publishing rate for local plan (Hz). If 0, don't publish local.
+     */
+    void startPlanPublishing(double global_rate = 10.0, double local_rate = 10.0);
+
+    /**
+     * @brief Stop publishing plans
+     */
+    void stopPlanPublishing();
+
   private:
+
+    /**
+     * @brief Convert robot_nav_2d_msgs::Path2D to nav_msgs::Path
+     * @param robot_path Input robot_nav_2d_msgs::Path2D
+     * @param nav_path Output nav_msgs::Path
+     */
+    void convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path, 
+                          nav_msgs::Path &nav_path);
+
     /**
      * @brief Callback for new subscription to global costmap
      * @param pub SingleSubscriberPublisher for the new subscription
@@ -164,16 +205,38 @@ namespace amr_control
      */
     void cmdVelTimerCallback(const ros::WallTimerEvent &event);
 
+    /**
+     * @brief Timer callback for updating global plan
+     * @param event Timer event
+     */
+    void globalPlanTimerCallback(const ros::TimerEvent &event);
+
+    /**
+     * @brief Timer callback for updating local plan
+     * @param event Timer event
+     */
+    void localPlanTimerCallback(const ros::TimerEvent &event);
+
     ros::NodeHandle nh_;
     robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
 
-    CostmapObject global_costmap_obj_;
+    PlannerObject global_planner_obj_;
-    CostmapObject local_costmap_obj_;
+    PlannerObject local_planner_obj_;
     ros::Publisher cmd_vel_pub_;
     
     // ros::WallTimer for cmd_vel publishing
     ros::WallTimer cmd_vel_timer_;
     bool cmd_vel_publishing_active_;
+
+    // Timers for plan publishing
+    ros::Timer global_plan_timer_;
+    ros::Timer local_plan_timer_;
+    bool global_plan_publishing_active_;
+    bool local_plan_publishing_active_;
+
+    // Cached plans with timestamps
+    robot_nav_2d_msgs::Path2D cached_global_plan_;
+    robot_nav_2d_msgs::Path2D cached_local_plan_;
   };
 
 } // namespace amr_control

Controllers/Packages/amr_control/src/amr_control.cpp

@@ -77,11 +77,12 @@ namespace amr_control
       vda_5050_client_api_ptr_.reset();
     }
 
-    // Stop costmap and cmd_vel publishing
+    // Stop costmap, cmd_vel and plan publishing
     if (amr_publisher_ptr_)
     {
       amr_publisher_ptr_->stopPublishing();
       amr_publisher_ptr_->stopCmdVelPublishing();
+      amr_publisher_ptr_->stopPlanPublishing();
       amr_publisher_ptr_.reset();
     }
   }
@@ -339,6 +340,15 @@ namespace amr_control
           nh.param("cmd_vel_publish_rate", cmd_vel_rate, cmd_vel_rate);
           robot::log_info("[%s:%d]\n Starting cmd_vel publishing at %.2f Hz...", __FILE__, __LINE__, cmd_vel_rate);
           amr_publisher_ptr_->startCmdVelPublishing(cmd_vel_rate);
+          
+          // Start publishing plans using Timer
+          double global_plan_rate = 10.0;  // Default: 10 Hz
+          double local_plan_rate = 10.0;    // Default: 10 Hz
+          nh.param("global_plan_publish_rate", global_plan_rate, global_plan_rate);
+          nh.param("local_plan_publish_rate", local_plan_rate, local_plan_rate);
+          robot::log_info("[%s:%d]\n Starting plan publishing - Global: %.2f Hz, Local: %.2f Hz...", 
+                         __FILE__, __LINE__, global_plan_rate, local_plan_rate);
+          amr_publisher_ptr_->startPlanPublishing(global_plan_rate, local_plan_rate);
         }
       }
     }

Controllers/Packages/amr_control/src/amr_publiser.cpp

@@ -3,8 +3,7 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <robot_costmap_2d/cost_values.h>
 #include <boost/bind.hpp>
-#include <robot_nav_2d_msgs/Twist2DStamped.h>
+
-#include <geometry_msgs/Twist.h>
 
 namespace amr_control
 {
@@ -22,29 +21,34 @@ AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavig
   nh_.param("local_costmap_topic", local_costmap_topic, local_costmap_topic);
   nh_.param("cmd_vel_topic", cmd_vel_topic, cmd_vel_topic);
   // Initialize publishers with callback for new subscriptions
-  global_costmap_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
+  global_planner_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
     global_costmap_topic, 1, 
     boost::bind(&AmrPublisher::onNewSubscriptionGlobal, this, _1));
-  global_costmap_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(global_costmap_topic + "_updates", 1);
+  global_planner_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(global_costmap_topic + "_updates", 1);
-  global_costmap_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(global_costmap_topic + "/footprint", 1);
+  global_planner_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(global_costmap_topic + "/footprint", 1);
+  global_planner_obj_.plan_pub_ = nh_.advertise< nav_msgs::Path>(global_costmap_topic + "/plan", 1);
   
-  local_costmap_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
+  local_planner_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
     local_costmap_topic, 1, 
     boost::bind(&AmrPublisher::onNewSubscriptionLocal, this, _1));
-  local_costmap_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(local_costmap_topic + "_updates", 1);
+  local_planner_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(local_costmap_topic + "_updates", 1);
-  local_costmap_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(local_costmap_topic + "/footprint", 1);
+  local_planner_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(local_costmap_topic + "/footprint", 1);
-  
+  local_planner_obj_.plan_pub_ = nh_.advertise< nav_msgs::Path>(local_costmap_topic + "/plan", 1);
 
   cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>(cmd_vel_topic, 1);
   cmd_vel_publishing_active_ = false;
   robot::log_info("[AmrPublisher] Initialized. Cmd vel topic: %s", cmd_vel_topic.c_str());
   // Initialize topic names in CostmapObject
-  global_costmap_obj_.topic_ = global_costmap_topic;
+  global_planner_obj_.topic_ = global_costmap_topic;
-  local_costmap_obj_.topic_ = local_costmap_topic;
+  local_planner_obj_.topic_ = local_costmap_topic;
   
   // Initialize other CostmapObject members
-  global_costmap_obj_.active_ = false;
+  global_planner_obj_.active_ = false;
-  local_costmap_obj_.active_ = false;
+  local_planner_obj_.active_ = false;
+
+  // Initialize plan publishing flags
+  global_plan_publishing_active_ = false;
+  local_plan_publishing_active_ = false;
 
 
   robot::log_info("[AmrPublisher] Initialized. Global costmap topic: %s, Local costmap topic: %s",
@@ -58,6 +62,7 @@ AmrPublisher::~AmrPublisher()
 {
   stopPublishing();
   stopCmdVelPublishing();
+  stopPlanPublishing();
 }
 
 void AmrPublisher::convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid, 
@@ -138,13 +143,13 @@ void AmrPublisher::publishGlobalCostmap(double publish_rate)
     {
       nav_msgs::OccupancyGrid nav_grid;
       convertToNavOccupancyGrid(global_data.costmap, nav_grid);
-      global_costmap_obj_.pub_.publish(nav_grid);
+      global_planner_obj_.pub_.publish(nav_grid);
     }
     else
     {
       map_msgs::OccupancyGridUpdate grid_update;
       convertToRobotOccupancyGridUpdate(global_data.costmap_update, grid_update);
-      global_costmap_obj_.update_pub_.publish(grid_update);
+      global_planner_obj_.update_pub_.publish(grid_update);
     }
     
     // Publish footprint
@@ -177,13 +182,13 @@ void AmrPublisher::publishLocalCostmap(double publish_rate)
     {
       nav_msgs::OccupancyGrid nav_grid;
       convertToNavOccupancyGrid(local_data.costmap, nav_grid);
-      local_costmap_obj_.pub_.publish(nav_grid);
+      local_planner_obj_.pub_.publish(nav_grid);
     }
     else
     {
       map_msgs::OccupancyGridUpdate grid_update;
       convertToRobotOccupancyGridUpdate(local_data.costmap_update, grid_update);
-      local_costmap_obj_.update_pub_.publish(grid_update);
+      local_planner_obj_.update_pub_.publish(grid_update);
     }
     
     // Publish footprint
@@ -220,7 +225,7 @@ void AmrPublisher::publishGlobalFootprint()
     // Convert and publish footprint
     geometry_msgs::PolygonStamped nav_footprint;
     convertToNavPolygonStamped(global_data.footprint, nav_footprint);
-    global_costmap_obj_.footprint_pub_.publish(nav_footprint);
+    global_planner_obj_.footprint_pub_.publish(nav_footprint);
     
     // robot::log_debug("[AmrPublisher] Published global footprint with %zu points, frame_id='%s'", 
     //                  nav_footprint.polygon.points.size(), nav_footprint.header.frame_id.c_str());
@@ -247,7 +252,7 @@ void AmrPublisher::publishLocalFootprint()
     // Convert and publish footprint
     geometry_msgs::PolygonStamped nav_footprint;
     convertToNavPolygonStamped(local_data.footprint, nav_footprint);
-    local_costmap_obj_.footprint_pub_.publish(nav_footprint);
+    local_planner_obj_.footprint_pub_.publish(nav_footprint);
     
     // robot::log_debug("[AmrPublisher] Published local footprint with %zu points, frame_id='%s'", 
     //                  nav_footprint.polygon.points.size(), nav_footprint.header.frame_id.c_str());
@@ -271,8 +276,8 @@ void AmrPublisher::startPublishing(double global_rate, double local_rate)
   // Start global costmap timer if rate > 0
   if (global_rate > 0.0)
   {
-    global_costmap_obj_.active_ = true;
+    global_planner_obj_.active_ = true;
-    global_costmap_obj_.timer_ = nh_.createTimer(
+    global_planner_obj_.timer_ = nh_.createTimer(
       ros::Duration(1.0 / global_rate),
       &AmrPublisher::globalCostmapTimerCallback,
       this
@@ -283,8 +288,8 @@ void AmrPublisher::startPublishing(double global_rate, double local_rate)
   // Start local costmap timer if rate > 0
   if (local_rate > 0.0)
   {
-    local_costmap_obj_.active_ = true;
+    local_planner_obj_.active_ = true;
-    local_costmap_obj_.timer_ = nh_.createTimer(
+    local_planner_obj_.timer_ = nh_.createTimer(
       ros::Duration(1.0 / local_rate),
       &AmrPublisher::localCostmapTimerCallback,
       this
@@ -296,16 +301,16 @@ void AmrPublisher::startPublishing(double global_rate, double local_rate)
 void AmrPublisher::stopPublishing()
 {
   // Stop timers
-  if (global_costmap_obj_.active_)
+  if (global_planner_obj_.active_)
   {
-    global_costmap_obj_.timer_.stop();
+    global_planner_obj_.timer_.stop();
-    global_costmap_obj_.active_ = false;
+    global_planner_obj_.active_ = false;
   }
   
-  if (local_costmap_obj_.active_)
+  if (local_planner_obj_.active_)
   {
-    local_costmap_obj_.timer_.stop();
+    local_planner_obj_.timer_.stop();
-    local_costmap_obj_.active_ = false;
+    local_planner_obj_.active_ = false;
   }
 
   robot::log_info("[AmrPublisher] Stopped publishing costmaps");
@@ -313,7 +318,7 @@ void AmrPublisher::stopPublishing()
 
 void AmrPublisher::globalCostmapTimerCallback(const ros::TimerEvent &event)
 {
-  if (global_costmap_obj_.active_)
+  if (global_planner_obj_.active_)
   {
     publishGlobalCostmap();
   }
@@ -321,7 +326,7 @@ void AmrPublisher::globalCostmapTimerCallback(const ros::TimerEvent &event)
 
 void AmrPublisher::localCostmapTimerCallback(const ros::TimerEvent &event)
 {
-  if (local_costmap_obj_.active_)
+  if (local_planner_obj_.active_)
   {
     publishLocalCostmap();
   }
@@ -333,6 +338,61 @@ void AmrPublisher::onNewSubscriptionLocal(const ros::SingleSubscriberPublisher&
   publishLocalCostmap();
 }
 
+void AmrPublisher::convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path, 
+                          nav_msgs::Path &nav_path)
+{
+  // Convert header
+  nav_2d_msgs::Path2D path2d;
+  path2d.header.seq = robot_path.header.seq;
+  path2d.header.frame_id = robot_path.header.frame_id;
+  path2d.header.stamp = ros::Time::now();
+  path2d.poses.resize(robot_path.poses.size());
+  for (unsigned int i = 0; i < robot_path.poses.size(); i++)
+  {
+    path2d.poses[i].pose.x = robot_path.poses[i].pose.x;
+    path2d.poses[i].pose.y = robot_path.poses[i].pose.y;
+    path2d.poses[i].pose.theta = robot_path.poses[i].pose.theta;
+  }
+  nav_path = nav_2d_utils::pathToPath(path2d);
+}
+
+void AmrPublisher::publishGlobalPlan()
+{
+  if(!move_base_ptr_)
+  {
+    return;
+  }
+  
+  // Check if plan is empty
+  if(cached_global_plan_.poses.empty())
+  {
+    return;
+  }
+  
+  robot::Time now = robot::Time::now();
+  robot::Time plan_stamp = cached_global_plan_.header.stamp;
+  
+  // Check if timestamp is zero (uninitialized)
+  if(plan_stamp.sec == 0 && plan_stamp.nsec == 0)
+  {
+    return;
+  }
+  
+  robot::Duration age = now - plan_stamp;
+  robot::Duration max_age(0.5);
+  
+  // Check if timestamp is not older than 0.5 seconds
+  if(plan_stamp < now - max_age)
+  {
+    return;
+  }
+  
+  nav_msgs::Path nav_path;
+  convertToNavPath(cached_global_plan_, nav_path);
+  
+  global_planner_obj_.plan_pub_.publish(nav_path);
+}
+
 void AmrPublisher::onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher& pub)
 {
   robot::log_info("[AmrPublisher] New subscription: %s", pub.getSubscriberName().c_str());
@@ -405,5 +465,128 @@ void AmrPublisher::cmdVelTimerCallback(const ros::WallTimerEvent &event)
   }
 }
 
+void AmrPublisher::publishLocalPlan()
+{
+  if(!move_base_ptr_)
+  {
+    return;
+  }
+  
+  // Check if plan is empty
+  if(cached_local_plan_.poses.empty())
+  {
+    return;
+  }
+  
+  robot::Time now = robot::Time::now();
+  robot::Time plan_stamp = cached_local_plan_.header.stamp;
+  
+  // Check if timestamp is zero (uninitialized)
+  if(plan_stamp.sec == 0 && plan_stamp.nsec == 0)
+  {
+    return;
+  }
+  
+  robot::Duration age = now - plan_stamp;
+  robot::Duration max_age(0.5);
+  
+  // Check if timestamp is not older than 0.5 seconds
+  if(plan_stamp < now - max_age)
+  {
+    return;
+  }
+  
+  nav_msgs::Path nav_path;
+  convertToNavPath(cached_local_plan_, nav_path);
+  local_planner_obj_.plan_pub_.publish(nav_path);
+}
+
+void AmrPublisher::globalPlanTimerCallback(const ros::TimerEvent &event)
+{
+  if (!move_base_ptr_)
+  {
+    return;
+  }
+  
+  // Update cached global plan
+  try
+  {
+    cached_global_plan_ = move_base_ptr_->getGlobalData().plan;
+    // Publish if timestamp is valid
+    publishGlobalPlan();
+  }
+  catch (const std::exception &e)
+  {
+    robot::log_error("[AmrPublisher] Error updating global plan: %s", e.what());
+  }
+}
+
+void AmrPublisher::localPlanTimerCallback(const ros::TimerEvent &event)
+{
+  if (!move_base_ptr_)
+  {
+    return;
+  }
+  
+  // Update cached local plan
+  try
+  {
+    cached_local_plan_ = move_base_ptr_->getLocalData().plan;
+    // Publish if timestamp is valid
+    publishLocalPlan();
+  }
+  catch (const std::exception &e)
+  {
+    robot::log_error("[AmrPublisher] Error updating local plan: %s", e.what());
+  }
+}
+
+void AmrPublisher::startPlanPublishing(double global_rate, double local_rate)
+{
+  stopPlanPublishing(); // Stop any existing publishing first
+
+  // Start global plan timer if rate > 0
+  if (global_rate > 0.0)
+  {
+    global_plan_publishing_active_ = true;
+    global_plan_timer_ = nh_.createTimer(
+      ros::Duration(1.0 / global_rate),
+      &AmrPublisher::globalPlanTimerCallback,
+      this
+    );
+    robot::log_info("[AmrPublisher] Started publishing global plan at %.2f Hz", global_rate);
+  }
+
+  // Start local plan timer if rate > 0
+  if (local_rate > 0.0)
+  {
+    local_plan_publishing_active_ = true;
+    local_plan_timer_ = nh_.createTimer(
+      ros::Duration(1.0 / local_rate),
+      &AmrPublisher::localPlanTimerCallback,
+      this
+    );
+    robot::log_info("[AmrPublisher] Started publishing local plan at %.2f Hz", local_rate);
+  }
+}
+
+void AmrPublisher::stopPlanPublishing()
+{
+  // Stop timers
+  if (global_plan_publishing_active_)
+  {
+    global_plan_timer_.stop();
+    global_plan_publishing_active_ = false;
+  }
+  
+  if (local_plan_publishing_active_)
+  {
+    local_plan_timer_.stop();
+    local_plan_publishing_active_ = false;
+  }
+
+  robot::log_info("[AmrPublisher] Stopped publishing plans");
+}
+
 } // namespace amr_control
 

Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp

@@ -160,88 +160,85 @@ namespace amr_control
       }
       else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
       {
-        // vda5050_msgs::Order order_msg;
-        // order_msg.headerId = order.headerId;
-        // order_msg.timestamp = order.timestamp;
-        // order_msg.version = order.version;
-        // order_msg.manufacturer = order.manufacturer;
-        // order_msg.serialNumber = order.serialNumber;
-        // order_msg.orderId = order.orderId;
-        // order_msg.orderUpdateId = order.orderUpdateId;
-
-        // for (auto node : order.nodes)
-        // {
-        //   vda5050_msgs::Node node_msg;
-        //   node_msg.nodeId = node.nodeId;
-        //   node_msg.sequenceId = node.sequenceId;
-        //   node_msg.nodeDescription = node.nodeDescription;
-        //   node_msg.released = node.released;
-
-        //   node_msg.nodePosition.x = node.nodePosition.x;
-        //   node_msg.nodePosition.y = node.nodePosition.y;
-        //   node_msg.nodePosition.theta = node.nodePosition.theta;
-        //   node_msg.nodePosition.allowedDeviationXY = node.nodePosition.allowedDeviationXY;
-        //   node_msg.nodePosition.allowedDeviationTheta = node.nodePosition.allowedDeviationTheta;
-        //   node_msg.nodePosition.mapId = node.nodePosition.mapId;
-        //   node_msg.nodePosition.mapDescription = node.nodePosition.mapDescription;
-        //   // vda5050_msgs/Action[] actions
-        //   order_msg.nodes.push_back(node_msg);
-        // }
-
-        // for (auto edge : order.edges)
-        // {
-        //   vda5050_msgs::Edge edge_msg;
-        //   edge_msg.edgeId = edge.edgeId;
-        //   edge_msg.sequenceId = edge.sequenceId;
-        //   edge_msg.edgeDescription = edge.edgeDescription;
-        //   edge_msg.released = edge.released;
-        //   edge_msg.startNodeId = edge.startNodeId;
-        //   edge_msg.endNodeId = edge.endNodeId;
-        //   edge_msg.maxSpeed = edge.maxSpeed;
-        //   edge_msg.maxHeight = edge.maxHeight;
-        //   edge_msg.minHeight = edge.minHeight;
-        //   edge_msg.orientation = edge.orientation;
-        //   edge_msg.orientationType = edge.orientationType;
-        //   edge_msg.direction = edge.direction;
-        //   edge_msg.rotationAllowed = edge.rotationAllowed;
-        //   edge_msg.maxRotationSpeed = edge.maxRotationSpeed;
-        //   edge_msg.length = edge.length;
-        //   edge_msg.trajectory.degree = edge.trajectory.degree;
-        //   edge_msg.trajectory.knotVector = edge.trajectory.knotVector;
-
-        //   for (auto controlPoint : edge.trajectory.controlPoints)
-        //   {
-        //     vda5050_msgs::ControlPoint controlPoint_msg;
-        //     controlPoint_msg.x = controlPoint.x;
-        //     controlPoint_msg.y = controlPoint.y;
-        //     controlPoint_msg.weight = controlPoint.weight;
-        //     edge_msg.trajectory.controlPoints.push_back(controlPoint_msg);
-        //   }
-        //   // Corridor corridor;
-        //   // std::vector<Action> actions;
-        //   order_msg.edges.push_back(edge_msg);
-        // }
-
-        // // order_msg.edges zoneSetId =VDA5050ClientAPI::client_ptr_->vda5050_order_.
-        // VDA5050ClientAPI::plan_pub_.publish(order_msg);
-
-        // vda_5050::NodePosition position = order.nodes.back().nodePosition;
-        // goal.header.stamp = robot::Time::now();
-        // goal.header.frame_id = "map";
-        // goal.pose.position.x = position.x;
-        // goal.pose.position.y = position.y;
-        // goal.pose.position.z = 0.0;
-        
-        // // Convert theta (yaw) to Quaternion using setRPY
-        // // setRPY(roll, pitch, yaw) - for 2D navigation, only yaw is used
-        // tf3::Quaternion quat;
-        // quat.setRPY(0.0, 0.0, position.theta);
-        // goal.pose.orientation.x = quat.x();
-        // goal.pose.orientation.y = quat.y();
-        // goal.pose.orientation.z = quat.z();
-        // goal.pose.orientation.w = quat.w();
-
         robot_protocol_msgs::Order order_msg;
+        // vda5050_msgs::Order order_msg;
+        order_msg.headerId = order.headerId;
+        order_msg.timestamp = order.timestamp;
+        order_msg.version = order.version;
+        order_msg.manufacturer = order.manufacturer;
+        order_msg.serialNumber = order.serialNumber;
+        order_msg.orderId = order.orderId;
+        order_msg.orderUpdateId = order.orderUpdateId;
+
+        for (auto node : order.nodes)
+        {
+          robot_protocol_msgs::Node node_msg;
+          node_msg.nodeId = node.nodeId;
+          node_msg.sequenceId = node.sequenceId;
+          node_msg.nodeDescription = node.nodeDescription;
+          node_msg.released = node.released;
+
+          node_msg.nodePosition.x = node.nodePosition.x;
+          node_msg.nodePosition.y = node.nodePosition.y;
+          node_msg.nodePosition.theta = node.nodePosition.theta;
+          node_msg.nodePosition.allowedDeviationXY = node.nodePosition.allowedDeviationXY;
+          node_msg.nodePosition.allowedDeviationTheta = node.nodePosition.allowedDeviationTheta;
+          node_msg.nodePosition.mapId = node.nodePosition.mapId;
+          node_msg.nodePosition.mapDescription = node.nodePosition.mapDescription;
+          // vda5050_msgs/Action[] actions
+          order_msg.nodes.push_back(node_msg);
+        }
+
+        for (auto edge : order.edges)
+        {
+          robot_protocol_msgs::Edge edge_msg;
+          edge_msg.edgeId = edge.edgeId;
+          edge_msg.sequenceId = edge.sequenceId;
+          edge_msg.edgeDescription = edge.edgeDescription;
+          edge_msg.released = edge.released;
+          edge_msg.startNodeId = edge.startNodeId;
+          edge_msg.endNodeId = edge.endNodeId;
+          edge_msg.maxSpeed = edge.maxSpeed;
+          edge_msg.maxHeight = edge.maxHeight;
+          edge_msg.minHeight = edge.minHeight;
+          edge_msg.orientation = edge.orientation;
+          edge_msg.orientationType = edge.orientationType;
+          edge_msg.direction = edge.direction;
+          edge_msg.rotationAllowed = edge.rotationAllowed;
+          edge_msg.maxRotationSpeed = edge.maxRotationSpeed;
+          edge_msg.length = edge.length;
+          edge_msg.trajectory.degree = edge.trajectory.degree;
+          edge_msg.trajectory.knotVector = edge.trajectory.knotVector;
+
+          for (auto controlPoint : edge.trajectory.controlPoints)
+          {
+            robot_protocol_msgs::ControlPoint controlPoint_msg;
+            controlPoint_msg.x = controlPoint.x;
+            controlPoint_msg.y = controlPoint.y;
+            controlPoint_msg.weight = controlPoint.weight;
+            edge_msg.trajectory.controlPoints.push_back(controlPoint_msg);
+          }
+          // Corridor corridor;
+          // std::vector<Action> actions;
+          order_msg.edges.push_back(edge_msg);
+        }
+
+        vda_5050::NodePosition position = order.nodes.back().nodePosition;
+        goal.header.stamp = robot::Time::now();
+        goal.header.frame_id = "map";
+        goal.pose.position.x = position.x;
+        goal.pose.position.y = position.y;
+        goal.pose.position.z = 0.0;
+
+        // Convert theta (yaw) to Quaternion using setRPY
+        // setRPY(roll, pitch, yaw) - for 2D navigation, only yaw is used
+        tf3::Quaternion quat;
+        quat.setRPY(0.0, 0.0, position.theta);
+        goal.pose.orientation.x = quat.x();
+        goal.pose.orientation.y = quat.y();
+        goal.pose.orientation.z = quat.z();
+        goal.pose.orientation.w = quat.w();
+
         VDA5050ClientAPI::move_base_ptr_->moveTo(order_msg, goal);
       }
       else
@@ -260,7 +257,6 @@ namespace amr_control
       robot_geometry_msgs::Vector3 angular;
       angular.z = cmd_vel_max_.theta;
       VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
- 
     }
   }
 
@@ -397,8 +393,6 @@ namespace amr_control
       robot_geometry_msgs::Vector3 angular;
       angular.z = cmd_vel_max_.theta;
       VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
-
-
     }
   }
 

Controllers/Packages/amr_startup/rviz/navigation.rviz

@@ -7,6 +7,8 @@ Panels:
         - /Global Options1
         - /Grid1/Offset1
         - /TF1/Frames1
+        - /Global Map1
+        - /Local Map1
       Splitter Ratio: 0.37295082211494446
     Tree Height: 682
   - Class: rviz/Selection
@@ -363,13 +365,13 @@ Visualization Manager:
           Buffer Length: 1
           Class: rviz/Path
           Color: 25; 255; 0
-          Enabled: false
+          Enabled: true
           Head Diameter: 0.30000001192092896
           Head Length: 0.20000000298023224
           Length: 0.20000000298023224
           Line Style: Lines
           Line Width: 0.05000000074505806
-          Name: Full Plan
+          Name: Plan
           Offset:
             X: 0
             Y: 0
@@ -380,9 +382,9 @@ Visualization Manager:
           Radius: 0.019999999552965164
           Shaft Diameter: 0.10000000149011612
           Shaft Length: 0.10000000149011612
-          Topic: /amr_node/SBPLLatticePlanner/plan
+          Topic: /global_costmap/costmap/plan
           Unreliable: false
-          Value: false
+          Value: true
         - Alpha: 1
           Class: rviz/Polygon
           Color: 255; 0; 0
@@ -392,82 +394,10 @@ Visualization Manager:
           Topic: /global_costmap/costmap/footprint
           Unreliable: false
           Value: true
-        - Alpha: 1
-          Buffer Length: 1
-          Class: rviz/Path
-          Color: 25; 255; 0
-          Enabled: true
-          Head Diameter: 0.30000001192092896
-          Head Length: 0.20000000298023224
-          Length: 0.00800000037997961
-          Line Style: Billboards
-          Line Width: 0.009999999776482582
-          Name: CustomPath
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0.8999999761581421
-          Pose Color: 255; 85; 255
-          Pose Style: None
-          Queue Size: 10
-          Radius: 0.004999999888241291
-          Shaft Diameter: 0.10000000149011612
-          Shaft Length: 0.10000000149011612
-          Topic: /amr_node/CustomPlanner/plan
-          Unreliable: false
-          Value: true
-        - Alpha: 1
-          Buffer Length: 1
-          Class: rviz/Path
-          Color: 0; 0; 0
-          Enabled: false
-          Head Diameter: 0.30000001192092896
-          Head Length: 0.20000000298023224
-          Length: 0.30000001192092896
-          Line Style: Lines
-          Line Width: 0.029999999329447746
-          Name: Straight Path
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0
-          Pose Color: 255; 85; 255
-          Pose Style: Axes
-          Queue Size: 10
-          Radius: 0.029999999329447746
-          Shaft Diameter: 0.10000000149011612
-          Shaft Length: 0.10000000149011612
-          Topic: /amr_node/LocalPlannerAdapter/MKTGoStraightLocalPlanner/global_plan
-          Unreliable: false
-          Value: false
       Enabled: true
       Name: Global Map
     - Class: rviz/Group
       Displays:
-        - Alpha: 1
-          Buffer Length: 1
-          Class: rviz/Path
-          Color: 252; 175; 62
-          Enabled: false
-          Head Diameter: 0.30000001192092896
-          Head Length: 0.20000000298023224
-          Length: 0.03999999910593033
-          Line Style: Lines
-          Line Width: 0.029999999329447746
-          Name: Global Path
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0
-          Pose Color: 255; 85; 255
-          Pose Style: Axes
-          Queue Size: 10
-          Radius: 0.009999999776482582
-          Shaft Diameter: 0.10000000149011612
-          Shaft Length: 0.10000000149011612
-          Topic: /amr_node/LocalPlannerAdapter/global_plan
-          Unreliable: false
-          Value: false
         - Alpha: 0.699999988079071
           Class: rviz/Map
           Color Scheme: map
@@ -491,13 +421,13 @@ Visualization Manager:
           Buffer Length: 1
           Class: rviz/Path
           Color: 138; 226; 52
-          Enabled: false
+          Enabled: true
           Head Diameter: 0.30000001192092896
           Head Length: 0.20000000298023224
           Length: 0.029999999329447746
           Line Style: Billboards
           Line Width: 0.029999999329447746
-          Name: Local Plan
+          Name: Plan
           Offset:
             X: 0
             Y: 0
@@ -508,37 +438,9 @@ Visualization Manager:
           Radius: 0.009999999776482582
           Shaft Diameter: 0.10000000149011612
           Shaft Length: 0.10000000149011612
-          Topic: /amr_node/LocalPlannerAdapter/transformed_global_plan
+          Topic: /local_costmap/costmap/plan
           Unreliable: false
-          Value: false
+          Value: true
-        - Alpha: 1
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 10
-            Min Value: -10
-            Value: true
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz/PointCloud2
-          Color: 255; 255; 255
-          Color Transformer: Intensity
-          Decay Time: 0
-          Enabled: false
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Min Color: 0; 0; 0
-          Name: PointCloud2
-          Position Transformer: XYZ
-          Queue Size: 10
-          Selectable: true
-          Size (Pixels): 3
-          Size (m): 0.009999999776482582
-          Style: Flat Squares
-          Topic: /amr_node/PredictiveTrajectory/cost_cloud
-          Unreliable: false
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: false
         - Class: rviz/Group
           Displays:
             - Alpha: 1
@@ -702,10 +604,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: -62.776824951171875
+      Scale: -105.0660171508789
       Target Frame: base_link
-      X: 1.189023494720459
+      X: 2.555877447128296
-      Y: 0.44669991731643677
+      Y: 1.0760011672973633
     Saved:
       - Angle: -34.55989074707031
         Class: rviz/TopDownOrtho
@@ -727,7 +629,7 @@ Window Geometry:
   Height: 833
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002e7fc0200000008fb000000100044006900730070006c006100790073010000003d000002e7000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000002d9000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002e7fc0200000008fb000000100044006900730070006c006100790073000000003d000002e7000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d0065010000000000000450000000000000000000000435000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

pnkx_nav_core

@@ -1 +1 @@
-Subproject commit 57b77ac14b144acddccc95cc9dd7e8d4d58728dc
+Subproject commit 0f58db34810790067177408f37dc0becb91620c1