add polygon_subscribe_transient_local parameter in collision monitor (r…

…os-navigation#4207) Signed-off-by: asarazin <[email protected]> Co-authored-by: asarazin <[email protected]>
Manos-G · Aug 1, 2024 · 40fdd3a · 40fdd3a
1 parent 7a692ba
commit 40fdd3a
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 0 deletions.
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
@@ -239,6 +239,8 @@ class Polygon
   double simulation_time_step_;
   /// @brief Whether polygon is enabled
   bool enabled_;
+  /// @brief Wether the subscription to polygon topic has transient local QoS durability
+  bool polygon_subscribe_transient_local_;
   /// @brief Polygon subscription
   rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_sub_;
   /// @brief Footprint subscriber

diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp
@@ -72,6 +72,9 @@ bool Polygon::configure()
       "[%s]: Subscribing on %s topic for polygon",
       polygon_name_.c_str(), polygon_sub_topic.c_str());
     rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS();  // set to default
+    if (polygon_subscribe_transient_local_) {
+      polygon_qos.transient_local();
+    }
     polygon_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
       polygon_sub_topic, polygon_qos,
       std::bind(&Polygon::polygonCallback, this, std::placeholders::_1));
@@ -433,6 +436,10 @@ bool Polygon::getParameters(
       footprint_topic =
         node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
     }
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".polygon_subscribe_transient_local", rclcpp::ParameterValue(false));
+    polygon_subscribe_transient_local_ =
+      node->get_parameter(polygon_name_ + ".polygon_subscribe_transient_local").as_bool();
   } catch (const std::exception & ex) {
     RCLCPP_ERROR(
       logger_,