From 87e8cc9748f030354917c5a1f40e0bdb03f96ecd Mon Sep 17 00:00:00 2001 From: Vladyslav Hrynchak Date: Thu, 18 Jul 2024 10:12:07 +0300 Subject: [PATCH 1/7] Fix Flickering visualization Signed-off-by: Vladyslav Hrynchak --- .../include/nav2_costmap_2d/costmap_2d_publisher.hpp | 4 +++- .../include/nav2_costmap_2d/costmap_2d_ros.hpp | 2 ++ nav2_costmap_2d/src/costmap_2d_publisher.cpp | 8 +++++--- nav2_costmap_2d/src/costmap_2d_ros.cpp | 9 +++++++-- 4 files changed, 17 insertions(+), 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 index 2074ad5ea60..d59983d9e76 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 @@ -71,7 +71,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_y = 0.0); /** * @brief Destructor @@ -156,6 +157,7 @@ class Costmap2DPublisher double saved_origin_y_; bool active_; bool always_send_full_costmap_; + double map_vis_y_; // 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 f319e52f330..aee0219b0c7 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 @@ -381,6 +381,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode 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_y_; + // Derived parameters bool use_radius_{false}; std::vector unpadded_footprint_; diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 87881bc7caa..6a4f2b913a5 100644 --- 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_y) : 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_y_(map_vis_y) { auto node = parent.lock(); clock_ = node->get_clock(); @@ -137,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 = 0.0; + grid_->info.origin.position.z = map_vis_y_; 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 c35933c9083..fc1a0982a90 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -111,6 +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("footprint_padding", rclcpp::ParameterValue(0.01f)); declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]"))); declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map"))); @@ -224,10 +225,14 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) footprint_pub_ = create_publisher( "published_footprint", rclcpp::SystemDefaultsQoS()); + + double map_vis_y_; + get_parameter("map_vis_y", map_vis_y_); + costmap_publisher_ = std::make_unique( shared_from_this(), layered_costmap_->getCostmap(), global_frame_, - "costmap", always_send_full_costmap_); + "costmap", always_send_full_costmap_, map_vis_y_); auto layers = layered_costmap_->getPlugins(); @@ -238,7 +243,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_) + layer->getName(), always_send_full_costmap_, map_vis_y_) ); } } From 45dbb67f7c307aa964bea4518c180e3c2c3c0d4f Mon Sep 17 00:00:00 2001 From: Vladyslav Hrynchak Date: Fri, 19 Jul 2024 15:46:06 +0300 Subject: [PATCH 2/7] Refactoring Costmap2DPublisher and Costmap2DROS Signed-off-by: Vladyslav Hrynchak --- .../include/nav2_costmap_2d/costmap_2d_publisher.hpp | 4 ++-- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 3 +-- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 -- 4 files changed, 4 insertions(+), 7 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 index d59983d9e76..72ba7acf421 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, - double map_vis_y = 0.0); + float map_vis_y = 0.0); /** * @brief Destructor @@ -157,7 +157,7 @@ class Costmap2DPublisher double saved_origin_y_; bool active_; bool always_send_full_costmap_; - double map_vis_y_; + float map_vis_y_; // 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 aee0219b0c7..902507ec9fb 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,8 +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 - - //double map_vis_y_; + float map_vis_y_{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 6a4f2b913a5..b8217337963 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -55,7 +55,7 @@ Costmap2DPublisher::Costmap2DPublisher( std::string global_frame, std::string topic_name, bool always_send_full_costmap, - double map_vis_y) + float map_vis_y) : costmap_(costmap), global_frame_(global_frame), topic_name_(topic_name), diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index fc1a0982a90..1df537e056e 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -225,8 +225,6 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) footprint_pub_ = create_publisher( "published_footprint", rclcpp::SystemDefaultsQoS()); - - double map_vis_y_; get_parameter("map_vis_y", map_vis_y_); costmap_publisher_ = std::make_unique( From f911fe4825fdd6eebed3e027550ac6ab9c32d6d1 Mon Sep 17 00:00:00 2001 From: Vladyslav Hrynchak Date: Tue, 23 Jul 2024 11:15:35 +0300 Subject: [PATCH 3/7] Refactoring costmap_2d_ros.cpp Signed-off-by: Vladyslav Hrynchak --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 1df537e056e..be834f28c32 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -225,8 +225,6 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) footprint_pub_ = create_publisher( "published_footprint", rclcpp::SystemDefaultsQoS()); - get_parameter("map_vis_y", map_vis_y_); - costmap_publisher_ = std::make_unique( shared_from_this(), layered_costmap_->getCostmap(), global_frame_, @@ -390,6 +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("footprint", footprint_); get_parameter("footprint_padding", footprint_padding_); get_parameter("global_frame", global_frame_); From f1b1bfe590076bc6506c86c286c797935b297117 Mon Sep 17 00:00:00 2001 From: Vladyslav Hrynchak Date: Tue, 23 Jul 2024 12:33:34 +0300 Subject: [PATCH 4/7] Refactoring Costmap2DPublisher and Costmap2DROS Signed-off-by: Vladyslav Hrynchak --- .../include/nav2_costmap_2d/costmap_2d_publisher.hpp | 4 ++-- .../include/nav2_costmap_2d/costmap_2d_ros.hpp | 2 +- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 6 +++--- nav2_costmap_2d/src/costmap_2d_ros.cpp | 8 ++++---- 4 files changed, 10 insertions(+), 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 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_); From 3bf69bf5bc81445c0f64c2a1df52618ef69cd935 Mon Sep 17 00:00:00 2001 From: Vladyslav Hrynchak Date: Thu, 25 Jul 2024 14:11:12 +0300 Subject: [PATCH 5/7] Update costmap_2d_publisher.cpp Signed-off-by: Vladyslav Hrynchak --- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index c877a5dfd66..011995aa884 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -146,8 +146,8 @@ void Costmap2DPublisher::prepareGrid() double wx, wy; 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.x = wx - grid_resolution_ / 2; + grid_->info.origin.position.y = wy - grid_resolution_ / 2; grid_->info.origin.position.z = map_vis_z_; grid_->info.origin.orientation.w = 1.0; From 35a6cbdaf3bc997902a8c1a7c7530d22a2b8d14f Mon Sep 17 00:00:00 2001 From: Vladyslav Hrynchak Date: Wed, 31 Jul 2024 10:02:29 +0300 Subject: [PATCH 6/7] Change map_vis_z from float to double Signed-off-by: Vladyslav Hrynchak --- .../include/nav2_costmap_2d/costmap_2d_publisher.hpp | 4 ++-- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 2 +- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- 3 files changed, 4 insertions(+), 4 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 index 2397a9ad2d2..97a9c0a4ff8 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 @@ -73,7 +73,7 @@ class Costmap2DPublisher std::string global_frame, std::string topic_name, bool always_send_full_costmap = false, - float map_vis_z = 0.0); + double map_vis_z = 0.0); /** * @brief Destructor @@ -167,7 +167,7 @@ class Costmap2DPublisher double saved_origin_y_; bool active_; bool always_send_full_costmap_; - float map_vis_z_; + double 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 d7ec01cf8f7..2119d6907a7 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 @@ -404,7 +404,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_z_{0}; + double map_vis_z_{0}; 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 index 011995aa884..bc1b14da786 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -55,7 +55,7 @@ Costmap2DPublisher::Costmap2DPublisher( std::string global_frame, std::string topic_name, bool always_send_full_costmap, - float map_vis_z) + double map_vis_z) : costmap_(costmap), global_frame_(global_frame), topic_name_(topic_name), From 29fafb53e7878af504ead39d64adbf99eafccdd8 Mon Sep 17 00:00:00 2001 From: Vladyslav Hrynchak Date: Wed, 31 Jul 2024 11:02:12 +0300 Subject: [PATCH 7/7] Add comment to map_vis_z_ parameter Signed-off-by: Vladyslav Hrynchak --- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2119d6907a7..f97e1272d90 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 @@ -404,7 +404,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}; + 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