createTimer doesn't seem to be calling the callbacks

edit

Hello everyone,

Specs : Ubuntu 16.04, ROS Kinetic + Gazebo 7.16

I'm trying to publish the joint state of my robot to a ROS topic from a Gazebo Plugin. To do so I first used a feature of Gazebo API (callback on WorldUpdate event), which works fine. But I want to have more control on the rate of publication (custom frequency instead of every iteration of the simulator), so the easiest way appears to be using a ros::Timer. The thing is that it just doesn't seem to work, as there is nothing published on the desired topic.

I've tried tinkering with rosparam set /use_sim_time false with no success.

Find an excerpt of my code below, it compiles just fine and runs when Gazebo loads the plugin, but impossible to get the callbacks. I don't see why it shouldn't work, and I lack ROS/Gazebo debugging skills. Any input will be appreciated.

#ifndef _LIDAR_3D_PLUGIN_HH_
#define _LIDAR_3D_PLUGIN_HH_

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include "ros/ros.h"
#include "std_msgs/Float32.h"
#include "sensor_msgs/JointState.h"

namespace gazebo
{
    class Lidar3DPlugin: public ModelPlugin
    {
        private :
            physics::ModelPtr model;
            std::vector<physics::JointPtr> joints;
            physics::JointControllerPtr joint_ctrl_ptr;
            ros::NodeHandle *rosNode;
            ros::Subscriber rosSub[2];
            ros::Publisher rosPub;
            sensor_msgs::JointState jointStateMsg;

            void OnRosMsg(const std_msgs::Float32ConstPtr &_msg, std::string name)
            {
                this->joint_ctrl_ptr->SetPositionTarget(name, _msg->data);
            }

            void OnUpdate(const ros::TimerEvent &_timer)
            {
                this->jointStateMsg.position[0] = (float)(this->joints[0]->GetAngle(0).Radian());
                this->jointStateMsg.position[1] = (float)(this->joints[1]->GetAngle(0).Radian());

                this->jointStateMsg.velocity[0] = (float)(this->joints[0]->GetVelocity(0));
                this->jointStateMsg.velocity[1] = (float)(this->joints[1]->GetVelocity(0));

                this->rosPub.publish(this->jointStateMsg);
            }

        public : 
            Lidar3DPlugin(){}

            virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
            {
                [...]

                int argc = 0;
                char** argv = NULL;
                ros::init(argc,argv,"lidar_3d", ros::init_options::NoSigintHandler);

                this->rosNode = new ros::NodeHandle("lidar_3d");

                [...]
                std::string joint_topic = "/"+this->model->GetName()+"/joint_state";
                this->rosPub = this->rosNode->advertise<sensor_msgs::JointState>(joint_topic,10);

                this->rosNode->createTimer(ros::Duration(0.04),&Lidar3DPlugin::OnUpdate,this);

            } 
    };
    GZ_REGISTER_MODEL_PLUGIN(Lidar3DPlugin)
}

#endif