fix flickering visualization (#4561)

* Fix Flickering visualization Signed-off-by: Vladyslav Hrynchak <[email protected]> * Refactoring Costmap2DPublisher and Costmap2DROS Signed-off-by: Vladyslav Hrynchak <[email protected]> * Refactoring costmap_2d_ros.cpp Signed-off-by: Vladyslav Hrynchak <[email protected]> * Refactoring Costmap2DPublisher and Costmap2DROS Signed-off-by: Vladyslav Hrynchak <[email protected]> * Update costmap_2d_publisher.cpp Signed-off-by: Vladyslav Hrynchak <[email protected]> * Change map_vis_z from float to double Signed-off-by: Vladyslav Hrynchak <[email protected]> * Add comment to map_vis_z_ parameter Signed-off-by: Vladyslav Hrynchak <[email protected]> --------- Signed-off-by: Vladyslav Hrynchak <[email protected]>
ros-navigation · Aug 23, 2024 · dc31e7b · dc31e7b
1 parent 32327ab
commit dc31e7b
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 6 deletions.
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
@@ -72,7 +72,8 @@ class Costmap2DPublisher
     Costmap2D * costmap,
     std::string global_frame,
     std::string topic_name,
-    bool always_send_full_costmap = false);
+    bool always_send_full_costmap = false,
+    double map_vis_z = 0.0);
 
   /**
    * @brief  Destructor
@@ -166,6 +167,7 @@ class Costmap2DPublisher
   double saved_origin_y_;
   bool active_;
   bool always_send_full_costmap_;
+  double map_vis_z_;
 
   // Publisher for translated costmap values as msg::OccupancyGrid used in visualization
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_pub_;

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
@@ -415,6 +415,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   bool track_unknown_space_{false};
   double transform_tolerance_{0};           ///< The timeout before transform errors
   double initial_transform_timeout_{0};   ///< The timeout before activation of the node errors
+  double map_vis_z_{0};                 ///< The height of map, allows to avoid flickering at -0.008
 
   bool is_lifecycle_follower_{true};   ///< whether is a child-LifecycleNode or an independent node
 

diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
@@ -54,12 +54,14 @@ Costmap2DPublisher::Costmap2DPublisher(
   Costmap2D * costmap,
   std::string global_frame,
   std::string topic_name,
-  bool always_send_full_costmap)
+  bool always_send_full_costmap,
+  double map_vis_z)
 : costmap_(costmap),
   global_frame_(global_frame),
   topic_name_(topic_name),
   active_(false),
-  always_send_full_costmap_(always_send_full_costmap)
+  always_send_full_costmap_(always_send_full_costmap),
+  map_vis_z_(map_vis_z)
 {
   auto node = parent.lock();
   clock_ = node->get_clock();
@@ -146,7 +148,7 @@ void Costmap2DPublisher::prepareGrid()
   costmap_->mapToWorld(0, 0, wx, wy);
   grid_->info.origin.position.x = wx - grid_resolution_ / 2;
   grid_->info.origin.position.y = wy - grid_resolution_ / 2;
-  grid_->info.origin.position.z = 0.0;
+  grid_->info.origin.position.z = map_vis_z_;
   grid_->info.origin.orientation.w = 1.0;
 
   grid_->data.resize(grid_->info.width * grid_->info.height);

diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -110,6 +110,7 @@ void Costmap2DROS::init()
   RCLCPP_INFO(get_logger(), "Creating Costmap");
 
   declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false));
+  declare_parameter("map_vis_z", rclcpp::ParameterValue(0.0));
   declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f));
   declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]")));
   declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map")));
@@ -225,7 +226,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
   costmap_publisher_ = std::make_unique<Costmap2DPublisher>(
     shared_from_this(),
     layered_costmap_->getCostmap(), global_frame_,
-    "costmap", always_send_full_costmap_);
+    "costmap", always_send_full_costmap_, map_vis_z_);
 
   auto layers = layered_costmap_->getPlugins();
 
@@ -236,7 +237,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
         std::make_unique<Costmap2DPublisher>(
           shared_from_this(),
           costmap_layer.get(), global_frame_,
-          layer->getName(), always_send_full_costmap_)
+          layer->getName(), always_send_full_costmap_, map_vis_z_)
       );
     }
   }
@@ -393,6 +394,7 @@ Costmap2DROS::getParameters()
 
   // Get all of the required parameters
   get_parameter("always_send_full_costmap", always_send_full_costmap_);
+  get_parameter("map_vis_z", map_vis_z_);
   get_parameter("footprint", footprint_);
   get_parameter("footprint_padding", footprint_padding_);
   get_parameter("global_frame", global_frame_);