Subscriber Callback Function is not being triggered

edit

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);
    }

}