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 fa1512e..10cd4dd 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 @@ -2,7 +2,8 @@ #define AGV_PRO_DRIVER_H #include -#include "serial_driver/serial_driver.hpp" +#include +#include #include "rclcpp/rclcpp.hpp" @@ -13,7 +14,9 @@ #include #include -#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 odom_pose_covariance; extern std::array odom_twist_covariance; @@ -21,20 +24,131 @@ extern std::array 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& data, std::optional 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& data, + std::optional 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& 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 build_serial_frame(uint8_t cmd_id, const std::vector& payload); - std::vector read_serial_response(const std::vector& 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 read_serial_response( + const std::vector& 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 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 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 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::SharedPtr pub_odom; @@ -71,9 +201,6 @@ private: rclcpp::Subscription::SharedPtr cmd_sub; std::unique_ptr odomBroadcaster; - std::shared_ptr serial_driver_; - std::shared_ptr io_context_; - }; #endif \ No newline at end of file diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp index 829522a..4122a73 100644 --- a/agv_pro_base/src/agv_pro_ros.cpp +++ b/agv_pro_base/src/agv_pro_ros.cpp @@ -1,22 +1,6 @@ #include "agv_pro_base/agv_pro_driver.h" -std::array 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 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} }; - -uint16_t crc16_ibm(const uint8_t* data, size_t length) { +uint16_t AGV_PRO::crc16_ibm(const uint8_t* data, size_t length) { uint16_t crc = 0xFFFF; for (size_t i = 0; i < length; ++i) { crc ^= static_cast(data[i]); @@ -32,7 +16,7 @@ uint16_t crc16_ibm(const uint8_t* data, size_t length) { std::vector AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector& payload) { - std::vector frame(RECEIVE_DATA_SIZE, 0x00); + std::vector frame(SEND_DATA_SIZE, 0x00); frame[0] = 0xFE; frame[1] = 0xFE; frame[2] = 0x0B; @@ -62,8 +46,7 @@ void AGV_PRO::print_hex(const std::string& label, const std::vector& da void AGV_PRO::send_serial_frame(const std::vector& frame, bool debug) { try { - auto port = serial_driver_->port(); - size_t bytes_transmit_size = port->send(frame); + size_t bytes_transmit_size = boost::asio::write(*serial_port_, boost::asio::buffer(frame)); if (debug) { print_hex("Sent", frame, bytes_transmit_size); } @@ -72,9 +55,11 @@ void AGV_PRO::send_serial_frame(const std::vector& frame, bool debug) } } -std::vector AGV_PRO::read_serial_response(const std::vector& expected_header, size_t payload_size, double timeout_sec) +std::vector AGV_PRO::read_serial_response( + const std::vector& expected_header, + size_t payload_size, + double timeout_sec) { - auto port = serial_driver_->port(); std::vector sliding_buf; uint8_t byte = 0; @@ -82,21 +67,24 @@ std::vector AGV_PRO::read_serial_response(const std::vector& e rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec); while ((this->now() - start_time) < timeout) { - std::vector temp_buf(1); - if (port->receive(temp_buf) == 1) { - byte = temp_buf[0]; + boost::asio::mutable_buffers_1 buf(&byte, 1); + boost::system::error_code ec; + size_t n = serial_port_->read_some(buf, ec); + if (ec) { + RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return {}; + } + if (n == 1) { sliding_buf.push_back(byte); - if (sliding_buf.size() > expected_header.size()) { - sliding_buf.erase(sliding_buf.begin()); + sliding_buf.erase(sliding_buf.begin()); } - if (sliding_buf == expected_header) { - break; + break; } } } - + if (sliding_buf != expected_header) { RCLCPP_WARN(this->get_logger(), "Timeout waiting for header"); return {}; @@ -104,7 +92,20 @@ std::vector AGV_PRO::read_serial_response(const std::vector& e size_t remain_len = payload_size + 2; std::vector remain_buf(remain_len); - if (port->receive(remain_buf) != remain_len) { + size_t total_read = 0; + + while (total_read < remain_len && (this->now() - start_time) < timeout) { + boost::asio::mutable_buffers_1 buf(&remain_buf[total_read], remain_len - total_read); + boost::system::error_code ec; + size_t n = serial_port_->read_some(buf, ec); + if (ec) { + RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return {}; + } + total_read += n; + } + + if (total_read != remain_len) { RCLCPP_WARN(this->get_logger(), "Timeout or incomplete data payload"); return {}; } @@ -115,22 +116,23 @@ std::vector AGV_PRO::read_serial_response(const std::vector& e return full_buf; } -void AGV_PRO::is_power_on(){ +bool AGV_PRO::is_power_on(){ auto power_query_frame = build_serial_frame(0x12, {}); send_serial_frame(power_query_frame,true); const std::vector expected_header = {0xFE, 0xFE, 0x0B, 0x12}; - auto power_query_response = read_serial_response(expected_header, 8, 1.0); + RCLCPP_INFO(this->get_logger(),"1111111"); + auto power_query_response = read_serial_response(expected_header, 8, 12.0); print_hex("recv_buf", power_query_response); - if (power_query_response.size() != 14) return; + if (power_query_response.size() != 14) return false; uint16_t received_crc = (power_query_response[12] << 8) | power_query_response[13]; uint16_t computed_crc = crc16_ibm(power_query_response.data(), 12); if (received_crc != computed_crc) { RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc); - return; + return false; } int is_poweron_status = static_cast(power_query_response[4]); @@ -140,19 +142,19 @@ void AGV_PRO::is_power_on(){ auto status_query_frame = build_serial_frame(0x10, {}); 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 + //rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command const std::vector expected_header = {0xFE, 0xFE, 0x0B, 0x10}; auto status_query_response = read_serial_response(expected_header, 8, 5.0);// Read the serial response with the specified expected header, payload size, and timeout of 5 seconds print_hex("recv_buf", status_query_response); - if (status_query_response.size() != 14) return; + if (status_query_response.size() != 14) return false; uint16_t received_crc = (status_query_response[12] << 8) | status_query_response[13]; uint16_t computed_crc = crc16_ibm(status_query_response.data(), 12); if (received_crc != computed_crc) { RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc); - return; + return false; } int poweron_status = static_cast(status_query_response[4]); @@ -162,37 +164,61 @@ void AGV_PRO::is_power_on(){ case 1: status_msg = "Motor is operating normally."; RCLCPP_INFO(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); - break; + return true; case 2: status_msg = "Emergency stop button is not released."; RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); - break; + return false; case 3: status_msg = "Battery voltage is below 19.5V."; RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); - break; + return false; case 4: status_msg = "CAN initialization error."; RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); - break; + return false; case 5: status_msg = "Motor initialization error."; RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); - break; + return false; default: RCLCPP_WARN(this->get_logger(), "power_status: %d, Unknown power status code", poweron_status); - break; + return false; } } - else + else{ RCLCPP_INFO(this->get_logger(), "Motor is operating normally."); + return true; + } } -void AGV_PRO::set_auto_report(){ - auto frame = build_serial_frame(0x23, {0x01}); +void AGV_PRO::set_auto_report(bool enable){ + auto frame = build_serial_frame(0x23, {static_cast(enable)}); send_serial_frame(frame,true); } +void AGV_PRO::clearSerialBuffer(int fd) { + if (::tcflush(fd, TCIOFLUSH) != 0) { + RCLCPP_WARN(this->get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno)); + } else { + RCLCPP_INFO(this->get_logger(), "Serial buffer flushed."); + } +} + +void AGV_PRO::disableDTR_RTS(int fd) { + int status; + if (::ioctl(fd, TIOCMGET, &status) == 0) { + status &= ~(TIOCM_DTR | TIOCM_RTS); + if (::ioctl(fd, TIOCMSET, &status) != 0) { + RCLCPP_WARN(this->get_logger(), "Failed to clear DTR and RTS: %s", std::strerror(errno)); + } else { + RCLCPP_INFO(this->get_logger(), "DTR and RTS lines disabled successfully."); + } + } else { + RCLCPP_WARN(this->get_logger(), "Failed to read modem status: %s", std::strerror(errno)); + } +} + void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { linearX = std::clamp(msg->linear.x, -1.5, 1.5); @@ -220,11 +246,9 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) std::vector data_vec(buf, buf + sizeof(buf)); - auto port = serial_driver_->port(); - try { - port->send(data_vec); + boost::asio::write(*serial_port_,boost::asio::buffer(data_vec)); // print_hex("Sent", data_vec);//debug } catch(const std::exception &ex) @@ -235,34 +259,46 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) bool AGV_PRO::readData() { - std::vector buf_header(1); std::vector buf_length(1); - std::vector data_buf(RECEIVE_DATA_SIZE-3); + std::vector data_buf(RECEIVE_PAYLOAD_SIZE); - auto port = serial_driver_->port(); + uint8_t byte = 0; + boost::system::error_code ec; while (true) { - size_t ret = port->receive(buf_header); - if (ret != 1 || buf_header[0] != 0xfe) { + size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec); + if (ec) { + RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return false; + } + if (ret != 1 || byte != 0xfe) { continue; } - ret = port->receive(buf_header); - if (ret == 1 && buf_header[0] == 0xfe) { + ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec); + if (ec) { + RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return false; + } + if (ret == 1 && byte == 0xfe) { break; } } - size_t ret = port->receive(buf_length); + size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(buf_length), ec); + if (ec) { + RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return false; + } - if (buf_length[0] != 0x0b) { + if (buf_length[0] != RECEIVE_FRAME_SIZE-6) { RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]); return false; } - ret = port->receive(data_buf); - if (ret != data_buf.size()) + ret = boost::asio::read(*serial_port_, boost::asio::buffer(data_buf), ec); + if (ec || ret != data_buf.size()) { RCLCPP_ERROR(this->get_logger(), "Failed to receive full payload"); return false; @@ -271,7 +307,7 @@ bool AGV_PRO::readData() std::vector recv_buf; recv_buf.push_back(0xFE); recv_buf.push_back(0xFE); - recv_buf.push_back(0x0B); + recv_buf.push_back(RECEIVE_FRAME_SIZE-6); recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end()); // print_hex("recv_buf", recv_buf); //debug @@ -281,8 +317,8 @@ bool AGV_PRO::readData() return false; } - uint16_t received_crc = recv_buf[13] | (recv_buf[12] << 8); - uint16_t computed_crc = crc16_ibm(recv_buf.data(), 12); + uint16_t received_crc = recv_buf[RECEIVE_FRAME_SIZE-1] | (recv_buf[RECEIVE_FRAME_SIZE-2] << 8); + uint16_t computed_crc = crc16_ibm(recv_buf.data(), RECEIVE_FRAME_SIZE-2); if (received_crc != computed_crc) { RCLCPP_WARN(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc); @@ -345,12 +381,12 @@ void AGV_PRO::publisherOdom(double dt) odom.pose.pose.position.y = y; odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; - odom.pose.covariance = odom_pose_covariance; + odom.pose.covariance = this->odom_pose_covariance; odom.twist.twist.linear.x = vx; odom.twist.twist.linear.y = vy; odom.twist.twist.angular.z = vtheta; - odom.twist.covariance = odom_twist_covariance; + odom.twist.covariance = this->odom_twist_covariance; pub_odom->publish(odom); } @@ -401,64 +437,52 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name) lastTime = this->get_clock()->now(); - drivers::serial_driver::SerialPortConfig config( - 1000000, - drivers::serial_driver::FlowControl::NONE, - drivers::serial_driver::Parity::NONE, - drivers::serial_driver::StopBits::ONE - ); - try{ - io_context_ = std::make_shared(1); - serial_driver_ = std::make_shared(*io_context_); - serial_driver_->init_port(device_name_, config); - serial_driver_->port()->open(); - - RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully"); - RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str()); - RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate()); + serial_port_ = std::make_unique(io_); - AGV_PRO::is_power_on(); - AGV_PRO::set_auto_report(); + serial_port_->open(device_name_); + serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000)); + serial_port_->set_option(boost::asio::serial_port_base::character_size(8)); + serial_port_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); + serial_port_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); + serial_port_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none)); + + int fd = serial_port_->native_handle(); + this->clearSerialBuffer(fd); + this->disableDTR_RTS(fd); + + RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully"); + RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str()); + + boost::asio::serial_port_base::baud_rate baud_option; + serial_port_->get_option(baud_option); + unsigned int current_baud = baud_option.value(); + RCLCPP_INFO(this->get_logger(), "Baud_rate: %u", current_baud); } catch (const std::exception &ex){ RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what()); return; } - - control_timer_ = this->create_wall_timer( - std::chrono::milliseconds(20), - std::bind(&AGV_PRO::Control, this) - ); - RCLCPP_INFO(this->get_logger(), "Control timer started"); + if (this->is_power_on()) { + this->set_auto_report(1); + + control_timer_ = this->create_wall_timer( + std::chrono::milliseconds(20), + std::bind(&AGV_PRO::Control, this) + ); + RCLCPP_INFO(this->get_logger(), "Control timer started"); + } + else { + RCLCPP_WARN(this->get_logger(), "Control timer not started."); + } } AGV_PRO::~AGV_PRO() -{ - std::array buf = { - 0xFE, 0xFE, 0x0b, 0x22, - 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00 - }; - - uint16_t crc = crc16_ibm(buf.data(), 12); - buf[12] = (crc >> 8) & 0xff; - buf[13] = crc & 0xff; - - std::vector data_vec(buf.begin(), buf.end()); - - auto port = serial_driver_->port(); - - try - { - port->send(data_vec); - } - catch(const std::exception &ex) - { - RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what()); - } - - serial_driver_->port()->close(); - RCLCPP_INFO(this->get_logger(),"Shutting down"); +{ + if (serial_port_ && serial_port_->is_open()) { + this->set_auto_report(0); + serial_port_->cancel(); + serial_port_->close(); + } } \ No newline at end of file