feat(agv_pro_msgs): add SetDigitalOutput and GetDigitalInput srv definitions

agv_pro_autocharge/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>agv_pro_autocharge</name>
-  <version>1.0.0</version>
+  <version>1.0.8</version>
   <description>AGV automatic charging system for ROS2 Humble</description>
   <maintainer email="your-email@example.com">Your Name</maintainer>
   <license>MIT</license>

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(<dependency> 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

agv_pro_base/include/agv_pro_base/agv_pro_driver.h

@@ -13,11 +13,19 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2/LinearMath/Quaternion.h>
 #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>
 
 #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<double, 36> odom_pose_covariance;
 extern std::array<double, 36> 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<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> 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<agv_pro_msgs::srv::GetDigitalInput::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response);
+
   boost::asio::io_service io_;
   std::unique_ptr<boost::asio::serial_port> serial_port_;
 
@@ -216,6 +260,8 @@ private:
   rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu;
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_voltage;
   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;
 
   sensor_msgs::msg::Imu imu_data;
   std::unique_ptr<tf2_ros::TransformBroadcaster> odomBroadcaster;

agv_pro_base/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>agv_pro_base</name>
-  <version>1.0.3</version>
+  <version>1.0.8</version>
   <description>Control Nodes for AGV Pro</description>
   <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
   <license>BSD-3-Clause license</license>
@@ -16,6 +16,7 @@
   <depend>asio_cmake_module</depend>
   <depend>io_context</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>agv_pro_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

agv_pro_base/src/agv_pro_ros.cpp

@@ -117,7 +117,7 @@ std::vector<uint8_t> 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<uint8_t> 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<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> 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<uint8_t> 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<agv_pro_msgs::srv::GetDigitalInput::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> 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<uint8_t> 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<int32_t>(status);
+    response->success = true;
+    response->message = "Success";
+  }
+}
+
 bool AGV_PRO::readData()
 {
   std::vector<uint8_t> buf_length(1);
@@ -333,17 +399,29 @@ bool AGV_PRO::readData()
   battery_voltage = static_cast<float>(recv_buf[9]) / 10.0f;
   enable_status = recv_buf[10];
 
-  imu_data.linear_acceleration.x = static_cast<double>((static_cast<int16_t>(recv_buf[11]) << 8) | recv_buf[12]) * 0.01;
+  imu_data.linear_acceleration.x = static_cast<double>(static_cast<int16_t>((recv_buf[11] << 8) | recv_buf[12])) * 0.01;
-  imu_data.linear_acceleration.y = static_cast<double>((static_cast<int16_t>(recv_buf[13]) << 8) | recv_buf[14]) * 0.01;
+  imu_data.linear_acceleration.y = static_cast<double>(static_cast<int16_t>((recv_buf[13] << 8) | recv_buf[14])) * 0.01;
-  imu_data.linear_acceleration.z = static_cast<double>((static_cast<int16_t>(recv_buf[15]) << 8) | recv_buf[16]) * 0.01;
+  imu_data.linear_acceleration.z = static_cast<double>(static_cast<int16_t>((recv_buf[15] << 8) | recv_buf[16])) * 0.01;
 
-  imu_data.angular_velocity.x = static_cast<double>((static_cast<int16_t>(recv_buf[17]) << 8) | recv_buf[18]) * 0.01;
+  imu_data.angular_velocity.x = static_cast<double>(static_cast<int16_t>((recv_buf[17] << 8) | recv_buf[18])) * 0.01;
-  imu_data.angular_velocity.y = static_cast<double>((static_cast<int16_t>(recv_buf[19]) << 8) | recv_buf[20]) * 0.01;
+  imu_data.angular_velocity.y = static_cast<double>(static_cast<int16_t>((recv_buf[19] << 8) | recv_buf[20])) * 0.01;
-  imu_data.angular_velocity.z = static_cast<double>((static_cast<int16_t>(recv_buf[21]) << 8) | recv_buf[22]) * 0.01;
+  imu_data.angular_velocity.z = static_cast<double>(static_cast<int16_t>((recv_buf[21] << 8) | recv_buf[22])) * 0.01;
 
-  roll  = static_cast<double>((static_cast<int16_t>(recv_buf[23]) << 8) | recv_buf[24]) * 0.01;
+  roll  = static_cast<double>(static_cast<int16_t>((recv_buf[23] << 8) | recv_buf[24])) * 0.01;
-  pitch = static_cast<double>((static_cast<int16_t>(recv_buf[25]) << 8) | recv_buf[26]) * 0.01;
+  pitch = static_cast<double>(static_cast<int16_t>((recv_buf[25] << 8) | recv_buf[26])) * 0.01;
-  yaw   = static_cast<double>((static_cast<int16_t>(recv_buf[27]) << 8) | recv_buf[28]) * 0.01;
+  yaw   = static_cast<double>(static_cast<int16_t>((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<geometry_msgs::msg::Twist>(
     "/cmd_vel", 10, std::bind(&AGV_PRO::cmdCallback, this, std::placeholders::_1));
 
+  set_output_service = this->create_service<agv_pro_msgs::srv::SetDigitalOutput>(
+    "set_digital_output",
+    std::bind(&AGV_PRO::handleSetDigitalOutput, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
+  get_input_service = this->create_service<agv_pro_msgs::srv::GetDigitalInput>(
+    "get_digital_input",
+    std::bind(&AGV_PRO::handleGetDigitalInput, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
   lastTime = this->get_clock()->now();
       
   try{

agv_pro_bringup/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>agv_pro_bringup</name>
-  <version>1.0.0</version>
+  <version>1.0.8</version>
   <description>ROS 2 launch scripts for starting the AGV Pro</description>
   <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
   <license>BSD-3-Clause license</license>

agv_pro_description/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>agv_pro_description</name>
-  <version>1.0.1</version>
+  <version>1.0.8</version>
   <description>
     <p>URDF Description package for AGV pro</p>
   </description>

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(<dependency> 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()

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

agv_pro_msgs/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agv_pro_msgs</name>
+  <version>1.0.8</version>
+  <description>TODO: Package description</description>
+  <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

agv_pro_msgs/srv/GetDigitalInput.srv

@@ -0,0 +1,5 @@
+int32 pin
+---
+bool success
+int32 state
+string message

agv_pro_msgs/srv/SetDigitalOutput.srv

@@ -0,0 +1,5 @@
+int32 pin
+int32 state
+---
+bool success
+string message

agv_pro_navigation2/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>agv_pro_navigation2</name>
-  <version>1.0.1</version>
+  <version>1.0.8</version>
   <description>ROS2 launch scripts for navigation2</description>
   <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
   <license>BSD-3-Clause license</license>