mirrors/scout_ros
1
0
Fork 0
mirror of https://github.com/westonrobot/scout_ros.git synced 2026-01-12 03:15:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed odom timing issue

Browse Source
This commit is contained in:
Ruixiang Du
2021-10-22 16:40:42 +08:00
parent 601ba3e0ca
commit 5f39943b68
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
scout_base/include/scout_base/scout_messenger.hpp
View File

@@ -94,7 +94,7 @@ class ScoutMessenger {
void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg) { void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg) {
SetScoutMotionCommand(scout_, msg); SetScoutMotionCommand(scout_, msg);
ROS_INFO("Cmd received:%f, %f", msg->linear.x, msg->angular.z); ROS_INFO("Cmd received:%f, %f\n", msg->linear.x, msg->angular.z);
} }
template <typename T, template <typename T,
@@ -148,6 +148,8 @@ class ScoutMessenger {
} }
void PublishOdometryToROS(const MotionStateMessage &msg, double dt) { void PublishOdometryToROS(const MotionStateMessage &msg, double dt) {
auto ctime = ros::Time::now();
// perform numerical integration to get an estimation of pose // perform numerical integration to get an estimation of pose
double linear_speed = msg.linear_velocity; double linear_speed = msg.linear_velocity;
double angular_speed = msg.angular_velocity; double angular_speed = msg.angular_velocity;
@@ -172,7 +174,7 @@ class ScoutMessenger {
// publish tf transformation // publish tf transformation
geometry_msgs::TransformStamped tf_msg; geometry_msgs::TransformStamped tf_msg;
tf_msg.header.stamp = current_time_; tf_msg.header.stamp = ctime;
tf_msg.header.frame_id = odom_frame_; tf_msg.header.frame_id = odom_frame_;
tf_msg.child_frame_id = base_frame_; tf_msg.child_frame_id = base_frame_;
@@ -185,7 +187,7 @@ class ScoutMessenger {
// publish odometry and tf messages // publish odometry and tf messages
nav_msgs::Odometry odom_msg; nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = current_time_; odom_msg.header.stamp = ctime;
odom_msg.header.frame_id = odom_frame_; odom_msg.header.frame_id = odom_frame_;
odom_msg.child_frame_id = base_frame_; odom_msg.child_frame_id = base_frame_;