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

updated odom calc for simulator

Browse Source
This commit is contained in:
Ruixiang Du
2020-08-28 17:47:54 +08:00
parent 9a263392fc
commit 2d70612bc0
1 changed files with 13 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
scout_base/src/scout_messenger.cpp
View File

@@ -179,7 +179,16 @@ void ScoutROSMessenger::PublishStateToROS()
void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
{ {
current_time_ = ros::Time::now(); 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 // publish scout state message
scout_msgs::ScoutStatus status_msg; scout_msgs::ScoutStatus status_msg;
@@ -211,6 +220,9 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
// publish odometry and tf // publish odometry and tf
PublishOdometryToROS(linear, angular, dt); PublishOdometryToROS(linear, angular, dt);
// record time for next integration
last_time_ = current_time_;
} }
void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt) void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt)