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]>
grupo-avispa · Apr 19, 2024 · 2fed904 · 2fed904
1 parent 08ad6d6
commit 2fed904
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
@@ -246,6 +246,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
@@ -71,6 +71,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));
@@ -432,6 +435,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_,