From aecf4102a2c2464c051e4d98edcd47f2c85b7cdf Mon Sep 17 00:00:00 2001
From: asarazin <anaelle.sarazin@robocc.com>
Date: Fri, 22 Mar 2024 18:10:58 +0100
Subject: [PATCH] add polygon_subscribe_transient_local parameter in collision
 monitor

Signed-off-by: asarazin <anaelle.sarazin@robocc.com>
---
 .../include/nav2_collision_monitor/polygon.hpp             | 2 ++
 nav2_collision_monitor/src/polygon.cpp                     | 7 +++++++
 2 files changed, 9 insertions(+)

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
index 5e342cdb7f1..1e6a72d1590 100644
--- 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
index 77fc1f304c1..6a0801d1d26 100644
--- 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_,