From 305373ed09aba37435b9f72149d9aeb383bc9305 Mon Sep 17 00:00:00 2001 From: X-lanni Date: Thu, 6 Nov 2025 18:24:31 +0800 Subject: [PATCH] feat(agv_pro_msgs): add SetDigitalOutput and GetDigitalInput srv definitions --- agv_pro_autocharge/package.xml | 2 +- agv_pro_base/CMakeLists.txt | 2 + .../include/agv_pro_base/agv_pro_driver.h | 46 +++++++ agv_pro_base/package.xml | 3 +- agv_pro_base/src/agv_pro_ros.cpp | 112 ++++++++++++++++-- agv_pro_bringup/package.xml | 2 +- agv_pro_description/package.xml | 2 +- agv_pro_msgs/CMakeLists.txt | 35 ++++++ agv_pro_msgs/msg/AGVProStatus.msg | 6 + agv_pro_msgs/package.xml | 23 ++++ agv_pro_msgs/srv/GetDigitalInput.srv | 5 + agv_pro_msgs/srv/SetDigitalOutput.srv | 5 + agv_pro_navigation2/package.xml | 2 +- 13 files changed, 228 insertions(+), 17 deletions(-) create mode 100644 agv_pro_msgs/CMakeLists.txt create mode 100644 agv_pro_msgs/msg/AGVProStatus.msg create mode 100644 agv_pro_msgs/package.xml create mode 100644 agv_pro_msgs/srv/GetDigitalInput.srv create mode 100644 agv_pro_msgs/srv/SetDigitalOutput.srv diff --git a/agv_pro_autocharge/package.xml b/agv_pro_autocharge/package.xml index cc05bd0..bdcb24e 100755 --- a/agv_pro_autocharge/package.xml +++ b/agv_pro_autocharge/package.xml @@ -2,7 +2,7 @@ agv_pro_autocharge - 1.0.0 + 1.0.8 AGV automatic charging system for ROS2 Humble Your Name MIT diff --git a/agv_pro_base/CMakeLists.txt b/agv_pro_base/CMakeLists.txt index 16661fd..f57b196 100644 --- a/agv_pro_base/CMakeLists.txt +++ b/agv_pro_base/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(agv_pro_msgs REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) @@ -42,6 +43,7 @@ ament_target_dependencies(agv_pro_node geometry_msgs serial_driver nav_msgs + agv_pro_msgs ) install(TARGETS agv_pro_node 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 deb81d0..d4a36c4 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 @@ -13,11 +13,19 @@ #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) #define RECEIVE_PAYLOAD_SIZE (RECEIVE_FRAME_SIZE - 3) // Payload length (excluding header) +#define POWER_ON 0x10 +#define GET_POWER_STATE 0x12 +#define SET_AUTO_REPORT_STATE 0x23 +#define SET_OUTPUT_IO 0x40 +#define GET_INPUT_IO 0x41 + extern std::array odom_pose_covariance; extern std::array odom_twist_covariance; @@ -152,6 +160,42 @@ private: */ uint16_t crc16_ibm(const uint8_t* data, size_t length); + /** + * @brief Handle the SetDigitalOutput service request. + * + * This service sets the state (HIGH/LOW) of a specific digital output pin on the AGV device. + * The request contains the pin number and desired state, which are sent to the hardware + * via the serial interface. The response reports whether the operation succeeded. + * + * @param[in] request The service request, containing: + * - pin: The digital output pin number. + * - state: Desired output state (true = HIGH, false = LOW). + * @param[out] response The service response, containing: + * - success: True if the operation succeeded. + * - message: Optional status or error description. + */ + void handleSetDigitalOutput( + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Handle the GetDigitalInput service request. + * + * This service reads the state (HIGH/LOW) of a specific digital input pin on the AGV device. + * The request specifies the pin number, and the node queries the hardware via the serial + * interface to retrieve its current state. + * + * @param[in] request The service request, containing: + * - pin: The digital input pin number to read. + * @param[out] response The service response, containing: + * - state: Current pin state (true = HIGH, false = LOW). + * - success: True if the read operation succeeded. + * - message: Optional status or error description. + */ + void handleGetDigitalInput( + const std::shared_ptr request, + std::shared_ptr response); + boost::asio::io_service io_; std::unique_ptr serial_port_; @@ -216,6 +260,8 @@ private: rclcpp::Publisher::SharedPtr pub_imu; rclcpp::Publisher::SharedPtr pub_voltage; rclcpp::Subscription::SharedPtr cmd_sub; + rclcpp::Service::SharedPtr set_output_service; + rclcpp::Service::SharedPtr get_input_service; sensor_msgs::msg::Imu imu_data; std::unique_ptr odomBroadcaster; diff --git a/agv_pro_base/package.xml b/agv_pro_base/package.xml index 9a92f48..cba969d 100644 --- a/agv_pro_base/package.xml +++ b/agv_pro_base/package.xml @@ -2,7 +2,7 @@ agv_pro_base - 1.0.3 + 1.0.8 Control Nodes for AGV Pro lanni BSD-3-Clause license @@ -16,6 +16,7 @@ asio_cmake_module io_context tf2_geometry_msgs + agv_pro_msgs ament_lint_auto ament_lint_common diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp index 2dfc219..3b8b81e 100644 --- a/agv_pro_base/src/agv_pro_ros.cpp +++ b/agv_pro_base/src/agv_pro_ros.cpp @@ -117,7 +117,7 @@ std::vector AGV_PRO::read_serial_response( } bool AGV_PRO::is_power_on(){ - auto power_query_frame = build_serial_frame(0x12, {}); + auto power_query_frame = build_serial_frame(GET_POWER_STATE, {}); send_serial_frame(power_query_frame,true); const std::vector expected_header = {0xFE, 0xFE, 0x0B, 0x12}; @@ -138,7 +138,7 @@ bool AGV_PRO::is_power_on(){ RCLCPP_INFO(this->get_logger(), "is_poweron_status: %d", is_poweron_status); if (is_poweron_status == 0){ - auto status_query_frame = build_serial_frame(0x10, {}); + auto status_query_frame = build_serial_frame(POWER_ON, {}); send_serial_frame(status_query_frame,true); rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command @@ -256,6 +256,72 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) } } +void AGV_PRO::handleSetDigitalOutput( + const std::shared_ptr request, + std::shared_ptr response) +{ + uint8_t output_number = request->pin; + uint8_t output_state = request->state; + + if (output_number < 1 || output_number > 6){ + RCLCPP_ERROR(this->get_logger(), "Invalid output pin number: %u", output_number); + response->success = false; + response->message = "Invalid output pin number"; + return; + } + + auto frame = build_serial_frame(SET_OUTPUT_IO, {output_number, output_state}); + send_serial_frame(frame, true); + + const std::vector expected_header = {0xFE, 0xFE, 0x0B, SET_OUTPUT_IO}; + 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_INFO(this->get_logger(), "SetDigitalOutput succeeded"); + response->success = true; + response->message = "Success"; + } else { + RCLCPP_ERROR(this->get_logger(), "SetDigitalOutput failed with status: 0x%02X", status); + response->success = false; + response->message = "Failed with status code"; + } +} + +void AGV_PRO::handleGetDigitalInput( + const std::shared_ptr request, + std::shared_ptr response) +{ + uint8_t input_number = request->pin; + + if (input_number < 1 || input_number > 6){ + RCLCPP_ERROR(this->get_logger(), "Invalid input pin number: %u", input_number); + response->success = false; + response->message = "Invalid input pin number"; + return; + } + + auto frame = build_serial_frame(GET_INPUT_IO, {input_number}); + send_serial_frame(frame, true); + + const std::vector expected_header = {0xFE, 0xFE, 0x0B, GET_INPUT_IO}; + auto response_frame = read_serial_response(expected_header, 8, 5.0); + // print_hex("recv_buf", response_frame); //debug + + uint8_t status = response_frame[5]; + if (status == 0xff) { + 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); + response->state = static_cast(status); + response->success = true; + response->message = "Success"; + } +} + bool AGV_PRO::readData() { std::vector buf_length(1); @@ -309,7 +375,7 @@ bool AGV_PRO::readData() recv_buf.push_back(0x1C); recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end()); - //print_hex("recv_buf", recv_buf); //debug + // print_hex("recv_buf", recv_buf); //debug if (recv_buf[3] != 0x25) { //RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); //debug @@ -333,17 +399,29 @@ bool AGV_PRO::readData() battery_voltage = static_cast(recv_buf[9]) / 10.0f; enable_status = recv_buf[10]; - imu_data.linear_acceleration.x = static_cast((static_cast(recv_buf[11]) << 8) | recv_buf[12]) * 0.01; - imu_data.linear_acceleration.y = static_cast((static_cast(recv_buf[13]) << 8) | recv_buf[14]) * 0.01; - imu_data.linear_acceleration.z = static_cast((static_cast(recv_buf[15]) << 8) | recv_buf[16]) * 0.01; + imu_data.linear_acceleration.x = static_cast(static_cast((recv_buf[11] << 8) | recv_buf[12])) * 0.01; + imu_data.linear_acceleration.y = static_cast(static_cast((recv_buf[13] << 8) | recv_buf[14])) * 0.01; + imu_data.linear_acceleration.z = static_cast(static_cast((recv_buf[15] << 8) | recv_buf[16])) * 0.01; - imu_data.angular_velocity.x = static_cast((static_cast(recv_buf[17]) << 8) | recv_buf[18]) * 0.01; - imu_data.angular_velocity.y = static_cast((static_cast(recv_buf[19]) << 8) | recv_buf[20]) * 0.01; - imu_data.angular_velocity.z = static_cast((static_cast(recv_buf[21]) << 8) | recv_buf[22]) * 0.01; + imu_data.angular_velocity.x = static_cast(static_cast((recv_buf[17] << 8) | recv_buf[18])) * 0.01; + imu_data.angular_velocity.y = static_cast(static_cast((recv_buf[19] << 8) | recv_buf[20])) * 0.01; + imu_data.angular_velocity.z = static_cast(static_cast((recv_buf[21] << 8) | recv_buf[22])) * 0.01; - roll = static_cast((static_cast(recv_buf[23]) << 8) | recv_buf[24]) * 0.01; - pitch = static_cast((static_cast(recv_buf[25]) << 8) | recv_buf[26]) * 0.01; - yaw = static_cast((static_cast(recv_buf[27]) << 8) | recv_buf[28]) * 0.01; + roll = static_cast(static_cast((recv_buf[23] << 8) | recv_buf[24])) * 0.01; + pitch = static_cast(static_cast((recv_buf[25] << 8) | recv_buf[26])) * 0.01; + yaw = static_cast(static_cast((recv_buf[27] << 8) | recv_buf[28])) * 0.01; + + // RCLCPP_INFO(this->get_logger(), + // "IMU Data - Accel[x: %.2f, y: %.2f, z: %.2f], " + // "Gyro[x: %.2f, y: %.2f, z: %.2f], " + // "RPY[roll: %.2f, pitch: %.2f, yaw: %.2f]", + // imu_data.linear_acceleration.x, + // imu_data.linear_acceleration.y, + // imu_data.linear_acceleration.z, + // imu_data.angular_velocity.x, + // imu_data.angular_velocity.y, + // imu_data.angular_velocity.z, + // roll, pitch, yaw); return true; } @@ -481,6 +559,16 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name) cmd_sub = this->create_subscription( "/cmd_vel", 10, std::bind(&AGV_PRO::cmdCallback, this, std::placeholders::_1)); + set_output_service = this->create_service( + "set_digital_output", + std::bind(&AGV_PRO::handleSetDigitalOutput, this, std::placeholders::_1, std::placeholders::_2) + ); + + get_input_service = this->create_service( + "get_digital_input", + std::bind(&AGV_PRO::handleGetDigitalInput, this, std::placeholders::_1, std::placeholders::_2) + ); + lastTime = this->get_clock()->now(); try{ diff --git a/agv_pro_bringup/package.xml b/agv_pro_bringup/package.xml index 2c642f8..1e83176 100644 --- a/agv_pro_bringup/package.xml +++ b/agv_pro_bringup/package.xml @@ -2,7 +2,7 @@ agv_pro_bringup - 1.0.0 + 1.0.8 ROS 2 launch scripts for starting the AGV Pro lanni BSD-3-Clause license diff --git a/agv_pro_description/package.xml b/agv_pro_description/package.xml index bc8dd36..65c8fd0 100755 --- a/agv_pro_description/package.xml +++ b/agv_pro_description/package.xml @@ -2,7 +2,7 @@ agv_pro_description - 1.0.1 + 1.0.8

URDF Description package for AGV pro

 diff --git a/agv_pro_msgs/CMakeLists.txt b/agv_pro_msgs/CMakeLists.txt new file mode 100644 index 0000000..6414126 --- /dev/null +++ b/agv_pro_msgs/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(agv_pro_msgs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/AGVProStatus.msg" + "srv/SetDigitalOutput.srv" + "srv/GetDigitalInput.srv" + DEPENDENCIES std_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/agv_pro_msgs/msg/AGVProStatus.msg b/agv_pro_msgs/msg/AGVProStatus.msg new file mode 100644 index 0000000..530ba70 --- /dev/null +++ b/agv_pro_msgs/msg/AGVProStatus.msg @@ -0,0 +1,6 @@ +std_msgs/Header header + +uint8 motor_status +uint8 motor_error +float64 battery_voltage +uint8 enable_status diff --git a/agv_pro_msgs/package.xml b/agv_pro_msgs/package.xml new file mode 100644 index 0000000..285fff9 --- /dev/null +++ b/agv_pro_msgs/package.xml @@ -0,0 +1,23 @@ + + + + agv_pro_msgs + 1.0.8 + TODO: Package description + lanni + TODO: License declaration + + ament_cmake + + std_msgs + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/agv_pro_msgs/srv/GetDigitalInput.srv b/agv_pro_msgs/srv/GetDigitalInput.srv new file mode 100644 index 0000000..7912e18 --- /dev/null +++ b/agv_pro_msgs/srv/GetDigitalInput.srv @@ -0,0 +1,5 @@ +int32 pin +--- +bool success +int32 state +string message diff --git a/agv_pro_msgs/srv/SetDigitalOutput.srv b/agv_pro_msgs/srv/SetDigitalOutput.srv new file mode 100644 index 0000000..30da966 --- /dev/null +++ b/agv_pro_msgs/srv/SetDigitalOutput.srv @@ -0,0 +1,5 @@ +int32 pin +int32 state +--- +bool success +string message diff --git a/agv_pro_navigation2/package.xml b/agv_pro_navigation2/package.xml index 089919d..19f0be0 100644 --- a/agv_pro_navigation2/package.xml +++ b/agv_pro_navigation2/package.xml @@ -2,7 +2,7 @@ agv_pro_navigation2 - 1.0.1 + 1.0.8 ROS2 launch scripts for navigation2 lanni BSD-3-Clause license