Guest User

ros_model_plugin.cc

a guest
Feb 23rd, 2013
314
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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. }
RAW Paste Data

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×