Problem running a modelplugin

asked 2017-07-05 14:59:12 -0500 gravatar image

updated 2017-07-05 15:03:04 -0500

sloretz gravatar image

Hello, I am running this plugin: but my joints are not moving. I tried to put a ROS_INFO message into each function to see if they are printed. Load function message is printed once, Onupdate function message is printed endlessly and RosCallback function message never gets printed. Why is this happening? What should I change?

#include <boost/bind.hpp>
#include <gazebo.hh>
#include <physics/physics.hh>
#include <common/common.hh>
#include <stdio.h>

#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "geometry_msgs/Twist.h"

namespace gazebo
  class ROSModelPlugin : public ModelPlugin

    public: ROSModelPlugin()

      // Start up ROS
      std::string name = "gazebo_p3at";
      int argc = 0;
      ros::init(argc, NULL, name);

    public: ~ROSModelPlugin()
      delete this->node;

    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
      // Store the pointer to the model
      this->model = _parent;

      // ROS Nodehandle
      this->node = new ros::NodeHandle("~");

      // ROS Subscriber
      this->sub = this->node->subscribe<geometry_msgs::Twist>("cmd_vel", 10, &ROSModelPlugin::ROSCallback, this );

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateStart(
          boost::bind(&ROSModelPlugin::OnUpdate, this));

    // Called by the world update start event
    public: void OnUpdate()

    void ROSCallback(const boost::shared_ptr<geometry_msgs::Twist const>& cmd_vel)

      physics::Joint_V joints = this->model->GetJoints();

      float maxForce = 3.0;
      joints[0]->SetMaxForce(0, maxForce);
      joints[1]->SetMaxForce(0, maxForce);
      joints[2]->SetMaxForce(0, maxForce);
      joints[3]->SetMaxForce(0, maxForce);

      float wheelRadius = 0.110; // (Meters)
      float wheelBase   = 0.250; // Distance between opposite wheels (Meters)
      float RotVel_lin = cmd_vel->linear.x / wheelRadius;
      float RotVel_rot = cmd_vel->angular.z * (wheelBase / wheelRadius);

      joints[0]->SetVelocity(0, RotVel_lin - RotVel_rot);
      joints[1]->SetVelocity(0, RotVel_lin - RotVel_rot);
      joints[2]->SetVelocity(0, RotVel_lin + RotVel_rot);
      joints[3]->SetVelocity(0, RotVel_lin + RotVel_rot);

      ROS_INFO("cmd_vel: [%f, %f]"
              , cmd_vel->linear.x  
              , cmd_vel->angular.z );

    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    // ROS Nodehandle
    private: ros::NodeHandle* node;

    // ROS Subscriber
    ros::Subscriber sub;


  // Register this plugin with the simulator
edit retag flag offensive close merge delete


Hope you don't mind, I added the text of the plugin to the question body in case the pastebin link breaks in the future.

sloretz gravatar imagesloretz ( 2017-07-05 15:04:02 -0500 )edit