m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
X-lanni
2026-03-04 18:58:47 +08:00
parent 021fb5ba1a
commit fcb8987589
6 changed files with 256 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_pro_base/include/agv_pro_base/agv_pro_driver.h
+14
View File
@@ -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
+95 -2
View File
@@ -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{