Problem running a modelplugin

edit

Hello, I am running this plugin: https://pastebin.com/HUUJFMnU 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()
    {
      ros::spinOnce();
    }

    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
  GZ_REGISTER_MODEL_PLUGIN(ROSModelPlugin)
}