led_effect: add led parameter to specify led namespace

When using ROS namespace, subscription to mavros topics is broken
CopterExpress · Oct 11, 2023 · af1b993 · af1b993
1 parent d3bda9d
commit af1b993
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 6 deletions.
diff --git 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
@@ -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);