test costmap + cam depth

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();
     }