upodate

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

@@ -28,6 +28,7 @@
 #include <amr_control/amr_safety.h>
 #include <amr_control/tf_converter.h>
 #include <amr_control/sensor_converter.h>
+#include <amr_control/amr_publiser.h>
 
 #include <boost/dll/import.hpp>
 
@@ -146,6 +147,7 @@ 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_;
 
     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

@@ -0,0 +1,155 @@
+#ifndef __AMR_PUBLISHER_H_INCLUDED_
+#define __AMR_PUBLISHER_H_INCLUDED_
+
+#include <ros/ros.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>
+
+namespace amr_control
+{
+  struct CostmapObject
+  {
+    // ROS publishers
+    ros::Publisher pub_;
+    ros::Publisher update_pub_;
+    ros::Publisher footprint_pub_;
+    ros::Timer timer_;
+    std::string topic_;
+    bool active_;
+  };
+
+  /**
+   * @class AmrPublisher
+   * @brief Publisher class for publishing costmap data from move_base_core to RViz
+   * 
+   * This class converts robot_nav_msgs::OccupancyGrid to nav_msgs::OccupancyGrid
+   * and publishes global and local costmaps to ROS topics that RViz can visualize.
+   */
+  class AmrPublisher
+  {
+  public:
+    /**
+     * @brief Constructor
+     * @param nh ROS NodeHandle for creating publishers
+     * @param move_base_ptr Shared pointer to BaseNavigation instance
+     */
+    AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr);
+    
+    /**
+     * @brief Destructor
+     */
+    virtual ~AmrPublisher();
+
+    /**
+     * @brief Publish global costmap to RViz
+     * @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
+     */
+    void publishGlobalCostmap(double publish_rate = 0.0);
+
+    /**
+     * @brief Publish local costmap to RViz
+     * @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
+     */
+    void publishLocalCostmap(double publish_rate = 0.0);
+
+    /**
+     * @brief Publish both global and local costmaps
+     * @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
+     */
+    void publishCostmaps(double publish_rate = 0.0);
+
+    /**
+     * @brief Publish global footprint to RViz
+     */
+    void publishGlobalFootprint();
+
+    /**
+     * @brief Publish local footprint to RViz
+     */
+    void publishLocalFootprint();
+
+    /**
+     * @brief Publish both global and local footprints
+     */
+    void publishFootprints();
+
+    /**
+     * @brief Start publishing costmaps at a fixed rate
+     * @param global_rate Publishing rate for global costmap (Hz). If 0, don't publish global.
+     * @param local_rate Publishing rate for local costmap (Hz). If 0, don't publish local.
+     */
+    void startPublishing(double global_rate = 1.0, double local_rate = 5.0);
+
+    /**
+     * @brief Stop publishing costmaps
+     */
+    void stopPublishing();
+
+    /**
+     * @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_; }
+
+  private:
+    /**
+     * @brief Callback for new subscription to global costmap
+     * @param pub SingleSubscriberPublisher for the new subscription
+     */
+    void onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher& pub);
+
+    /**
+     * @brief Callback for new subscription to local costmap
+     * @param pub SingleSubscriberPublisher for the new subscription
+     */
+    void onNewSubscriptionLocal(const ros::SingleSubscriberPublisher& pub);
+
+    /**
+     * @brief Convert robot_nav_msgs::OccupancyGrid to nav_msgs::OccupancyGrid
+     * @param robot_grid Input robot_nav_msgs::OccupancyGrid
+     * @param nav_grid Output nav_msgs::OccupancyGrid
+     */
+    void convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid, 
+                                    nav_msgs::OccupancyGrid &nav_grid);
+
+    void convertToRobotOccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate &robot_grid_update, 
+                                           map_msgs::OccupancyGridUpdate &grid_update);
+
+    /**
+     * @brief Convert robot_geometry_msgs::PolygonStamped to geometry_msgs::PolygonStamped
+     * @param robot_footprint Input robot_geometry_msgs::PolygonStamped
+     * @param nav_footprint Output geometry_msgs::PolygonStamped
+     */
+    void convertToNavPolygonStamped(const robot_geometry_msgs::PolygonStamped &robot_footprint, 
+                                     geometry_msgs::PolygonStamped &nav_footprint);
+
+    /**
+     * @brief Timer callback for publishing global costmap
+     * @param event Timer event
+     */
+    void globalCostmapTimerCallback(const ros::TimerEvent &event);
+
+    /**
+     * @brief Timer callback for publishing local costmap
+     * @param event Timer event
+     */
+    void localCostmapTimerCallback(const ros::TimerEvent &event);
+
+    ros::NodeHandle nh_;
+    robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
+
+    CostmapObject global_costmap_obj_;
+    CostmapObject local_costmap_obj_;
+  };
+
+} // namespace amr_control
+
+#endif // __AMR_PUBLISHER_H_INCLUDED_
+