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

refactor(agv_pro_base): Change serial_driver to boost/asio serial port library

Browse Source
This commit is contained in:
X-lanni
2025-09-01 19:07:59 +08:00
parent 6e8320c438
commit e4c9f9b489
2 changed files with 275 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_pro_base/include/agv_pro_base/agv_pro_driver.h
+136 -9
View File
@@ -2,7 +2,8 @@
#define AGV_PRO_DRIVER_H
#include <algorithm>
#include "serial_driver/serial_driver.hpp"
#include <iostream>
#include <boost/asio.hpp>
#include "rclcpp/rclcpp.hpp"
@@ -13,7 +14,9 @@
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#define RECEIVE_DATA_SIZE 14 //The length of the data sent by the esp32
#define SEND_DATA_SIZE 14 // Total bytes in a command frame to ESP32
#define RECEIVE_FRAME_SIZE 32 // Total bytes in a frame from ESP32
#define RECEIVE_PAYLOAD_SIZE (RECEIVE_FRAME_SIZE - 3) // Payload length (excluding header)
extern std::array<double, 36> odom_pose_covariance;
extern std::array<double, 36> odom_twist_covariance;
@@ -21,20 +24,131 @@ extern std::array<double, 36> odom_twist_covariance;
class AGV_PRO : public rclcpp::Node
{
public:
/**
* @brief Constructor
*/
AGV_PRO(std::string node_name);
/**
* @brief Destructor
*/
~AGV_PRO();
private:
/**
* @brief Main control loop for the AGV.
*/
void Control();
void print_hex(const std::string& label, const std::vector<uint8_t>& data, std::optional<size_t> override_size = std::nullopt);
/**
* @brief Print a vector of bytes in hexadecimal format to the ROS logger.
*
* @param[in] label A label to prepend to the printed data.
* @param[in] data The byte vector to print.
* @param[in] override_size Optional size to display instead of the full data length.
*/
void print_hex(const std::string& label,
const std::vector<uint8_t>& data,
std::optional<size_t> override_size = std::nullopt);
/**
* @brief Send a serial frame to the AGV and optionally print it in hex.
*
* @param[in] frame The byte vector representing the serial frame to send.
* @param[in] debug If true, prints the transmitted frame using print_hex().
*/
void send_serial_frame(const std::vector<uint8_t>& frame, bool debug);
void is_power_on();
void set_auto_report();
/**
* @brief Query and print the current power status of the AGV.
* @return true if AGV is successfully power on; false otherwise.
*/
bool is_power_on();
/**
* @brief Enable or disable AGV auto-reporting
* @param[in] enable 0 = disable, 1 = enable
*/
void set_auto_report(bool enable);
/**
* @brief Clear the serial port input and output buffers
* @param[in] fd File descriptor of the serial port
*/
void clearSerialBuffer(int fd);
/**
* @brief Disable the DTR (Data Terminal Ready) and RTS (Request To Send) lines of the serial port
* @param[in] fd File descriptor of the serial port
*/
void disableDTR_RTS(int fd);
/**
* @brief Read sensor and motor data from the AGV via serial port.
* @return true if data is successfully read and verified; false otherwise.
*/
bool readData();
/**
* @brief Odometry publisher
* @param[in] dt Time difference (in seconds) since the last odometry update.
*/
void publisherOdom(double dt);
/**
* @brief Voltage publisher
*/
void publisherVoltage();
/**
* @brief Callback for velocity command updates
* @param[in] msg The Twist message containing desired linear and angular velocities
*/
void cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
/**
* @brief Build a standard AGV serial frame with header, payload, and CRC.
*
* The frame has a fixed size of RECEIVE_DATA_SIZE, starts with 0xFE 0xFE 0x0B,
* includes a command ID and up to 8 payload bytes, and ends with a 16-bit CRC.
*
* @param[in] cmd_id The command ID for the serial frame.
* @param[in] payload The payload bytes to include (up to 8 bytes).
* @return A vector containing the complete serial frame ready to transmit.
*/
std::vector<uint8_t> build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload);
std::vector<uint8_t> read_serial_response(const std::vector<uint8_t>& expected_header, size_t payload_size, double timeout_sec);
/**
* @brief Read a serial response from the AGV device, waiting for a specific header.
*
* This function reads bytes from the serial port until the expected header
* sequence is detected or the timeout expires. After detecting the header,
* it reads the remaining payload bytes along with a 2-byte CRC.
*
* @param[in] expected_header The byte sequence to identify the start of a valid frame.
* @param[in] payload_size The expected number of payload bytes following the header.
* @param[in] timeout_sec Maximum time (in seconds) to wait for the header.
* @return A vector containing the complete frame (header + payload + CRC).
* Returns an empty vector if a timeout occurs or the full payload is not received.
*/
std::vector<uint8_t> read_serial_response(
const std::vector<uint8_t>& expected_header,
size_t payload_size,
double timeout_sec);
/**
* @brief Compute the CRC-16-IBM checksum for a byte array.
*
* This function calculates the CRC using the standard IBM polynomial 0xA001.
*
* @param[in] data Pointer to the byte array.
* @param[in] length Number of bytes to include in the CRC calculation.
* @return The computed 16-bit CRC value.
*/
uint16_t crc16_ibm(const uint8_t* data, size_t length);
boost::asio::io_service io_;
std::unique_ptr<boost::asio::serial_port> serial_port_;
std::string frame_id_of_odometry_;
std::string child_frame_id_of_odometry_;
@@ -63,6 +177,22 @@ private:
float battery_voltage = 0.0f;
std::array<double, 36> odom_pose_covariance = {
{1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9} };
std::array<double, 36> odom_twist_covariance = {
{1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9} };
rclcpp::Time currentTime, lastTime;
rclcpp::TimerBase::SharedPtr control_timer_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
@@ -71,9 +201,6 @@ private:
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub;
std::unique_ptr<tf2_ros::TransformBroadcaster> odomBroadcaster;
std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
std::shared_ptr<drivers::common::IoContext> io_context_;
};
#endif