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

Fix the issue of odom publishing time being out of sync

Browse Source
This commit is contained in:
X-lanni
2025-06-21 17:32:53 +08:00
parent 6ef6e50cc8
commit 90d7bde0e3
2 changed files with 15 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_pro_base/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3"> <package format="3">
<name>agv_pro_base</name> <name>agv_pro_base</name>
<version>1.0.1</version> <version>1.0.2</version>
<description>Control Nodes for AGV Pro</description> <description>Control Nodes for AGV Pro</description>
<maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer> <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
<license>BSD-3-Clause license</license> <license>BSD-3-Clause license</license>
agv_pro_base/src/agv_pro_ros.cpp
+14 -13
View File
@@ -196,15 +196,8 @@ void AGV_PRO::publisherVoltage()
} }
void AGV_PRO::publisherOdom(double dt) void AGV_PRO::publisherOdom(double dt)
{ {
geometry_msgs::msg::TransformStamped odom_trans; currentTime = this->get_clock()->now();
odom_trans.header.stamp = this->get_clock()->now();
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
tf2::Quaternion quat;
quat.setRPY(0.0, 0.0, theta);
geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(quat);
double delta_x = (vx * cos(theta) - vy * sin(theta)) * dt; double delta_x = (vx * cos(theta) - vy * sin(theta)) * dt;
double delta_y = (vx * sin(theta) + vy * cos(theta)) * dt; double delta_y = (vx * sin(theta) + vy * cos(theta)) * dt;
@@ -214,18 +207,26 @@ void AGV_PRO::publisherOdom(double dt)
y += delta_y; y += delta_y;
theta += delta_th; theta += delta_th;
geometry_msgs::msg::TransformStamped odom_trans;
odom_trans.header.stamp = currentTime;
odom_trans.header.frame_id = frame_id_of_odometry_;
odom_trans.child_frame_id = child_frame_id_of_odometry_;
tf2::Quaternion quat;
quat.setRPY(0.0, 0.0, theta);
geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(quat);
odom_trans.transform.translation.x = x; odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y; odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0; odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat; odom_trans.transform.rotation = odom_quat;
odomBroadcaster->sendTransform(odom_trans); odomBroadcaster->sendTransform(odom_trans);
nav_msgs::msg::Odometry odom; nav_msgs::msg::Odometry odom;
odom.header.stamp = this->get_clock()->now();; odom.header.stamp = currentTime;
odom.header.frame_id = "odom"; odom.header.frame_id = frame_id_of_odometry_;
odom.child_frame_id = "base_footprint"; odom.child_frame_id = child_frame_id_of_odometry_;
odom.pose.pose.position.x = x; odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y; odom.pose.pose.position.y = y;