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) {
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,
@@ -148,6 +148,8 @@ class ScoutMessenger {
}
void PublishOdometryToROS(const MotionStateMessage &msg, double dt) {
auto ctime = ros::Time::now();
// perform numerical integration to get an estimation of pose
double linear_speed = msg.linear_velocity;
double angular_speed = msg.angular_velocity;
@@ -172,7 +174,7 @@ class ScoutMessenger {
// publish tf transformation
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.child_frame_id = base_frame_;
@@ -185,7 +187,7 @@ class ScoutMessenger {
// publish odometry and tf messages
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.child_frame_id = base_frame_;