G2A Many GEOs
SHARE
TWEET

Diff.cc

anandvgeorge Dec 18th, 2015 16 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <gazebo/transport/transport.hh>
  2. #include <gazebo/physics/physics.hh>
  3. #include <gazebo/gazebo_client.hh>
  4. #include <gazebo/common/common.hh>
  5. #include <gazebo/math/gzmath.hh>
  6. #include <gazebo/msgs/msgs.hh>
  7. #include <gazebo/gazebo.hh>
  8. #include <boost/bind.hpp>
  9. #include <stdio.h>
  10.  
  11. namespace gazebo
  12. {
  13.     enum {LEFT, RIGHT};
  14.  
  15.     class Diff : public ModelPlugin
  16.     {
  17.         /////////////////////////////////////////////////////////////
  18.         // Constructor
  19.         public: Diff() : ModelPlugin()
  20.         {
  21.             this->wheelSpeed[LEFT] = this->wheelSpeed[RIGHT] = 0;
  22.             this->wheelSeparation = 1.2;
  23.             this->wheelRadius = 0.125;
  24.         }
  25.  
  26.         /////////////////////////////////////////////////////////////
  27.         // Load function
  28.         public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
  29.         {
  30.             this->model = _model;
  31.  
  32.             // Create a node for listening target pos
  33.             this->node = transport::NodePtr(new transport::Node());
  34.             this->node->Init("default"); // World name
  35.            
  36.             // Subscribe to the topic 'targetPose'
  37.             this->PoseSub = this->node->Subscribe("~/my_robot/targetPose", &Diff::OnPoseMsg, this);
  38.            
  39.             // Check joints
  40.             if (!_sdf->HasElement("left_wheel_hinge"))
  41.                 gzerr << "Diff plugin missing <left_joint> element\n";
  42.  
  43.           if (!_sdf->HasElement("right_wheel_hinge"))
  44.                 gzerr << "Diff plugin missing <right_joint> element\n";
  45.  
  46.           this->rightJoint = _model->GetJoint(std::string("right_joint"));
  47.       this->leftJoint = _model->GetJoint(std::string("left_joint"));
  48.  
  49.       // Find the joints
  50.       if (!this->rightJoint)
  51.         gzerr << "Unable to find right joint["
  52.                     << _sdf->GetElement("right_joint")->Get<std::string>() << "]\n";
  53.  
  54.       if (!this->leftJoint)
  55.         gzerr << "Unable to find left joint["
  56.                     << _sdf->GetElement("left_joint")->Get<std::string>() << "]\n";
  57.  
  58.             this->updateConnection = event::Events::ConnectWorldUpdateBegin(
  59.                             boost::bind(&Diff::OnUpdate, this));       
  60.         }
  61.  
  62.         /////////////////////////////////////////////////////////////////
  63.         // Initialize
  64.     public: virtual void Init()
  65.         {
  66.  
  67.         }
  68.  
  69.         /////////////////////////////////////////////////////////////////
  70.         // On recieving Msg
  71.     private: void OnPoseMsg(ConstPosePtr &_msg)
  72.         {
  73.             float vc, va;
  74.  
  75.             // Calculatons to find vr and va
  76.  
  77.             this->wheelSpeed[LEFT] = vc + va * this->wheelSeparation / 2.0;
  78.         this->wheelSpeed[RIGHT] = vc - va * this->wheelSeparation / 2.0;
  79.     }
  80.  
  81.     /////////////////////////////////////////////////////////////////
  82.     // On update
  83.     private: void OnUpdate()
  84.     {
  85.         double leftVelDesired = (this->wheelSpeed[LEFT] / this->wheelRadius);
  86.           double rightVelDesired = (this->wheelSpeed[RIGHT] / this->wheelRadius);
  87.  
  88.           this->leftJoint->SetVelocity(0, leftVelDesired);
  89.           this->rightJoint->SetVelocity(0, rightVelDesired);
  90.     }
  91.  
  92.     private: transport::NodePtr node;
  93.     private: transport::SubscriberPtr PoseSub;
  94.  
  95.     private: physics::ModelPtr model;
  96.     private: physics::JointPtr leftJoint, rightJoint;
  97.     private: event::ConnectionPtr updateConnection;
  98.     private: double wheelSpeed[2];
  99.     private: double wheelSeparation;
  100.     private: double wheelRadius;
  101.     private: common::Time prevUpdateTime;
  102.  
  103.     private: physics::LinkPtr link, leftWheelLink, rightWheelLink;
  104.  
  105.     };
  106.  
  107.     // Register the plugin
  108.   GZ_REGISTER_MODEL_PLUGIN(Diff)
  109.  
  110. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top