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

Add odometry publishing functionality with initial validation

Browse Source
This commit is contained in:
X-lanni
2025-06-10 17:34:47 +08:00
parent 4eefeb4b61
commit e551ce2394
2 changed files with 81 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_pro_base/include/agv_pro_base/agv_pro_driver.h
+4 -3
View File
@@ -13,8 +13,8 @@
#include <tf2/LinearMath/Quaternion.h> #include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#define RECEIVE_DATA_SIZE 13 //The length of the data sent by the esp32 #define RECEIVE_DATA_SIZE 14 //The length of the data sent by the esp32
#define SEND_DATA_SIZE 13 //The length of data sent by ROS to the esp32 #define SEND_DATA_SIZE 14 //The length of data sent by ROS to the esp32
#define RETURN_COMMAND 0x25 #define RETURN_COMMAND 0x25
extern std::array<double, 36> odom_pose_covariance; extern std::array<double, 36> odom_pose_covariance;
@@ -31,7 +31,6 @@ private:
bool readData(); bool readData();
void publisherOdom(double dt); void publisherOdom(double dt);
void publisherVoltage(); void publisherVoltage();
void clearSerialBuffer();
void cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg); void cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
std::string frame_id_of_odometry_; std::string frame_id_of_odometry_;
@@ -39,6 +38,8 @@ private:
std::string frame_id_of_imu_; std::string frame_id_of_imu_;
std::string name_space_; std::string name_space_;
std::string device_name_; std::string device_name_;
std::thread control_thread_;
double x= 0.0; double x= 0.0;
double y= 0.0; double y= 0.0;
agv_pro_base/src/agv_pro_ros.cpp
+77 -80
View File
@@ -32,15 +32,15 @@ uint16_t crc16_ibm(const uint8_t* data, size_t length) {
void AGV_PRO::set_auto_report(){ void AGV_PRO::set_auto_report(){
std::array<uint8_t, 13> buf = { std::array<uint8_t, 14> buf = {
0xFE, 0xFE, 0x23, 0x01, 0xFE, 0xFE, 0x0b, 0x23,
0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00 0x00, 0x00, 0x00, 0x00
}; };
uint16_t crc = crc16_ibm(buf.data(), buf.size()); uint16_t crc = crc16_ibm(buf.data(), 12);
buf[11] = crc & 0xff;
buf[12] = (crc >> 8) & 0xff; buf[12] = (crc >> 8) & 0xff;
buf[13] = crc & 0xff;
std::vector<uint8_t> data_vec(buf.begin(), buf.end()); std::vector<uint8_t> data_vec(buf.begin(), buf.end());
@@ -48,14 +48,13 @@ void AGV_PRO::set_auto_report(){
try try
{ {
port->send(data_vec); size_t bytes_transmit_size = port->send(data_vec);
// size_t bytes_transmit_size = port->send(data_vec); std::stringstream ss;
// std::stringstream ss; for (auto b : data_vec) {
// for (auto b : data_vec) { ss << std::hex << std::uppercase << std::setfill('0') << std::setw(2)
// ss << std::hex << std::uppercase << std::setfill('0') << std::setw(2) << static_cast<int>(b) << " ";
// << static_cast<int>(b) << " "; }
// } RCLCPP_INFO(this->get_logger(), "Sent %ld bytes: [%s]", bytes_transmit_size, ss.str().c_str());
// RCLCPP_INFO(this->get_logger(), "Sent %ld bytes: [%s]", bytes_transmit_size, ss.str().c_str());
} }
catch(const std::exception &ex) catch(const std::exception &ex)
{ {
@@ -70,22 +69,24 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
linearY = std::clamp(msg->linear.y, -1.0, 1.0); linearY = std::clamp(msg->linear.y, -1.0, 1.0);
angularZ = std::clamp(msg->angular.z, -1.0, 1.0); angularZ = std::clamp(msg->angular.z, -1.0, 1.0);
int16_t x_send = static_cast<int16_t>(linearX * 1000); int16_t x_send = static_cast<int16_t>(linearX * 100);
int16_t y_send = static_cast<int16_t>(linearY * 1000); int16_t y_send = static_cast<int16_t>(linearY * 100);
int16_t rot_send = static_cast<int16_t>(angularZ * 1000); int16_t rot_send = static_cast<int16_t>(angularZ * 100);
uint8_t buf[13] = { 0xfe,0xfe,0x21 }; uint8_t buf[14] = { 0xfe,0xfe,0x0b,0x21 };
buf[3] = x_send & 0xff;
buf[4] = (x_send >> 8) & 0xff; buf[4] = (x_send >> 8) & 0xff;
buf[5] = y_send & 0xff; buf[5] = x_send & 0xff;
buf[6] = (y_send >> 8) & 0xff; buf[6] = (y_send >> 8) & 0xff;
buf[7] = rot_send & 0xff; buf[7] = y_send & 0xff;
buf[8] = (rot_send >> 8) & 0xff; buf[8] = (rot_send >> 8) & 0xff;
buf[9] = rot_send & 0xff;
buf[10] = 0x00;
buf[11] = 0x00;
uint16_t crc = crc16_ibm(buf, 9); uint16_t crc = crc16_ibm(buf, 12);
buf[9] = crc & 0xff; buf[12] = (crc >> 8) & 0xff;
buf[10] = (crc >> 8) & 0xff; buf[13] = crc & 0xff;
std::vector<uint8_t> data_vec(buf, buf + sizeof(buf)); std::vector<uint8_t> data_vec(buf, buf + sizeof(buf));
@@ -94,6 +95,8 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
try try
{ {
port->send(data_vec); port->send(data_vec);
//debug************************************
// size_t bytes_transmit_size = port->send(data_vec); // size_t bytes_transmit_size = port->send(data_vec);
// std::stringstream ss; // std::stringstream ss;
// for (auto b : data_vec) { // for (auto b : data_vec) {
@@ -101,6 +104,8 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
// << static_cast<int>(b) << " "; // << static_cast<int>(b) << " ";
// } // }
// RCLCPP_INFO(this->get_logger(), "Sent %ld bytes: [%s]", bytes_transmit_size, ss.str().c_str()); // RCLCPP_INFO(this->get_logger(), "Sent %ld bytes: [%s]", bytes_transmit_size, ss.str().c_str());
//debug************************************
} }
catch(const std::exception &ex) catch(const std::exception &ex)
{ {
@@ -108,27 +113,11 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
} }
} }
void AGV_PRO::clearSerialBuffer()
{
auto port = serial_driver_->port();
std::vector<uint8_t> temp_buf(64);
size_t total_discarded = 0;
while (true) {
size_t n = port->receive(temp_buf);
if (n == 0) {
break;
}
total_discarded += n;
}
RCLCPP_WARN(this->get_logger(), "Serial buffer flushed, %zu bytes discarded.", total_discarded);
}
bool AGV_PRO::readData() bool AGV_PRO::readData()
{ {
std::vector<uint8_t> buf_header(1); std::vector<uint8_t> buf_header(1);
std::vector<uint8_t> recv_buf(RECEIVE_DATA_SIZE); std::vector<uint8_t> buf_length(1);
std::vector<uint8_t> data_buf(RECEIVE_DATA_SIZE-3);
auto port = serial_driver_->port(); auto port = serial_driver_->port();
@@ -145,40 +134,56 @@ bool AGV_PRO::readData()
} }
} }
std::vector<uint8_t> remaining_buf(RECEIVE_DATA_SIZE - 2); size_t ret = port->receive(buf_length);
size_t ret = port->receive(remaining_buf);
if (ret != (RECEIVE_DATA_SIZE - 2)) { if (buf_length[0] != 0x0b) {
RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%zu", ret); RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
clearSerialBuffer();
return false; return false;
} }
recv_buf[0] = 0xFE; ret = port->receive(data_buf);
recv_buf[1] = 0xFE; if (ret != data_buf.size())
std::copy(remaining_buf.begin(), remaining_buf.end(), recv_buf.begin() + 2); {
RCLCPP_ERROR(this->get_logger(), "Failed to receive full payload");
if (recv_buf[2] != 0x25) {
RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]);
clearSerialBuffer();
return false; return false;
} }
uint16_t received_crc = recv_buf[11] | (recv_buf[12] << 8); std::vector<uint8_t> recv_buf;
uint16_t computed_crc = crc16_ibm(recv_buf.data(), 11); recv_buf.push_back(0xFE);
recv_buf.push_back(0xFE);
recv_buf.push_back(0x0B);
recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end());
//debug************************************
// std::stringstream ss;
// for (const auto& byte : recv_buf) {
// ss << std::hex << std::uppercase << std::setw(2) << std::setfill('0')
// << static_cast<int>(byte) << " ";
// }
// RCLCPP_INFO(this->get_logger(), "recv_buf: [%s]", ss.str().c_str());
//debug************************************
if (recv_buf[3] != 0x25) {
// RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]);
return false;
}
uint16_t received_crc = recv_buf[13] | (recv_buf[12] << 8);
uint16_t computed_crc = crc16_ibm(recv_buf.data(), 12);
if (received_crc != computed_crc) { if (received_crc != computed_crc) {
RCLCPP_ERROR(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc); RCLCPP_WARN(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc);
return false; return false;
} }
vx = (static_cast<double>(recv_buf[3]) - 128.0) * 0.01; vx = static_cast<double>(static_cast<int8_t>(recv_buf[4])) * 0.01;
vy = (static_cast<double>(recv_buf[4]) - 128.0) * 0.01; vy = static_cast<double>(static_cast<int8_t>(recv_buf[5])) * 0.01;
vtheta = (static_cast<double>(recv_buf[5]) - 128.0) * 0.01; vtheta = static_cast<double>(static_cast<int8_t>(recv_buf[6])) * 0.01;
motor_status = recv_buf[6]; motor_status = recv_buf[7];
motor_error = recv_buf[7]; motor_error = recv_buf[8];
battery_voltage = static_cast<float>(recv_buf[8]) / 10.0f; battery_voltage = static_cast<float>(recv_buf[9]) / 10.0f;
enable_status = recv_buf[9]; enable_status = recv_buf[10];
return true; return true;
} }
@@ -246,7 +251,7 @@ void AGV_PRO::Control()
if (true == readData()) if (true == readData())
{ {
publisherOdom(dt); publisherOdom(dt);
RCLCPP_INFO(this->get_logger(), "dt:%f", dt); //RCLCPP_INFO(this->get_logger(), "dt:%f", dt);
publisherVoltage(); publisherVoltage();
} }
lastTime = currentTime; lastTime = currentTime;
@@ -304,21 +309,20 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
return; return;
} }
AGV_PRO::Control();//Loop through data collection and publish the topic control_thread_ = std::thread(&AGV_PRO::Control, this);
} }
AGV_PRO::~AGV_PRO() AGV_PRO::~AGV_PRO()
{ {
std::array<uint8_t, 13> buf = { std::array<uint8_t, 14> buf = {
0xFE, 0xFE, 0x22, 0x01, 0xFE, 0xFE, 0x0b, 0x22,
0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00 0x00, 0x00, 0x00, 0x00
}; };
uint16_t crc = crc16_ibm(buf.data(), buf.size()); uint16_t crc = crc16_ibm(buf.data(), 12);
buf[11] = crc & 0xff;
buf[12] = (crc >> 8) & 0xff; buf[12] = (crc >> 8) & 0xff;
buf[13] = crc & 0xff;
std::vector<uint8_t> data_vec(buf.begin(), buf.end()); std::vector<uint8_t> data_vec(buf.begin(), buf.end());
@@ -327,13 +331,6 @@ AGV_PRO::~AGV_PRO()
try try
{ {
port->send(data_vec); port->send(data_vec);
// size_t bytes_transmit_size = port->send(data_vec);
// std::stringstream ss;
// for (auto b : data_vec) {
// ss << std::hex << std::uppercase << std::setfill('0') << std::setw(2)
// << static_cast<int>(b) << " ";
// }
// RCLCPP_INFO(this->get_logger(), "Sent %ld bytes: [%s]", bytes_transmit_size, ss.str().c_str());
} }
catch(const std::exception &ex) catch(const std::exception &ex)
{ {