Subscriber Callback Function is not being triggered
I have written a plugin that is supposed to synchronize two Gazebo simulations in Lock-Step fashion. The plugin is loaded to both simulation worlds, so each of the worlds can run for dt
time (for example for 1 s) uninterrupted, but then it pauses and waits for a message from the other simulation. If the plugin registered a message with simulation time at least the same as its own simulation time or larger, it will run for another dt
. This way the simulations will never be more than dt
seconds away from each other.
Here is a diagram how I want it to work.
The problem is, that when one simulation sends the message with its time (I know it has been send because it shows in rostopic echo
), the other simulation does not register the message (the callback function is not triggered). However, if I send the message using rostopic pub
the callback function is triggered. I don't understand why should the plugin behave such way. I'm out of ideas.
Plugin code:
#include "WorldLockStepSync.hh"
namespace gazebo
{
WorldLockStepSync::WorldLockStepSync(){}
void WorldLockStepSync::Init(){}
void WorldLockStepSync::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
{
this->world = _world;
this->sdf = _sdf;
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&WorldLockStepSync::onUpdate, this));
std::string node_name = "lock_step_sync";
if( _sdf->HasElement("nodeName") )
{
std::string node_name = _sdf->Get<std::string>("nodeName");
}
if (!ros::isInitialized())
{
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, node_name,
ros::init_options::NoSigintHandler);
}
this->rosNode.reset(new ros::NodeHandle(node_name));
if( _sdf->HasElement("otherClockTopic") )
{
std::string other_clock_topic = _sdf->Get<std::string>("otherClockTopic");
this->rosSub = this->rosNode->subscribe(other_clock_topic, 1, &WorldLockStepSync::onMsg, this);
} else {
std::cerr << "[WorldLockStepSync.cc] otherClockTopic tag not found.\n";
}
if( _sdf->HasElement("myClockTopic") )
{
std::string my_clock_topic = _sdf->Get<std::string>("myClockTopic");
// advertise(const std::string& topic, uint32_t queue_size, bool latch = false);
this->rosPub = this->rosNode->advertise<std_msgs::Float32>(my_clock_topic, 1, true);
} else {
std::cerr << "[WorldLockStepSync.cc] myClockTopic tag not found.\n";
}
std::cout << "[WorldLockStepSync.cc] WorldLockStepSync plugin Loaded.\n";
ros::AsyncSpinner spinner(2); // Use 2 threads
spinner.start(); // spin() will not return until the node has been shutdown
}
void WorldLockStepSync::onMsg(const std_msgs::Float32Ptr& msg)
{
// callback fcn is not being triggered
this->other_sim_time = msg->data;
WorldLockStepSync::compareTime();
}
void WorldLockStepSync::onUpdate()
{
this->my_sim_time = this->world->GetSimTime().sec;
WorldLockStepSync::compareTime();
}
void WorldLockStepSync::compareTime()
{
if((this->my_sim_time - this->last_run_time) >= this->dt / 1000000)
{
if(this->my_sim_time <= this->other_sim_time)
{
WorldLockStepSync::publishTime();
physics::pause_world(this->world, false);
this->last_run_time = this->my_sim_time;
} else {
if(!this->world->IsPaused()) {
WorldLockStepSync::publishTime();
}
physics::pause_world(this->world, true);
}
}
}
void WorldLockStepSync::publishTime()
{
std_msgs::Float32 msg;
msg.data = this->my_sim_time; //seconds float
this->rosPub.publish(msg);
}
}
Asked by kump on 2019-06-24 07:04:23 UTC
Comments