Refactoring Costmap2DPublisher and Costmap2DROS

ros-navigation · Jul 25, 2024 · 89f43fc · 89f43fc
1 parent 07cc3cb
commit 89f43fc
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 10 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,7 @@ class Costmap2DPublisher
     std::string global_frame,
     std::string topic_name,
     bool always_send_full_costmap = false,
-    float map_vis_y = 0.0);
+    float map_vis_z = 0.0);
 
   /**
    * @brief  Destructor
@@ -157,7 +157,7 @@ class Costmap2DPublisher
   double saved_origin_y_;
   bool active_;
   bool always_send_full_costmap_;
-  float map_vis_y_;
+  float 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
@@ -380,7 +380,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
-  float map_vis_y_{0};
+  float map_vis_z_{0};
 
   // Derived parameters
   bool use_radius_{false};

diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
@@ -55,13 +55,13 @@ Costmap2DPublisher::Costmap2DPublisher(
   std::string global_frame,
   std::string topic_name,
   bool always_send_full_costmap,
-  float map_vis_y)
+  float map_vis_z)
 : costmap_(costmap),
   global_frame_(global_frame),
   topic_name_(topic_name),
   active_(false),
   always_send_full_costmap_(always_send_full_costmap),
-  map_vis_y_(map_vis_y)
+  map_vis_z_(map_vis_z)
 {
   auto node = parent.lock();
   clock_ = node->get_clock();
@@ -139,7 +139,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 = map_vis_y_;
+  grid_->info.origin.position.z = map_vis_z_;
   grid_->info.origin.orientation.w = 1.0;
   saved_origin_x_ = costmap_->getOriginX();
   saved_origin_y_ = costmap_->getOriginY();

diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -111,7 +111,7 @@ void Costmap2DROS::init()
   std::vector<std::string> clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"};
 
   declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false));
-  declare_parameter("map_vis_y", rclcpp::ParameterValue(0.0));
+  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")));
@@ -228,7 +228,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_, map_vis_y_);
+    "costmap", always_send_full_costmap_, map_vis_z_);
 
   auto layers = layered_costmap_->getPlugins();
 
@@ -239,7 +239,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_, map_vis_y_)
+          layer->getName(), always_send_full_costmap_, map_vis_z_)
       );
     }
   }
@@ -388,7 +388,7 @@ Costmap2DROS::getParameters()
 
   // Get all of the required parameters
   get_parameter("always_send_full_costmap", always_send_full_costmap_);
-  get_parameter("map_vis_y", map_vis_y_);
+  get_parameter("map_vis_z", map_vis_z_);
   get_parameter("footprint", footprint_);
   get_parameter("footprint_padding", footprint_padding_);
   get_parameter("global_frame", global_frame_);