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

changed sim integration

Browse Source
This commit is contained in:
Tan Pin Da
2020-09-02 10:30:18 +08:00
parent b38787c86c
commit 5dd389d07b
1 changed files with 21 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
scout_base/src/scout_messenger.cpp
View File

@@ -13,6 +13,8 @@
#include "scout_msgs/ScoutStatus.h" #include "scout_msgs/ScoutStatus.h"
#include <ros/console.h>
namespace westonrobot namespace westonrobot
{ {
ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh) : scout_(nullptr), nh_(nh) ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh) : scout_(nullptr), nh_(nh)
@@ -131,6 +133,7 @@ void ScoutROSMessenger::PublishStateToROS()
current_time_ = ros::Time::now(); current_time_ = ros::Time::now();
double dt = (current_time_ - last_time_).toSec(); double dt = (current_time_ - last_time_).toSec();
static bool init_run = true; static bool init_run = true;
if (init_run) if (init_run)
{ {
@@ -171,6 +174,9 @@ void ScoutROSMessenger::PublishStateToROS()
// publish odometry and tf // publish odometry and tf
PublishOdometryToROS(state.linear_velocity, state.angular_velocity, dt); PublishOdometryToROS(state.linear_velocity, state.angular_velocity, dt);
// ROS_DEBUG("dt is %lg",dt);
std::cout<<"dt is"<<dt<< std::endl;
ROS_INFO("dt is %lg",dt);
// record time for next integration // record time for next integration
last_time_ = current_time_; last_time_ = current_time_;
@@ -179,7 +185,20 @@ 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_;
static bool init_run = true;
if (init_run)
{
last_time_ = current_time_;
init_run = false;
return;
}
double dt = (current_time_ - last_time_).toSec();
// double dt = 1.0 / sim_control_rate_;
ROS_INFO("dt is %lg",dt);
// double test_time = current_time_.toSec();
// ROS_INFO("current time is %lg",test_time);
// publish scout state message // publish scout state message
scout_msgs::ScoutStatus status_msg; scout_msgs::ScoutStatus status_msg;
@@ -211,6 +230,7 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
// publish odometry and tf // publish odometry and tf
PublishOdometryToROS(linear, angular, dt); PublishOdometryToROS(linear, angular, dt);
last_time_ = current_time_;
} }
void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt) void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt)