fix flickering visualization (#4561)

* Fix Flickering visualization

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Refactoring Costmap2DPublisher and Costmap2DROS

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Refactoring costmap_2d_ros.cpp

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Refactoring Costmap2DPublisher and Costmap2DROS

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Update costmap_2d_publisher.cpp

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Change map_vis_z from float to double

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

* Add comment to  map_vis_z_ parameter

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

---------

Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>

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_;

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
 

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

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_);