diff --git a/agv_pro_base/include/agv_pro_base/agv_pro_driver.h b/agv_pro_base/include/agv_pro_base/agv_pro_driver.h index d4a36c4..2d21cdc 100644 --- a/agv_pro_base/include/agv_pro_base/agv_pro_driver.h +++ b/agv_pro_base/include/agv_pro_base/agv_pro_driver.h @@ -15,6 +15,8 @@ #include #include #include +#include +#include #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 request, std::shared_ptr response); + void handleSetLedColor( + const std::shared_ptr request, + std::shared_ptr response); + + void handleSetLedMode( + const std::shared_ptr request, + std::shared_ptr response); + boost::asio::io_service io_; std::unique_ptr serial_port_; @@ -262,6 +274,8 @@ private: rclcpp::Subscription::SharedPtr cmd_sub; rclcpp::Service::SharedPtr set_output_service; rclcpp::Service::SharedPtr get_input_service; + rclcpp::Service::SharedPtr set_led_service; + rclcpp::Service::SharedPtr set_led_mode_service; sensor_msgs::msg::Imu imu_data; std::unique_ptr odomBroadcaster; diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp index 3b8b81e..ff8489b 100644 --- a/agv_pro_base/src/agv_pro_ros.cpp +++ b/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(status); response->success = true; response->message = "Success"; } } +void AGV_PRO::handleSetLedColor( + const std::shared_ptr request, + std::shared_ptr 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(request->position); + uint8_t brightness = static_cast(request->brightness); + uint8_t r = static_cast(request->r); + uint8_t g = static_cast(request->g); + uint8_t b = static_cast(request->b); + + auto frame = build_serial_frame(SET_LED_COLOR, {position, brightness, r, g, b}); + send_serial_frame(frame, true); + + const std::vector 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 request, + std::shared_ptr 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 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 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( + "set_led_color", + std::bind(&AGV_PRO::handleSetLedColor, this, std::placeholders::_1, std::placeholders::_2) + ); + + set_led_mode_service = this->create_service( + "set_led_mode", + std::bind(&AGV_PRO::handleSetLedMode, this, std::placeholders::_1, std::placeholders::_2) + ); + lastTime = this->get_clock()->now(); try{ diff --git a/agv_pro_msgs/CMakeLists.txt b/agv_pro_msgs/CMakeLists.txt index 6414126..596dbab 100644 --- a/agv_pro_msgs/CMakeLists.txt +++ b/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 ) diff --git a/agv_pro_msgs/scripts/ros_client.py b/agv_pro_msgs/scripts/ros_client.py new file mode 100644 index 0000000..d6db17d --- /dev/null +++ b/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() \ No newline at end of file diff --git a/agv_pro_msgs/srv/SetLedColor.srv b/agv_pro_msgs/srv/SetLedColor.srv new file mode 100644 index 0000000..3ed5477 --- /dev/null +++ b/agv_pro_msgs/srv/SetLedColor.srv @@ -0,0 +1,8 @@ +int32 position +int32 brightness +int32 r +int32 g +int32 b +--- +bool success +string message \ No newline at end of file diff --git a/agv_pro_msgs/srv/SetLedMode.srv b/agv_pro_msgs/srv/SetLedMode.srv new file mode 100644 index 0000000..af23b3b --- /dev/null +++ b/agv_pro_msgs/srv/SetLedMode.srv @@ -0,0 +1,4 @@ +bool mode +--- +bool success +string message \ No newline at end of file