update

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

@@ -29,6 +29,7 @@
 #include <amr_control/tf_converter.h>
 #include <amr_control/sensor_converter.h>
 #include <amr_control/amr_publiser.h>
+#include <amr_control/amr_subcriber.h>
 
 #include <boost/dll/import.hpp>
 
@@ -147,7 +148,8 @@ namespace amr_control
     robot::TFListenerPtr tf_core_ptr_;
     std::shared_ptr<amr_control::TfConverter> tf_converter_ptr_;
     std::shared_ptr<amr_control::SensorConverter> sensor_converter_ptr_;
-    std::shared_ptr<amr_control::AmrPublisher> costmap_publisher_ptr_;
+    std::shared_ptr<amr_control::AmrPublisher> amr_publisher_ptr_;
+    std::shared_ptr<amr_control::AmrSubscriber> amr_subscriber_ptr_;
 
     ros::Subscriber is_detected_maker_sub_;
     std::shared_ptr<amr_control::OpcUAServerAPI> opc_ua_server_api_ptr_;

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

@@ -2,13 +2,13 @@
 #define __AMR_PUBLISHER_H_INCLUDED_
 
 #include <ros/ros.h>
+#include <robot/robot.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <geometry_msgs/PolygonStamped.h>
 #include <robot_nav_msgs/OccupancyGrid.h>
 #include <robot_map_msgs/OccupancyGridUpdate.h>
 #include <map_msgs/OccupancyGridUpdate.h>
 #include <move_base_core/navigation.h>
-#include <robot/time.h>
 #include <memory>
 #include <string>
 
@@ -96,7 +96,18 @@ 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_; }
+    bool isPublishing() const { return global_costmap_obj_.active_ || local_costmap_obj_.active_ || cmd_vel_publishing_active_; }
+
+    /**
+     * @brief Start publishing cmd_vel at a fixed rate using ros::WallTimer
+     * @param rate Publishing rate in Hz (default: 20.0 Hz)
+     */
+     void startCmdVelPublishing(double rate = 20.0);
+
+     /**
+      * @brief Stop publishing cmd_vel
+      */
+     void stopCmdVelPublishing();
 
   private:
     /**
@@ -142,11 +153,27 @@ namespace amr_control
      */
     void localCostmapTimerCallback(const ros::TimerEvent &event);
 
+    /**
+     * @brief Publish cmd_vel command
+     */
+    void publishCmdVel();
+
+    /**
+     * @brief WallTimer callback for publishing cmd_vel
+     * @param event WallTimer event
+     */
+    void cmdVelTimerCallback(const ros::WallTimerEvent &event);
+
     ros::NodeHandle nh_;
     robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
 
     CostmapObject global_costmap_obj_;
     CostmapObject local_costmap_obj_;
+    ros::Publisher cmd_vel_pub_;
+    
+    // ros::WallTimer for cmd_vel publishing
+    ros::WallTimer cmd_vel_timer_;
+    bool cmd_vel_publishing_active_;
   };
 
 } // namespace amr_control

Controllers/Packages/amr_control/include/amr_control/amr_subcriber.h

@@ -0,0 +1,28 @@
+#ifndef __AMR_SUBCRIBER_H_INCLUDED_
+#define __AMR_SUBCRIBER_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <robot/robot.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <move_base_core/navigation.h>
+#include <memory>
+#include <string>
+
+namespace amr_control
+{
+  class AmrSubscriber
+  {
+  public:
+    AmrSubscriber(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr);
+    virtual ~AmrSubscriber();
+
+  private:
+    void moveBaseSimpleCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);
+    
+    ros::NodeHandle nh_;
+    ros::Subscriber move_base_simple_sub_;
+    robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
+  };
+} // namespace amr_control
+
+#endif // __AMR_SUBCRIBER_H_INCLUDED_

Controllers/Packages/amr_control/include/amr_control/sensor_converter.h

@@ -1,6 +1,7 @@
 #ifndef __SENSOR_CONVERTER_H_INCLUDED_
 #define __SENSOR_CONVERTER_H_INCLUDED_
 #include <ros/ros.h>
+#include <robot/robot.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <nav_msgs/GetMap.h>
 #include <sensor_msgs/LaserScan.h>
@@ -10,8 +11,6 @@
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_geometry_msgs/Point.h>
 #include <robot_geometry_msgs/PoseStamped.h>
-#include <robot/console.h>
-#include <robot/time.h>
 #include <move_base_core/navigation.h>
 #include <memory>
 

Controllers/Packages/amr_control/include/amr_control/tf_converter.h

@@ -2,8 +2,7 @@
 #define __TF_CONVERTER_H_INCLUDED_
 
 #include <ros/ros.h>
-#include <robot/console.h>
-#include <robot/time.h>
+#include <robot/robot.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf3/buffer_core.h>
 #include <tf3/exceptions.h>