From af1b993e645851f94d049591acecab65f1593275 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Wed, 11 Oct 2023 10:06:18 +0300
Subject: [PATCH] led_effect: add led parameter to specify led namespace When
 using ROS namespace, subscription to mavros topics is broken

---
 clover/launch/led.launch |  3 ++-
 clover/src/led.cpp       | 14 +++++++++-----
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/clover/launch/led.launch b/clover/launch/led.launch
index 3e923897..c2923ce9 100644
--- a/clover/launch/led.launch
+++ b/clover/launch/led.launch
@@ -21,7 +21,8 @@
     </node>
 
     <!-- high level led effects control, events notification with leds -->
-    <node pkg="clover" name="led_effect" type="led" ns="led" clear_params="true" output="screen" if="$(arg led_effect)">
+    <node pkg="clover" name="led_effect" type="led" clear_params="true" output="screen" if="$(arg led_effect)">
+        <param name="led" value="led"/>
         <param name="blink_rate" value="2"/>
         <param name="fade_period" value="0.5"/>
         <param name="rainbow_period" value="5"/>
diff --git a/clover/src/led.cpp b/clover/src/led.cpp
index 91438df3..b9927900 100644
--- a/clover/src/led.cpp
+++ b/clover/src/led.cpp
@@ -309,15 +309,19 @@ int main(int argc, char **argv)
 	nh_priv.param("notify/low_battery/threshold", low_battery_threshold, 3.7);
 	nh_priv.param("notify/error/ignore", error_ignore, {});
 
-	ros::service::waitForService("set_leds"); // cannot work without set_leds service
-	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>("set_leds", true);
+	std::string led; // led namespace
+	nh_priv.param("led", led, std::string("led"));
+	if (!led.empty()) led += "/";
+
+	ros::service::waitForService(led + "set_leds"); // cannot work without set_leds service
+	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>(led + "set_leds", true);
 
 	// wait for leds count info
-	handleState(*ros::topic::waitForMessage<led_msgs::LEDStateArray>("state", nh));
+	handleState(*ros::topic::waitForMessage<led_msgs::LEDStateArray>(led + "state", nh));
 
-	auto state_sub = nh.subscribe("state", 1, &handleState);
+	auto state_sub = nh.subscribe(led + "state", 1, &handleState);
 
-	auto set_effect = nh.advertiseService("set_effect", &setEffect);
+	auto set_effect = nh.advertiseService(led + "set_effect", &setEffect);
 
 	auto mavros_state_sub = nh.subscribe("mavros/state", 1, &handleMavrosState);
 	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleBattery);