test costmap + cam depth

CMakeLists.txt

@@ -54,6 +54,6 @@ install(TARGETS depth_local_costmap_noetic_test_node
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-install(DIRECTORY config launch
+install(DIRECTORY config launch rviz
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )

README.md

@@ -35,6 +35,12 @@ source devel/setup.bash
 roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch
 ```
 
+Launch mặc định mở RViz với config sẵn. Nếu chỉ muốn chạy node không mở GUI:
+
+```bash
+roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch rviz:=false
+```
+
 Nếu robot dùng `base_footprint`:
 
 ```bash
@@ -55,6 +61,8 @@ Không bật `publish_odom_tf` nếu đã có node khác publish TF này.
 rostopic echo -n1 /camera/depth/points_proc/header
 rostopic echo -n1 /camera/depth/image_raw/header
 rostopic echo -n1 /camera/depth/camera_info/header
+rostopic echo -n1 /local_costmap/costmap/info
+rostopic echo -n1 /local_costmap/costmap/footprint/header
 rosrun tf tf_echo odom base_link
 rostopic list | grep local_costmap
 ```
@@ -65,6 +73,12 @@ Log runtime cần thấy:
 
 - `Fed depth cloud to robot_costmap_2d`
 - `Fed DepthCameraData to robot_costmap_2d`
+- `Published local costmap for RViz`
+
+Các topic RViz giống hướng `amr_control::AmrPublisher`:
+
+- `/local_costmap/costmap` kiểu `nav_msgs/OccupancyGrid`
+- `/local_costmap/costmap/footprint` kiểu `geometry_msgs/PolygonStamped`
 
 Config local cho `robot_costmap_2d` nằm trong thư mục `config/config/` của package này khi export:
 

launch/depth_local_costmap_test.launch

@@ -8,6 +8,8 @@
   <arg name="camera_right_info_topic" default="/camera_right/depth/camera_info"/>
   <arg name="odom_topic" default="/odom"/>
   <arg name="publish_odom_tf" default="false"/>
+  <arg name="publish_rate" default="5.0"/>
+  <arg name="rviz" default="true"/>
 
   <node pkg="depth_local_costmap_noetic_test"
         type="depth_local_costmap_noetic_test_node"
@@ -15,6 +17,7 @@
         output="screen">
     <param name="base_frame" value="$(arg base_frame)"/>
     <param name="publish_odom_tf" value="$(arg publish_odom_tf)"/>
+    <param name="publish_rate" value="$(arg publish_rate)"/>
     <param name="odom_topic" value="$(arg odom_topic)"/>
     <param name="depth_cloud_topic" value="$(arg depth_cloud_topic)"/>
     <param name="depth_image_topic" value="$(arg depth_image_topic)"/>
@@ -56,4 +59,10 @@
     <param name="local_costmap/obstacles/pc_clearing/max_obstacle_height" value="1.0"/>
     <param name="local_costmap/obstacles/frustum_depth_camera_topic" value="$(arg depth_camera_data_topic)"/>
   </node>
+
+  <node if="$(arg rviz)"
+        pkg="rviz"
+        type="rviz"
+        name="depth_local_costmap_rviz"
+        args="-d $(find depth_local_costmap_noetic_test)/rviz/depth_local_costmap.rviz"/>
 </launch>

package.xml

@@ -21,4 +21,5 @@
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>
   <depend>tf2_ros</depend>
+  <exec_depend>rviz</exec_depend>
 </package>

rviz/depth_local_costmap.rviz

@@ -0,0 +1,107 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Local Costmap1
+        - /Local Footprint1
+      Splitter Ratio: 0.5
+    Tree Height: 560
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 20
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 0.4
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 0.75
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Local Costmap
+      Topic: /local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Local Footprint
+      Topic: /local_costmap/costmap/footprint
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+  Value: true
+  Views:
+    Current:
+      Class: rviz/TopDownOrtho
+      Name: Current View
+      Scale: 12
+      Target Frame: <Fixed Frame>
+      X: 0
+      Y: 0
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 900
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f40000032efc0200000001fb000000100044006900730070006c006100790073010000003d0000032e000000c900ffffff000003840000032e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 80
+  Y: 60

src/depth_local_costmap_noetic_test_node.cpp

@@ -1,9 +1,13 @@
 #include <robot_costmap_2d/costmap_2d.h>
 #include <robot_costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/cost_values.h>
 #include <robot_costmap_2d/layer.h>
 
+#include <geometry_msgs/Point32.h>
+#include <geometry_msgs/PolygonStamped.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <nav_msgs/Odometry.h>
+#include <nav_msgs/OccupancyGrid.h>
 #include <robot_depth_image_proc/ros_message_conversions.h>
 #include <robot/robot.h>
 #include <robot_geometry_msgs/PoseStamped.h>
@@ -23,6 +27,7 @@
 #include <tf3/buffer_core.h>
 
 #include <atomic>
+#include <cmath>
 #include <exception>
 #include <memory>
 #include <mutex>
@@ -31,8 +36,14 @@
 #include <thread>
 #include <vector>
 
+#include <boost/thread/locks.hpp>
+
 namespace
 {
+constexpr double kOccupancyScale =
+    99.0 / static_cast<double>(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE -
+                               robot_costmap_2d::FREE_SPACE);
+
 robot_sensor_msgs::PointCloud2 toRobotPointCloud2(const sensor_msgs::PointCloud2& msg)
 {
   robot_sensor_msgs::PointCloud2 robot_pc;
@@ -63,10 +74,31 @@ robot_sensor_msgs::PointCloud2 toRobotPointCloud2(const sensor_msgs::PointCloud2
   return robot_pc;
 }
 
+int8_t toOccupancyValue(const unsigned char cost)
+{
+  if (cost == robot_costmap_2d::NO_INFORMATION)
+    return -1;
+  if (cost >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+    return 100;
+  if (cost <= robot_costmap_2d::FREE_SPACE)
+    return 0;
+
+  const double scaled = static_cast<double>(cost - robot_costmap_2d::FREE_SPACE) * kOccupancyScale;
+  return static_cast<int8_t>(std::max(0.0, std::min(99.0, std::round(scaled))));
+}
+
+double yawFromQuaternion(const robot_geometry_msgs::Quaternion& q)
+{
+  const double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
+  const double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
+  return std::atan2(siny_cosp, cosy_cosp);
+}
+
 bool isVoxelLayer(const robot_costmap_2d::Layer& layer)
 {
   return layer.getType() == robot_costmap_2d::LayerType::VOXEL_LAYER ||
-         layer.getName() == "local_costmap/voxel_layer";
+         layer.getName() == "local_costmap/voxel_layer" ||
+         layer.getName() == "local_costmap/obstacles";
 }
 
 template <typename MessageT>
@@ -379,6 +411,105 @@ private:
 
   ros::Subscriber camera_right_info_sub_;
 };
+
+class LocalCostmapRvizPublisher
+{
+public:
+  explicit LocalCostmapRvizPublisher(ros::NodeHandle& private_nh)
+  {
+    private_nh.param("local_costmap_topic", local_costmap_topic_, std::string("/local_costmap/costmap"));
+    private_nh.param("local_footprint_topic",
+                     local_footprint_topic_,
+                     local_costmap_topic_ + "/footprint");
+
+    costmap_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>(local_costmap_topic_, 1);
+    footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>(local_footprint_topic_, 1);
+
+    ROS_INFO("Publishing local costmap for RViz: %s", local_costmap_topic_.c_str());
+    ROS_INFO("Publishing local footprint for RViz: %s", local_footprint_topic_.c_str());
+  }
+
+  void publish(robot_costmap_2d::Costmap2DROBOT& local_costmap,
+               const bool pose_ok,
+               const robot_geometry_msgs::PoseStamped& robot_pose)
+  {
+    publishCostmap(local_costmap);
+    if (pose_ok)
+      publishFootprint(local_costmap, robot_pose);
+  }
+
+private:
+  void publishCostmap(robot_costmap_2d::Costmap2DROBOT& local_costmap)
+  {
+    robot_costmap_2d::Costmap2D* costmap = local_costmap.getCostmap();
+    if (costmap == nullptr)
+      return;
+
+    nav_msgs::OccupancyGrid grid;
+    grid.header.stamp = ros::Time::now();
+    grid.header.frame_id = local_costmap.getGlobalFrameID();
+    grid.info.map_load_time = grid.header.stamp;
+
+    {
+      boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*costmap->getMutex());
+
+      const unsigned int size_x = costmap->getSizeInCellsX();
+      const unsigned int size_y = costmap->getSizeInCellsY();
+      grid.info.resolution = costmap->getResolution();
+      grid.info.width = size_x;
+      grid.info.height = size_y;
+      grid.info.origin.position.x = costmap->getOriginX();
+      grid.info.origin.position.y = costmap->getOriginY();
+      grid.info.origin.position.z = 0.0;
+      grid.info.origin.orientation.w = 1.0;
+
+      const unsigned char* char_map = costmap->getCharMap();
+      grid.data.resize(static_cast<std::size_t>(size_x) * size_y);
+      for (std::size_t index = 0; index < grid.data.size(); ++index)
+        grid.data[index] = toOccupancyValue(char_map[index]);
+    }
+
+    costmap_pub_.publish(grid);
+    ROS_INFO_THROTTLE(5.0, "Published local costmap for RViz: %ux%u frame=%s topic=%s",
+                      grid.info.width,
+                      grid.info.height,
+                      grid.header.frame_id.c_str(),
+                      local_costmap_topic_.c_str());
+  }
+
+  void publishFootprint(robot_costmap_2d::Costmap2DROBOT& local_costmap,
+                        const robot_geometry_msgs::PoseStamped& robot_pose)
+  {
+    const robot_geometry_msgs::Polygon footprint = local_costmap.getRobotFootprintPolygon();
+    if (footprint.points.empty())
+      return;
+
+    const double yaw = yawFromQuaternion(robot_pose.pose.orientation);
+    const double cos_yaw = std::cos(yaw);
+    const double sin_yaw = std::sin(yaw);
+
+    geometry_msgs::PolygonStamped footprint_msg;
+    footprint_msg.header.stamp = ros::Time::now();
+    footprint_msg.header.frame_id = local_costmap.getGlobalFrameID();
+    footprint_msg.polygon.points.reserve(footprint.points.size());
+
+    for (const auto& point : footprint.points)
+    {
+      geometry_msgs::Point32 transformed;
+      transformed.x = static_cast<float>(robot_pose.pose.position.x + point.x * cos_yaw - point.y * sin_yaw);
+      transformed.y = static_cast<float>(robot_pose.pose.position.y + point.x * sin_yaw + point.y * cos_yaw);
+      transformed.z = static_cast<float>(robot_pose.pose.position.z + point.z);
+      footprint_msg.polygon.points.push_back(transformed);
+    }
+
+    footprint_pub_.publish(footprint_msg);
+  }
+
+  std::string local_costmap_topic_;
+  std::string local_footprint_topic_;
+  ros::Publisher costmap_pub_;
+  ros::Publisher footprint_pub_;
+};
 }  // namespace
 
 int main(int argc, char** argv)
@@ -391,8 +522,11 @@ int main(int argc, char** argv)
 
   std::string base_frame;
   bool publish_odom_tf = false;
+  double publish_rate = 5.0;
   private_nh.param("base_frame", base_frame, std::string("base_link"));
   private_nh.param("publish_odom_tf", publish_odom_tf, false);
+  private_nh.param("publish_rate", publish_rate, publish_rate);
+  publish_rate = std::max(0.5, publish_rate);
 
   auto tf2_buffer = std::make_shared<tf2_ros::Buffer>(ros::Duration(10.0));
   tf2_ros::TransformListener tf2_listener(*tf2_buffer);
@@ -410,11 +544,12 @@ int main(int argc, char** argv)
     robot_costmap_2d::Costmap2DROBOT local_costmap("local_costmap", tf3_buffer);
     DepthCloudFeeder depth_cloud_feeder(private_nh, local_costmap);
     DepthCameraDataFeeder depth_camera_data_feeder(private_nh, local_costmap);
+    LocalCostmapRvizPublisher rviz_publisher(private_nh);
     local_costmap.start();
 
     ROS_INFO("T800 robot_costmap_2d local costmap test started.");
 
-    ros::Rate rate(2.0);
+    ros::Rate rate(publish_rate);
     while (ros::ok())
     {
       robot_geometry_msgs::PoseStamped robot_pose;
@@ -446,6 +581,7 @@ int main(int argc, char** argv)
             robot_pose.pose.position.y);
       }
 
+      rviz_publisher.publish(local_costmap, pose_ok, robot_pose);
       rate.sleep();
     }