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_;
-    CostmapObject local_costmap_obj_;
+    PlannerObject global_planner_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