improve visualization of obstacles (#385)

rst-tu-dortmund · Oct 5, 2022 · f22b10a · f22b10a
1 parent 9d91cf2
commit f22b10a
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 9 deletions.
diff --git a/include/teb_local_planner/visualization.h b/include/teb_local_planner/visualization.h
@@ -151,8 +151,9 @@ class TebVisualization
    * @brief Publish obstacle positions to the ros topic \e ../../teb_markers
    * @todo Move filling of the marker message to polygon class in order to avoid checking types.
    * @param obstacles Obstacle container
+   * @param scale Size of the non-circular obstacles
    */
-  void publishObstacles(const ObstContainer& obstacles) const;
+  void publishObstacles(const ObstContainer& obstacles, double scale = 0.1) const;
 
   /**
    * @brief Publish via-points to the ros topic \e ../../teb_markers

diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp
@@ -452,7 +452,7 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt
 
   // Now visualize everything    
   planner_->visualize();
-  visualization_->publishObstacles(obstacles_);
+  visualization_->publishObstacles(obstacles_, costmap_->getResolution());
   visualization_->publishViaPoints(via_points_);
   visualization_->publishGlobalPlan(global_plan_);
   return mbf_msgs::ExePathResult::SUCCESS;

diff --git a/src/visualization.cpp b/src/visualization.cpp
@@ -150,7 +150,7 @@ void TebVisualization::publishInfeasibleRobotPose(const PoseSE2& current_pose, c
 }
 
 
-void TebVisualization::publishObstacles(const ObstContainer& obstacles) const
+void TebVisualization::publishObstacles(const ObstContainer& obstacles, double scale) const
 {
   if ( obstacles.empty() || printErrorWhenNotInitialized() )
     return;
@@ -200,8 +200,8 @@ void TebVisualization::publishObstacles(const ObstContainer& obstacles) const
       }
     }
 
-    marker.scale.x = 0.1;
-    marker.scale.y = 0.1;
+    marker.scale.x = scale;
+    marker.scale.y = scale;
     marker.color.a = 1.0;
     marker.color.r = 1.0;
     marker.color.g = 0.0;
@@ -272,8 +272,8 @@ void TebVisualization::publishObstacles(const ObstContainer& obstacles) const
       end.z = 0;
       marker.points.push_back(end);
 
-      marker.scale.x = 0.1;
-      marker.scale.y = 0.1;
+      marker.scale.x = scale;
+      marker.scale.y = scale;
       marker.color.a = 1.0;
       marker.color.r = 0.0;
       marker.color.g = 1.0;
@@ -321,8 +321,8 @@ void TebVisualization::publishObstacles(const ObstContainer& obstacles) const
         point.z = 0;
         marker.points.push_back(point);
       }
-      marker.scale.x = 0.1;
-      marker.scale.y = 0.1;
+      marker.scale.x = scale;
+      marker.scale.y = scale;
       marker.color.a = 1.0;
       marker.color.r = 1.0;
       marker.color.g = 0.0;