Diff.cc

#include <gazebo/transport/transport.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/gazebo_client.hh>
#include <gazebo/common/common.hh>
#include <gazebo/math/gzmath.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo.hh>
#include <boost/bind.hpp>
#include <stdio.h>

namespace gazebo
{
    enum {LEFT, RIGHT};

    class Diff : public ModelPlugin
    {
        /////////////////////////////////////////////////////////////
        // Constructor
        public: Diff() : ModelPlugin()
        {
            this->wheelSpeed[LEFT] = this->wheelSpeed[RIGHT] = 0;
            this->wheelSeparation = 1.2;
            this->wheelRadius = 0.125;
        }

        /////////////////////////////////////////////////////////////
        // Load function
        public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
        {
            this->model = _model;

            // Create a node for listening target pos
            this->node = transport::NodePtr(new transport::Node());
            this->node->Init("default"); // World name

            // Subscribe to the topic 'targetPose'
            this->PoseSub = this->node->Subscribe("~/my_robot/targetPose", &Diff::OnPoseMsg, this);

            // Check joints
            if (!_sdf->HasElement("left_wheel_hinge"))
                gzerr << "Diff plugin missing <left_joint> element\n";

          if (!_sdf->HasElement("right_wheel_hinge"))
                gzerr << "Diff plugin missing <right_joint> element\n";

          this->rightJoint = _model->GetJoint(std::string("right_joint"));
      this->leftJoint = _model->GetJoint(std::string("left_joint"));

      // Find the joints
      if (!this->rightJoint)
        gzerr << "Unable to find right joint["
                    << _sdf->GetElement("right_joint")->Get<std::string>() << "]\n";

      if (!this->leftJoint)
        gzerr << "Unable to find left joint["
                    << _sdf->GetElement("left_joint")->Get<std::string>() << "]\n";

            this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                            boost::bind(&Diff::OnUpdate, this));
        }

        /////////////////////////////////////////////////////////////////
        // Initialize
    public: virtual void Init()
        {

        }

        /////////////////////////////////////////////////////////////////
        // On recieving Msg
    private: void OnPoseMsg(ConstPosePtr &_msg)
        {
            float vc, va;

            // Calculatons to find vr and va

            this->wheelSpeed[LEFT] = vc + va * this->wheelSeparation / 2.0;
        this->wheelSpeed[RIGHT] = vc - va * this->wheelSeparation / 2.0;
    }

    /////////////////////////////////////////////////////////////////
    // On update
    private: void OnUpdate()
    {
        double leftVelDesired = (this->wheelSpeed[LEFT] / this->wheelRadius);
          double rightVelDesired = (this->wheelSpeed[RIGHT] / this->wheelRadius);

          this->leftJoint->SetVelocity(0, leftVelDesired);
          this->rightJoint->SetVelocity(0, rightVelDesired);
    }

    private: transport::NodePtr node;
    private: transport::SubscriberPtr PoseSub;

    private: physics::ModelPtr model;
    private: physics::JointPtr leftJoint, rightJoint;
    private: event::ConnectionPtr updateConnection;
    private: double wheelSpeed[2];
    private: double wheelSeparation;
    private: double wheelRadius;
    private: common::Time prevUpdateTime;

    private: physics::LinkPtr link, leftWheelLink, rightWheelLink;

    };

    // Register the plugin
  GZ_REGISTER_MODEL_PLUGIN(Diff)

}