From a29b104b9d7b1ed84126d3d99f322fcc460e3869 Mon Sep 17 00:00:00 2001 From: X-lanni Date: Tue, 2 Sep 2025 19:06:41 +0800 Subject: [PATCH] feat(agv_pro_base): add ImuSensor publisher --- .../include/agv_pro_base/agv_pro_driver.h | 22 ++- agv_pro_base/src/agv_pro_ros.cpp | 160 +++++++++++++----- 2 files changed, 134 insertions(+), 48 deletions(-) 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 10cd4dd..deb81d0 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 @@ -14,8 +14,8 @@ #include #include -#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 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) extern std::array odom_pose_covariance; @@ -100,6 +100,11 @@ private: */ void publisherVoltage(); + /** + * @brief ImuSensor publisher + */ + void publisherImuSensor(); + /** * @brief Callback for velocity command updates * @param[in] msg The Twist message containing desired linear and angular velocities @@ -168,6 +173,18 @@ private: double linearY = 0.0; double angularZ = 0.0; + double ax= 0.0; + double ay= 0.0; + double az= 0.0; + + double wx= 0.0; + double wy= 0.0; + double wz= 0.0; + + double roll = 0.0; + double pitch = 0.0; + double yaw = 0.0; + int is_poweron_status = 0; int poweron_status = 0; @@ -200,6 +217,7 @@ private: rclcpp::Publisher::SharedPtr pub_voltage; rclcpp::Subscription::SharedPtr cmd_sub; + sensor_msgs::msg::Imu imu_data; std::unique_ptr odomBroadcaster; }; diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp index 4122a73..bfac77e 100644 --- a/agv_pro_base/src/agv_pro_ros.cpp +++ b/agv_pro_base/src/agv_pro_ros.cpp @@ -55,65 +55,87 @@ 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) +// { +// boost::system::error_code ec; +// std::vector sliding_buf; +// uint8_t byte = 0; + +// while (true) { +// size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec); +// if (ec) { +// RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); +// break; +// } + +// if (ret > 0) { +// sliding_buf.push_back(byte); +// RCLCPP_INFO(this->get_logger(), "Recv: 0x%02X", byte); +// if (sliding_buf.size() > 1024) { +// sliding_buf.clear(); +// } +// } +// } +// } + std::vector AGV_PRO::read_serial_response( const std::vector& expected_header, size_t payload_size, double timeout_sec) { - std::vector sliding_buf; + std::vector buffer; uint8_t byte = 0; rclcpp::Time start_time = this->now(); rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec); - while ((this->now() - start_time) < timeout) { + while (rclcpp::ok()) { boost::asio::mutable_buffers_1 buf(&byte, 1); boost::system::error_code ec; - size_t n = serial_port_->read_some(buf, ec); + size_t len = serial_port_->read_some(boost::asio::buffer(&byte,1), ec); if (ec) { RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); - return {}; + break; } - if (n == 1) { - sliding_buf.push_back(byte); - if (sliding_buf.size() > expected_header.size()) { - sliding_buf.erase(sliding_buf.begin()); + if (len > 0) { + buffer.push_back(byte); + //**log */ + RCLCPP_INFO(this->get_logger(), "recv byte: 0x%02X", byte); + std::string buf_str; + for (auto b : buffer) { + char tmp[5]; + snprintf(tmp, sizeof(tmp), "%02X ", b); + buf_str += tmp; } - if (sliding_buf == expected_header) { - break; + std::string header_str; + for (auto b : expected_header) { + char tmp[5]; + snprintf(tmp, sizeof(tmp), "%02X ", b); + header_str += tmp; + } + RCLCPP_INFO(this->get_logger(), "buffer: %s", buf_str.c_str()); + RCLCPP_INFO(this->get_logger(), "expected_header: %s", header_str.c_str()); + //**log */ + if (buffer.size() >= expected_header.size()) { + bool match = true; + for (size_t i=0;iget_logger(), "Timeout waiting for header"); - return {}; + if (buffer.empty()) { + RCLCPP_WARN(this->get_logger(), "No data received within timeout!"); + } else { + RCLCPP_INFO(this->get_logger(), "Final buffer size=%zu", buffer.size()); } - - size_t remain_len = payload_size + 2; - std::vector 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 {}; - } - - std::vector full_buf = expected_header; - full_buf.insert(full_buf.end(), remain_buf.begin(), remain_buf.end()); - - return full_buf; + return buffer; } bool AGV_PRO::is_power_on(){ @@ -121,7 +143,6 @@ bool AGV_PRO::is_power_on(){ send_serial_frame(power_query_frame,true); const std::vector expected_header = {0xFE, 0xFE, 0x0B, 0x12}; - 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); @@ -198,7 +219,7 @@ void AGV_PRO::set_auto_report(bool enable){ } void AGV_PRO::clearSerialBuffer(int fd) { - if (::tcflush(fd, TCIOFLUSH) != 0) { + 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."); @@ -292,8 +313,8 @@ bool AGV_PRO::readData() return false; } - if (buf_length[0] != RECEIVE_FRAME_SIZE-6) { - RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]); + if (buf_length[0] != RECEIVE_PAYLOAD_SIZE) { + //RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]); return false; } @@ -307,13 +328,13 @@ bool AGV_PRO::readData() std::vector recv_buf; recv_buf.push_back(0xFE); recv_buf.push_back(0xFE); - recv_buf.push_back(RECEIVE_FRAME_SIZE-6); + 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]); + //RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); //debug return false; } @@ -334,6 +355,18 @@ 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.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; + return true; } @@ -344,6 +377,40 @@ void AGV_PRO::publisherVoltage() pub_voltage->publish(voltage_msg); } +void AGV_PRO::publisherImuSensor() +{ + sensor_msgs::msg::Imu ImuSensor; + + ImuSensor.header.stamp = this->get_clock()->now(); + ImuSensor.header.frame_id = "imu_link"; + + tf2::Quaternion qua; + qua.setRPY(0, 0, yaw * M_PI / 180.0); + + ImuSensor.orientation.x = qua[0]; + ImuSensor.orientation.y = qua[1]; + ImuSensor.orientation.z = qua[2]; + ImuSensor.orientation.w = qua[3]; + + ImuSensor.angular_velocity.x = imu_data.angular_velocity.x; + ImuSensor.angular_velocity.y = imu_data.angular_velocity.y; + ImuSensor.angular_velocity.z = imu_data.angular_velocity.z; + + ImuSensor.linear_acceleration.x = imu_data.linear_acceleration.x; + ImuSensor.linear_acceleration.y = imu_data.linear_acceleration.y; + ImuSensor.linear_acceleration.z = imu_data.linear_acceleration.z; + + ImuSensor.orientation_covariance[0] = 1e6; + ImuSensor.orientation_covariance[4] = 1e6; + ImuSensor.orientation_covariance[8] = 1e-6; + + ImuSensor.angular_velocity_covariance[0] = 1e6; + ImuSensor.angular_velocity_covariance[4] = 1e6; + ImuSensor.angular_velocity_covariance[8] = 1e-6; + + pub_imu->publish(ImuSensor); +} + void AGV_PRO::publisherOdom(double dt) { currentTime = this->get_clock()->now(); @@ -405,6 +472,7 @@ void AGV_PRO::Control() publisherOdom(dt); // RCLCPP_INFO(this->get_logger(), "dt:%f", dt); publisherVoltage(); + publisherImuSensor(); } }