Add LED stripe

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d3edb0d297d776bd643ce9733943dc269b10fba0..28f0b0953b8dd5ddd70866a0c37738062ad6448d 100644 (file)
--- 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 (file)
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 (file)
index 0000000..422f680
--- /dev/null
+++ b/msg/LedStripe.msg
@@ -0,0 +1 @@
+Led[] leds

diff --git a/package.xml b/package.xml
index a4f4f335ca2c63a8257b838d623d5dd2a12e34e0..bb825be3fdb8c762843bb1115503e074f96234e6 100644 (file)
--- 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 35c5d3cac0d72034c5a9274af72e1cf45aef7a0e..18d1ee6115a2345dc3a5e4bf277b446532bbb5ea 100644 (file)
--- 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() {