Fixed broken LED (#322)

* Fixed broken LED * Fixed test * Fixed test * Use Int32 instead of UInt32 to be closer to the Webots API, fixed test
cyberbotics · Nov 3, 2021 · 413dd68 · 413dd68
1 parent faee325
commit 413dd68
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 11 deletions.
diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LED.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LED.hpp
@@ -16,7 +16,7 @@
 #define ROS2_LED_HPP
 
 #include <unordered_map>
-#include <std_msgs/msg/color_rgba.hpp>
+#include <std_msgs/msg/int32.hpp>
 #include <webots/LED.hpp>
 #include <webots_ros2_driver/utils/Utils.hpp>
 #include <webots_ros2_driver/WebotsNode.hpp>
@@ -32,11 +32,11 @@ namespace webots_ros2_driver
     void step() override;
 
   private:
-    void onMessageReceived(const std_msgs::msg::ColorRGBA::SharedPtr message);
+    void onMessageReceived(const std_msgs::msg::Int32::SharedPtr message);
 
     webots::LED *mLED;
     webots_ros2_driver::WebotsNode *mNode;
-    rclcpp::Subscription<std_msgs::msg::ColorRGBA>::SharedPtr mSubscriber;
+    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr mSubscriber;
   };
 
 }

diff --git a/webots_ros2_driver/src/plugins/static/Ros2LED.cpp b/webots_ros2_driver/src/plugins/static/Ros2LED.cpp
@@ -26,18 +26,15 @@ namespace webots_ros2_driver
     assert(mLED != NULL);
 
     const std::string topicName = parameters.count("topicName") ? parameters["topicName"] : "~/" + getFixedNameString(parameters["name"]);
-    mSubscriber = mNode->create_subscription<std_msgs::msg::ColorRGBA>(topicName, rclcpp::SensorDataQoS().reliable(), std::bind(&Ros2LED::onMessageReceived, this, std::placeholders::_1));
+    mSubscriber = mNode->create_subscription<std_msgs::msg::Int32>(topicName, rclcpp::SensorDataQoS().reliable(), std::bind(&Ros2LED::onMessageReceived, this, std::placeholders::_1));
   }
 
   void Ros2LED::step()
   {
   }
 
-  void Ros2LED::onMessageReceived(const std_msgs::msg::ColorRGBA::SharedPtr message)
+  void Ros2LED::onMessageReceived(const std_msgs::msg::Int32::SharedPtr message)
   {
-    const int red = message->r * 255;
-    const int green = message->g * 255;
-    const int blue = message->b * 255;
-    mLED->set((red << 16) | (green << 8) | blue);
+    mLED->set(message->data);
   }
 }
diff --git a/webots_ros2_tests/test/test_system_driver.py b/webots_ros2_tests/test/test_system_driver.py
@@ -25,7 +25,7 @@
 import rclpy
 from std_srvs.srv import Trigger
 from sensor_msgs.msg import Range, Image, Imu, Illuminance
-from std_msgs.msg import ColorRGBA, Float32
+from std_msgs.msg import Int32, Float32
 from geometry_msgs.msg import PointStamped
 from launch import LaunchDescription
 from launch_ros.actions import Node
@@ -119,7 +119,7 @@ def testPythonPluginService(self):
         self.assertEqual(response.success, True)
 
     def testLED(self):
-        publisher = self.__node.create_publisher(ColorRGBA, '/Pioneer_3_AT/led', 1)
+        publisher = self.__node.create_publisher(Int32, '/Pioneer_3_AT/led', 1)
         check_start_time = time.time()
         while publisher.get_subscription_count() == 0:
             rclpy.spin_once(self.__node, timeout_sec=0.1)