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 index 72ba7acf421..772ede34c70 100644 --- 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::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 index 902507ec9fb..62667949ece 100644 --- 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 index b8217337963..7d013ac75b7 100644 --- 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 index be834f28c32..4856191a7e5 100644 --- 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 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( 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( 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_);