Home | Tutorials | Wiki | Issues
Ask Your Question
0

createTimer doesn't seem to be calling the callbacks

asked 2019-10-29 11:20:51 -0600

mxch_18 gravatar image

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
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-11-04 04:51:31 -0600

mxch_18 gravatar image

I found the answer, it's actually a really simple mistake. Creating the timer this way in the Load() method means that once Load() finishes, the timer will simply go out of scope. The documentation of createTimer indicates that this will simply stop and destroy the timer. Therefore, I only need to add a ros::Timer timer attribute to my class and replace

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

with

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

This way when Load() finishes, the timer stays in scope until Gazebo is closed and my plugin instance is automatically destroyed.

Best.

edit flag offensive delete link more
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2019-10-29 11:20:51 -0600

Seen: 20 times

Last updated: Nov 04