From: Erik Andresen <erik@vontaene.de>
Date: Mon, 27 Apr 2026 18:11:56 +0000 (+0200)
Subject: Add LED stripe
X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=3c52320ab4880b41bf846325e547e35117008572;p=a4wd3.git

Add LED stripe
---

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d3edb0d..28f0b09 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,13 +17,21 @@ find_package(geometry_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Led.msg"
+  "msg/LedStripe.msg"
+  #DEPENDENCIES geometry_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg
+)
 
 add_executable(hw_node src/hw_node.cpp)
 ament_target_dependencies(hw_node rclcpp std_msgs sensor_msgs geometry_msgs std_srvs nav_msgs tf2_geometry_msgs)
 target_include_directories(hw_node PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
-target_link_libraries(hw_node i2c)
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+target_link_libraries(hw_node i2c "${cpp_typesupport_target}")
 target_compile_features(hw_node PUBLIC c_std_99 cxx_std_17)
 
 add_executable(display src/display.cpp)
diff --git a/msg/Led.msg b/msg/Led.msg
new file mode 100644
index 0000000..6861dd1
--- /dev/null
+++ b/msg/Led.msg
@@ -0,0 +1,6 @@
+int32 num
+int32 red
+int32 green
+int32 blue
+int32 ww
+int32 cw
diff --git a/msg/LedStripe.msg b/msg/LedStripe.msg
new file mode 100644
index 0000000..422f680
--- /dev/null
+++ b/msg/LedStripe.msg
@@ -0,0 +1 @@
+Led[] leds
diff --git a/package.xml b/package.xml
index a4f4f33..bb825be 100644
--- a/package.xml
+++ b/package.xml
@@ -8,6 +8,7 @@
   <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
@@ -25,6 +26,9 @@
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>robot_localization</exec_depend>
   <exec_depend>twist_mux</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/src/hw_node.cpp b/src/hw_node.cpp
index 35c5d3c..18d1ee6 100644
--- a/src/hw_node.cpp
+++ b/src/hw_node.cpp
@@ -22,6 +22,7 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2_ros/transform_broadcaster.h"
 #include "geometry_msgs/msg/transform_stamped.hpp"
+#include "a4wd3/msg/led_stripe.hpp"
 
 extern "C" {
 #include <i2c/smbus.h>
@@ -101,6 +102,7 @@ class A4wd3 : public rclcpp::Node {
 			pub_light = this->create_publisher<sensor_msgs::msg::Illuminance>("light", 10);
 			sub_cmd_vel = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 10, std::bind(&A4wd3::cmdvel_callback, this, std::placeholders::_1));
 			sub_imu = this->create_subscription<sensor_msgs::msg::Imu>("imu", 10, std::bind(&A4wd3::imu_callback, this, std::placeholders::_1));
+			sub_led = this->create_subscription<a4wd3::msg::LedStripe>("led_stripe", 10, std::bind(&A4wd3::led_stripe_callback, this, std::placeholders::_1));
 			tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
 			this->declare_parameter("enable_odom_tf", true);
 			this->declare_parameter("odom_covar_xy", 0.01);
@@ -122,6 +124,7 @@ class A4wd3 : public rclcpp::Node {
 		rclcpp::Publisher<sensor_msgs::msg::Illuminance>::SharedPtr pub_light;
 		rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_vel;
 		rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu;
+		rclcpp::Subscription<a4wd3::msg::LedStripe>::SharedPtr sub_led;
 		std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
 		std::vector<double> cmd_vel;
 		double pos_x=0, pos_y=0, angle=0;
@@ -152,7 +155,31 @@ class A4wd3 : public rclcpp::Node {
 		}
 
 		void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
-			imu_received=true;
+			if (!msg->header.frame_id.empty()) {
+				imu_received=true;
+			}
+		}
+
+		// ros2 topic pub -1 /led_stripe a4wd3/msg/LedStripe "{leds: [{num: 1, ww: 1}]}"
+		void led_stripe_callback(const a4wd3::msg::LedStripe::SharedPtr msg) {
+			for (size_t i=0; i<msg->leds.size(); i++) {
+				if (msg->leds[i].num < 1 || msg->leds[i].num > 10) {
+					RCLCPP_ERROR(this->get_logger(), "Led %d out of range 1..10", msg->leds[i].num);
+					continue;
+				}
+				std::vector<uint8_t> buf;
+				buf.push_back(msg->leds[i].red);
+				buf.push_back(msg->leds[i].green);
+				buf.push_back(msg->leds[i].blue);
+				buf.push_back(msg->leds[i].ww);
+				buf.push_back(msg->leds[i].cw);
+				buf.push_back(i == msg->leds.size()-1 ? 0x1 : 0x2); // commit
+				int ret = i2c_write_reg(0x62, msg->leds[i].num, buf);
+				if (ret != 0) {
+					RCLCPP_ERROR(this->get_logger(), "Failed to write to LED Stripe err=%d", ret);
+					break;
+				}
+			}
 		}
 
 		void update_odom() {