planner_cspace: publish internally used maps as OccupancyGrid

neonavigation_launch/config/navigate.yaml

@@ -31,7 +31,6 @@ planner_3d:
   freq: 2.0
   freq_min: 0.5
   local_range: 2.0
-  remember_update: true
 spur:
   hz: 30.0
   k_dist: 16.0

neonavigation_launch/config/visualization.rviz

@@ -25,7 +25,9 @@ Panels:
     Experimental: true
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: Distance Map (debug output)
+Toolbars:
+  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -184,14 +186,35 @@ Visualization Manager:
       Size (Pixels): 5
       Size (m): 0.05000000074505806
       Style: Flat Squares
-      Topic: /planner_3d/debug
+      Topic: /planner_3d/distance_map
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: false
       Value: true
+    - Alpha: 0.200000003
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: false
+      Name: Hysteresis Map (debug output)
+      Topic: /planner_3d/hysteresis_map
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 0.300000012
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: false
+      Name: Remembered Costmap (debug output)
+      Topic: /planner_3d/remembered_map
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
+    Default Light: true
     Fixed Frame: map
     Frame Rate: 5
   Name: root

neonavigation_launch/launch/demo.launch

@@ -3,6 +3,13 @@
   <arg name="use_path_with_velocity" default="true" />
   <arg name="vel" default="0.5" />
 
+  <rosparam>
+    planner_3d:
+      remember_updates: true
+      hist_ignore_range: 0.0
+      hist_ignore_range_max: 1.5
+  </rosparam>
+
   <include file="$(find neonavigation_launch)/launch/navigate.launch">
     <arg name="simulate" value="true" />
     <arg name="map_file" value="$(find neonavigation_launch)/map/demo_map.yaml" />

planner_cspace/src/planner_3d.cpp

@@ -42,6 +42,7 @@
 #include <geometry_msgs/PoseArray.h>
 #include <nav_msgs/GetPlan.h>
 #include <nav_msgs/Path.h>
+#include <nav_msgs/OccupancyGrid.h>
 #include <planner_cspace_msgs/PlannerStatus.h>
 #include <sensor_msgs/PointCloud.h>
 #include <std_srvs/Empty.h>
@@ -89,8 +90,9 @@ class Planner3dNode
   ros::Publisher pub_path_;
   ros::Publisher pub_path_velocity_;
   ros::Publisher pub_path_poses_;
-  ros::Publisher pub_debug_;
-  ros::Publisher pub_hist_;
+  ros::Publisher pub_hysteresis_map_;
+  ros::Publisher pub_distance_map_;
+  ros::Publisher pub_remembered_map_;
   ros::Publisher pub_start_;
   ros::Publisher pub_end_;
   ros::Publisher pub_status_;
@@ -222,13 +224,6 @@ class Planner3dNode
   int goal_tolerance_ang_;
   float angle_resolution_aspect_;
 
-  enum DebugMode
-  {
-    DEBUG_HYSTERESIS,
-    DEBUG_COST_ESTIM
-  };
-  DebugMode debug_out_;
-
   planner_cspace_msgs::PlannerStatus status_;
 
   bool find_best_;
@@ -734,23 +729,20 @@ class Planner3dNode
       has_hysteresis_map_ = false;
     }
 
-    publishCostmap();
+    publishDebug();
 
     goal_updated_ = true;
 
     return true;
   }
-  void publishCostmap()
+  void publishDebug()
   {
-    if (pub_debug_.getNumSubscribers() == 0)
-      return;
-
-    sensor_msgs::PointCloud debug;
-    debug.header = map_header_;
-    debug.header.stamp = ros::Time::now();
+    if (pub_distance_map_.getNumSubscribers() > 0)
     {
-      Astar::Vec p;
-      for (p[1] = 0; p[1] < cost_estim_cache_.size()[1]; p[1]++)
+      sensor_msgs::PointCloud distance_map;
+      distance_map.header = map_header_;
+      distance_map.header.stamp = ros::Time::now();
+      for (Astar::Vec p(0, 0, 0); p[1] < cost_estim_cache_.size()[1]; p[1]++)
       {
         for (p[0] = 0; p[0] < cost_estim_cache_.size()[0]; p[0]++)
         {
@@ -760,29 +752,63 @@ class Planner3dNode
           geometry_msgs::Point32 point;
           point.x = x;
           point.y = y;
-          switch (debug_out_)
-          {
-            case DEBUG_HYSTERESIS:
-              if (cost_estim_cache_[p] == FLT_MAX)
-                continue;
+          if (cost_estim_cache_[p] == FLT_MAX)
+            continue;
+          point.z = cost_estim_cache_[p] / 500;
+          distance_map.points.push_back(point);
+        }
+      }
+      pub_distance_map_.publish(distance_map);
+    }
 
-              point.z = 100;
-              for (p[2] = 0; p[2] < static_cast<int>(map_info_.angle); ++p[2])
-                point.z = std::min(static_cast<float>(cm_hyst_[p]), point.z);
+    if (pub_hysteresis_map_.getNumSubscribers() > 0)
+    {
+      nav_msgs::OccupancyGrid hysteresis_map;
+      hysteresis_map.header.frame_id = map_header_.frame_id;
+      hysteresis_map.info.resolution = map_info_.linear_resolution;
+      hysteresis_map.info.width = map_info_.width;
+      hysteresis_map.info.height = map_info_.height;
+      hysteresis_map.info.origin = map_info_.origin;
+      hysteresis_map.data.resize(map_info_.width * map_info_.height, 100);
 
-              point.z *= 0.01;
-              break;
-            case DEBUG_COST_ESTIM:
-              if (cost_estim_cache_[p] == FLT_MAX)
-                continue;
-              point.z = cost_estim_cache_[p] / 500;
-              break;
+      for (Astar::Vec p(0, 0, 0); p[1] < cost_estim_cache_.size()[1]; p[1]++)
+      {
+        for (p[0] = 0; p[0] < cost_estim_cache_.size()[0]; p[0]++)
+        {
+          if (cost_estim_cache_[p] == FLT_MAX)
+            continue;
+
+          char cost = 100;
+          for (Astar::Vec p2 = p; p2[2] < static_cast<int>(map_info_.angle); ++p2[2])
+          {
+            cost = std::min(cm_hyst_[p2], cost);
           }
-          debug.points.push_back(point);
+          hysteresis_map.data[p[0] + p[1] * map_info_.width] = cost;
         }
       }
+      pub_hysteresis_map_.publish(hysteresis_map);
+    }
+  }
+  void publishRememberedMap()
+  {
+    if (pub_remembered_map_.getNumSubscribers() > 0 && remember_updates_)
+    {
+      nav_msgs::OccupancyGrid remembered_map;
+      remembered_map.header.frame_id = map_header_.frame_id;
+      remembered_map.info.resolution = map_info_.linear_resolution;
+      remembered_map.info.width = map_info_.width;
+      remembered_map.info.height = map_info_.height;
+      remembered_map.info.origin = map_info_.origin;
+      remembered_map.data.resize(map_info_.width * map_info_.height);
+
+      const auto generate_pointcloud = [this, &remembered_map](const Astar::Vec& p, bbf::BinaryBayesFilter& bbf)
+      {
+        remembered_map.data[p[0] + p[1] * map_info_.width] =
+            (bbf.getProbability() - bbf::MIN_PROBABILITY) * 100 / (bbf::MAX_PROBABILITY - bbf::MIN_PROBABILITY);
+      };
+      bbf_costmap_.forEach(generate_pointcloud);
+      pub_remembered_map_.publish(remembered_map);
     }
-    pub_debug_.publish(debug);
   }
   void publishEmptyPath()
   {
@@ -881,28 +907,7 @@ class Planner3dNode
           &cm_updates_, s,
           remember_hit_odds_, remember_miss_odds_,
           hist_ignore_range_, hist_ignore_range_max_);
-
-      if (pub_hist_.getNumSubscribers() > 0)
-      {
-        sensor_msgs::PointCloud pc;
-        pc.header = map_header_;
-        pc.header.stamp = now;
-        const auto generate_pointcloud = [this, &pc](const Astar::Vec& p, bbf::BinaryBayesFilter& bbf)
-        {
-          if (bbf.get() <= bbf::probabilityToOdds(0.1))
-            return;
-          float x, y, yaw;
-          grid_metric_converter::grid2Metric(map_info_, p[0], p[1], p[2], x, y, yaw);
-          geometry_msgs::Point32 point;
-          point.x = x;
-          point.y = y;
-          point.z = bbf.getProbability();
-          pc.points.push_back(point);
-        };
-        bbf_costmap_.forEach(generate_pointcloud);
-        pub_hist_.publish(pc);
-      }
-
+      publishRememberedMap();
       bbf_costmap_.updateCostmap();
     }
     if (!has_goal_)
@@ -1014,7 +1019,7 @@ class Planner3dNode
     const auto tnow = boost::chrono::high_resolution_clock::now();
     ROS_DEBUG("Cost estimation cache updated (%0.4f sec.)",
               boost::chrono::duration<float>(tnow - ts).count());
-    publishCostmap();
+    publishDebug();
   }
   void cbMap(const costmap_cspace_msgs::CSpace3D::ConstPtr& msg)
   {
@@ -1214,8 +1219,6 @@ class Planner3dNode
     sub_goal_ = neonavigation_common::compat::subscribe(
         nh_, "move_base_simple/goal",
         pnh_, "goal", 1, &Planner3dNode::cbGoal, this);
-    pub_debug_ = pnh_.advertise<sensor_msgs::PointCloud>("debug", 1, true);
-    pub_hist_ = pnh_.advertise<sensor_msgs::PointCloud>("remembered", 1, true);
     pub_start_ = pnh_.advertise<geometry_msgs::PoseStamped>("path_start", 1, true);
     pub_end_ = pnh_.advertise<geometry_msgs::PoseStamped>("path_end", 1, true);
     pub_status_ = pnh_.advertise<planner_cspace_msgs::PlannerStatus>("status", 1, true);
@@ -1224,6 +1227,11 @@ class Planner3dNode
         pnh_, "forget", &Planner3dNode::cbForget, this);
     srs_make_plan_ = pnh_.advertiseService("make_plan", &Planner3dNode::cbMakePlan, this);
 
+    // Debug outputs
+    pub_distance_map_ = pnh_.advertise<sensor_msgs::PointCloud>("distance_map", 1, true);
+    pub_hysteresis_map_ = pnh_.advertise<nav_msgs::OccupancyGrid>("hysteresis_map", 1, true);
+    pub_remembered_map_ = pnh_.advertise<nav_msgs::OccupancyGrid>("remembered_map", 1, true);
+
     act_.reset(new Planner3DActionServer(ros::NodeHandle(), "move_base", false));
     act_->registerGoalCallback(boost::bind(&Planner3dNode::cbAction, this));
     act_->registerPreemptCallback(boost::bind(&Planner3dNode::cbPreempt, this));
@@ -1311,12 +1319,12 @@ class Planner3dNode
     {
       ROS_WARN("planner_3d: Experimental fast_map_update is enabled. ");
     }
-    std::string debug_mode;
-    pnh_.param("debug_mode", debug_mode, std::string("cost_estim"));
-    if (debug_mode == "hyst")
-      debug_out_ = DEBUG_HYSTERESIS;
-    else if (debug_mode == "cost_estim")
-      debug_out_ = DEBUG_COST_ESTIM;
+    if (pnh_.hasParam("debug_mode"))
+    {
+      ROS_ERROR(
+          "planner_3d: ~/debug_mode parameter and ~/debug topic are deprecated. "
+          "Use ~/distance_map, ~/hysteresis_map, and ~/remembered_map topics instead.");
+    }
 
     bool print_planning_duration;
     pnh_.param("print_planning_duration", print_planning_duration, false);
@@ -1755,7 +1763,7 @@ class Planner3dNode
       const auto tnow = boost::chrono::high_resolution_clock::now();
       ROS_DEBUG("Hysteresis map generated (%0.4f sec.)",
                 boost::chrono::duration<float>(tnow - ts).count());
-      publishCostmap();
+      publishDebug();
     }
 
     return true;

planner_cspace/test/CMakeLists.txt

@@ -28,6 +28,13 @@ catkin_add_gtest(test_blockmem_gridmap_performance
 target_link_libraries(test_blockmem_gridmap_performance ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenMP_CXX_FLAGS})
 set_target_properties(test_blockmem_gridmap_performance PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_RELEASE}")  # Force release build for performance test.
 
+add_rostest_gtest(test_debug_outputs
+  test/debug_outputs_rostest.test
+  src/test_debug_outputs.cpp
+)
+target_link_libraries(test_debug_outputs ${catkin_LIBRARIES})
+add_dependencies(test_debug_outputs planner_3d)
+
 add_rostest_gtest(test_navigate
   test/navigation_rostest.test
   src/test_navigate.cpp

planner_cspace/test/data/goal_debug_outputs.yaml

@@ -0,0 +1,16 @@
+header:
+  seq: 0
+  stamp:
+    secs: 0
+    nsecs: 0
+  frame_id: 'map'
+pose:
+  position:
+    x: 1.0
+    y: 0.475
+    z: 0.0
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 1.0
+    w: 0.0