Advertisement
Guest User

ros_model_plugin.cc

a guest
Feb 23rd, 2013
501
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 2.64 KB | None | 0 0
  1. #include <boost/bind.hpp>
  2. #include <gazebo.hh>
  3. #include <physics/physics.hh>
  4. #include <common/common.hh>
  5. #include <stdio.h>
  6.  
  7. #include "ros/ros.h"
  8. #include "std_msgs/Float64.h"
  9. #include "geometry_msgs/Twist.h"
  10.  
  11. namespace gazebo
  12. {  
  13.   class ROSModelPlugin : public ModelPlugin
  14.   {
  15.  
  16.     public: ROSModelPlugin()
  17.     {
  18.  
  19.       // Start up ROS
  20.       std::string name = "gazebo_p3at";
  21.       int argc = 0;
  22.       ros::init(argc, NULL, name);
  23.  
  24.     }
  25.     public: ~ROSModelPlugin()
  26.     {
  27.       delete this->node;
  28.     }
  29.  
  30.     public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
  31.     {
  32.       // Store the pointer to the model
  33.       this->model = _parent;
  34.  
  35.       // ROS Nodehandle
  36.       this->node = new ros::NodeHandle("~");
  37.  
  38.       // ROS Subscriber
  39.       this->sub = this->node->subscribe<geometry_msgs::Twist>("cmd_vel", 10, &ROSModelPlugin::ROSCallback, this );
  40.  
  41.       // Listen to the update event. This event is broadcast every
  42.       // simulation iteration.
  43.       this->updateConnection = event::Events::ConnectWorldUpdateStart(
  44.           boost::bind(&ROSModelPlugin::OnUpdate, this));
  45.     }
  46.  
  47.     // Called by the world update start event
  48.     public: void OnUpdate()
  49.     {
  50.       ros::spinOnce();
  51.     }
  52.  
  53.     void ROSCallback(const boost::shared_ptr<geometry_msgs::Twist const>& cmd_vel)
  54.     {
  55.                                              
  56.       physics::Joint_V joints = this->model->GetJoints();
  57.      
  58.       float maxForce = 3.0;
  59.       joints[0]->SetMaxForce(0, maxForce);
  60.       joints[1]->SetMaxForce(0, maxForce);
  61.       joints[2]->SetMaxForce(0, maxForce);
  62.       joints[3]->SetMaxForce(0, maxForce);
  63.      
  64.       float wheelRadius = 0.110; // (Meters)
  65.       float wheelBase   = 0.250; // Distance between opposite wheels (Meters)
  66.       float RotVel_lin = cmd_vel->linear.x / wheelRadius;
  67.       float RotVel_rot = cmd_vel->angular.z * (wheelBase / wheelRadius);
  68.      
  69.       joints[0]->SetVelocity(0, RotVel_lin - RotVel_rot);
  70.       joints[1]->SetVelocity(0, RotVel_lin - RotVel_rot);
  71.       joints[2]->SetVelocity(0, RotVel_lin + RotVel_rot);
  72.       joints[3]->SetVelocity(0, RotVel_lin + RotVel_rot);
  73.      
  74.       ROS_INFO("cmd_vel: [%f, %f]"
  75.               , cmd_vel->linear.x  
  76.               , cmd_vel->angular.z );
  77.     }
  78.  
  79.     // Pointer to the model
  80.     private: physics::ModelPtr model;
  81.  
  82.     // Pointer to the update event connection
  83.     private: event::ConnectionPtr updateConnection;
  84.  
  85.     // ROS Nodehandle
  86.     private: ros::NodeHandle* node;
  87.  
  88.     // ROS Subscriber
  89.     ros::Subscriber sub;
  90.    
  91.   };
  92.  
  93.   // Register this plugin with the simulator
  94.   GZ_REGISTER_MODEL_PLUGIN(ROSModelPlugin)
  95. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement