Merge pull request #10 from elephantrobotics/humble-dev

feat(agv_pro_base,agv_pro_msgs): add LED color and mode services with…

agv_pro_base/include/agv_pro_base/agv_pro_driver.h

@@ -15,6 +15,8 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <agv_pro_msgs/srv/set_digital_output.hpp>
 #include <agv_pro_msgs/srv/get_digital_input.hpp>
+#include <agv_pro_msgs/srv/set_led_color.hpp>
+#include <agv_pro_msgs/srv/set_led_mode.hpp>
 
 #define SEND_DATA_SIZE 14                               // Total bytes in a command frame to ESP32(version>=V1.0.8)
 #define RECEIVE_FRAME_SIZE 31                           // Total bytes in a frame from ESP32(version>=V1.0.8)
@@ -23,6 +25,8 @@
 #define POWER_ON 0x10
 #define GET_POWER_STATE 0x12
 #define SET_AUTO_REPORT_STATE 0x23
+#define SET_LED_COLOR 0x34
+#define SET_LED_MODE 0x3A
 #define SET_OUTPUT_IO 0x40
 #define GET_INPUT_IO 0x41
 
@@ -196,6 +200,14 @@ private:
     const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
     std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response);
 
+  void handleSetLedColor(
+    const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response);
+
+  void handleSetLedMode(
+    const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response);
+
   boost::asio::io_service io_;
   std::unique_ptr<boost::asio::serial_port> serial_port_;
 
@@ -262,6 +274,8 @@ private:
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub;
   rclcpp::Service<agv_pro_msgs::srv::SetDigitalOutput>::SharedPtr set_output_service;
   rclcpp::Service<agv_pro_msgs::srv::GetDigitalInput>::SharedPtr get_input_service;
+  rclcpp::Service<agv_pro_msgs::srv::SetLedColor>::SharedPtr set_led_service;
+  rclcpp::Service<agv_pro_msgs::srv::SetLedMode>::SharedPtr set_led_mode_service;
 
   sensor_msgs::msg::Imu imu_data;
   std::unique_ptr<tf2_ros::TransformBroadcaster> odomBroadcaster;

agv_pro_base/src/agv_pro_ros.cpp

@@ -280,7 +280,7 @@ void AGV_PRO::handleSetDigitalOutput(
 
   uint8_t status = response_frame[4];
   if (status == 0x01) {
-    RCLCPP_INFO(this->get_logger(), "SetDigitalOutput succeeded");
+    RCLCPP_DEBUG(this->get_logger(), "SetDigitalOutput succeeded");
     response->success = true;
     response->message = "Success";
   } else {
@@ -315,13 +315,96 @@ void AGV_PRO::handleGetDigitalInput(
     RCLCPP_ERROR(this->get_logger(), "GetDigitalInput failed with status: 0x%02X", status);
     response->success = false;
   } else {
-    RCLCPP_INFO(this->get_logger(), "GetDigitalInput succeeded, state: %u", status);
+    RCLCPP_DEBUG(this->get_logger(), "GetDigitalInput succeeded, state: %u", status);
     response->state = static_cast<int32_t>(status);
     response->success = true;
     response->message = "Success";
   }
 }
 
+void AGV_PRO::handleSetLedColor(
+  const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response)
+{
+  if (request->position < 0 || request->position > 1) {
+    RCLCPP_ERROR(this->get_logger(), "Invalid LED position: %d", request->position);
+    response->success = false;
+    response->message = "Invalid LED position";
+    return;
+  }
+
+  if (request->brightness < 0 || request->brightness > 255) {
+    RCLCPP_ERROR(this->get_logger(), "Invalid brightness: %d", request->brightness);
+    response->success = false;
+    response->message = "Invalid brightness";
+    return;
+  }
+
+  if (request->r < 0 || request->r > 255 ||
+      request->g < 0 || request->g > 255 ||
+      request->b < 0 || request->b > 255) {
+    RCLCPP_ERROR(
+      this->get_logger(),
+      "Invalid RGB value: r=%d g=%d b=%d",
+      request->r, request->g, request->b
+    );
+    response->success = false;
+    response->message = "Invalid RGB value";
+    return;
+  }
+
+  uint8_t position   = static_cast<uint8_t>(request->position);
+  uint8_t brightness = static_cast<uint8_t>(request->brightness);
+  uint8_t r          = static_cast<uint8_t>(request->r);
+  uint8_t g          = static_cast<uint8_t>(request->g);
+  uint8_t b          = static_cast<uint8_t>(request->b);
+
+  auto frame = build_serial_frame(SET_LED_COLOR, {position, brightness, r, g, b});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_LED_COLOR};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[4];
+  if (status == 0x01) {
+    RCLCPP_DEBUG(this->get_logger(), "SetLedColor succeeded");
+    response->success = true;
+    response->message = "Success";
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "SetLedColor failed with status: 0x%02X", status);
+    response->success = false;
+    response->message = "Failed with status code";
+  }
+}
+
+void AGV_PRO::handleSetLedMode(
+  const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response)
+{
+  uint8_t mode = request->mode ? 0x01 : 0x00;
+
+  auto frame = build_serial_frame(SET_LED_MODE, {mode});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_LED_MODE};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[4];
+  if (status == 0x01) {
+    RCLCPP_DEBUG(this->get_logger(), "SetLedMode succeeded");
+    response->success = true;
+    response->message = "Success";
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "SetLedMode failed with status: 0x%02X", status);
+    response->success = false;
+    response->message = "Failed with status code";
+  }
+}
+
 bool AGV_PRO::readData()
 {
   std::vector<uint8_t> buf_length(1);
@@ -569,6 +652,16 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
     std::bind(&AGV_PRO::handleGetDigitalInput, this, std::placeholders::_1, std::placeholders::_2)
   );
 
+  set_led_service = this->create_service<agv_pro_msgs::srv::SetLedColor>(
+    "set_led_color",
+    std::bind(&AGV_PRO::handleSetLedColor, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
+  set_led_mode_service = this->create_service<agv_pro_msgs::srv::SetLedMode>(
+    "set_led_mode",
+    std::bind(&AGV_PRO::handleSetLedMode, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
   lastTime = this->get_clock()->now();
       
   try{

agv_pro_msgs/CMakeLists.txt

@@ -17,6 +17,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/AGVProStatus.msg"
   "srv/SetDigitalOutput.srv"
   "srv/GetDigitalInput.srv"
+  "srv/SetLedColor.srv"
+  "srv/SetLedMode.srv"
   DEPENDENCIES std_msgs
 )
 

agv_pro_msgs/scripts/ros_client.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python3
+import rclpy
+import time
+from rclpy.node import Node
+
+from agv_pro_msgs.srv import (
+    SetDigitalOutput,
+    GetDigitalInput,
+    SetLedColor,
+    SetLedMode
+)
+
+class AGVIOClient(Node):
+    def __init__(self):
+        super().__init__('agv_io_client')
+
+        # Create service client
+        self.cli_set_io = self.create_client(SetDigitalOutput, 'set_digital_output')
+        self.cli_get_io = self.create_client(GetDigitalInput, 'get_digital_input')
+        self.cli_led_output = self.create_client(SetLedColor, 'set_led_color')
+        self.cli_led_mode = self.create_client(SetLedMode, 'set_led_mode')
+
+        # Wait until all services are available
+        self._wait_for_services()
+
+    def _wait_for_services(self):
+        """Wait for all services to become available."""
+        clients = [
+            self.cli_set_io,
+            self.cli_get_io,
+            self.cli_led_output,
+            self.cli_led_mode
+        ]
+
+        for cli in clients:
+            while not cli.wait_for_service(timeout_sec=1.0):
+                pass
+
+    def _call_service(self, client, request):
+        future = client.call_async(request)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
+
+        if future.result() is not None:
+            return future.result()
+        else:
+            self.get_logger().error(f'Service call failed: {future.exception()}')
+            return None
+
+    # ------------------------------
+    # Set digital output
+    # ------------------------------
+    def set_digital_output(self, pin: int, state: int) -> bool:
+        """Set digital output pin state."""
+        req = SetDigitalOutput.Request()
+        req.pin = pin
+        req.state = state
+
+        res = self._call_service(self.cli_set_io, req)
+        return res.success if res else False
+
+    # ------------------------------
+    # Get digital input
+    # ------------------------------
+    def get_digital_input(self, pin: int):
+        """Read digital input pin state."""
+        req = GetDigitalInput.Request()
+        req.pin = pin
+
+        res = self._call_service(self.cli_get_io, req)
+        return res.state if (res and res.success) else None
+
+    # ------------------------------
+    # Set LED color
+    # ------------------------------
+    def set_led_color(self,
+                      position: int,
+                      brightness: int,
+                      r: int,
+                      g: int,
+                      b: int) -> bool:
+        """Set LED RGB color and brightness."""
+        req = SetLedColor.Request()
+        req.position = position
+        req.brightness = brightness
+        req.r = r
+        req.g = g
+        req.b = b
+
+        res = self._call_service(self.cli_led_output, req)
+        return res.success if res else False
+
+    # ------------------------------
+    # Set LED mode
+    # ------------------------------
+    def set_led_mode(self, mode: bool) -> bool:
+        """Set LED mode (True/False)."""
+        req = SetLedMode.Request()
+        req.mode = mode
+
+        res = self._call_service(self.cli_led_mode, req)
+        return res.success if res else False
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    client = AGVIOClient()
+
+    ################
+    # Example usage:
+    ################
+
+    # client.set_digital_output(pin=1, state=1)
+    # client.get_digital_input(pin=2)
+
+    # client.set_led_mode(True)
+    # for i in range(10):
+    #     client.set_led_color(0, 100, 255, 255, 0)
+    #     client.set_led_color(1, 100, 255, 255, 0)
+    #     time.sleep(0.5)
+    #     client.set_led_color(0, 0, 0, 0, 0)
+    #     client.set_led_color(1, 0, 0, 0, 0)
+    #     time.sleep(0.5)
+
+    # client.set_led_color(0, 100, 255, 255, 0)
+    # client.set_led_color(1, 100, 255, 255, 0)
+    # client.set_led_mode(True)
+
+    client.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_msgs/srv/SetLedColor.srv

@@ -0,0 +1,8 @@
+int32 position
+int32 brightness
+int32 r
+int32 g
+int32 b
+---
+bool success
+string message

agv_pro_msgs/srv/SetLedMode.srv

@@ -0,0 +1,4 @@
+bool mode
+---
+bool success
+string message