From 2d70612bc047c18762d690b1541d8f6fd1974ca6 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Fri, 28 Aug 2020 17:47:54 +0800 Subject: [PATCH] updated odom calc for simulator --- scout_base/src/scout_messenger.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/scout_base/src/scout_messenger.cpp b/scout_base/src/scout_messenger.cpp index 02870b0..fd8594f 100644 --- a/scout_base/src/scout_messenger.cpp +++ b/scout_base/src/scout_messenger.cpp @@ -179,7 +179,16 @@ void ScoutROSMessenger::PublishStateToROS() void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) { current_time_ = ros::Time::now(); - double dt = 1.0 / sim_control_rate_; + + double dt = (current_time_ - last_time_).toSec(); + + static bool init_run = true; + if (init_run) + { + last_time_ = current_time_; + init_run = false; + return; + } // publish scout state message scout_msgs::ScoutStatus status_msg; @@ -211,6 +220,9 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) // publish odometry and tf PublishOdometryToROS(linear, angular, dt); + + // record time for next integration + last_time_ = current_time_; } void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt)