Ledger Nano X - The secure hardware wallet
SHARE
TWEET

DiffDrive.cc

anandvgeorge Dec 17th, 2015 (edited) 15 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*
  2.  * Copyright (C) 2012-2015 Open Source Robotics Foundation
  3.  *
  4.  * Licensed under the Apache License, Version 2.0 (the "License");
  5.  * you may not use this file except in compliance with the License.
  6.  * You may obtain a copy of the License at
  7.  *
  8.  *     http://www.apache.org/licenses/LICENSE-2.0
  9.  *
  10.  * Unless required by applicable law or agreed to in writing, software
  11.  * distributed under the License is distributed on an "AS IS" BASIS,
  12.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13.  * See the License for the specific language governing permissions and
  14.  * limitations under the License.
  15.  *
  16. */
  17.  
  18. // ALL THE MEASUREMENTS ARE OF Pinoneer2DX
  19.  
  20. #include <gazebo/transport/transport.hh>
  21. #include "gazebo/util/system.hh"
  22. #include <gazebo/math/gzmath.hh>
  23. #include <gazebo/gazebo_client.hh>
  24. #include <boost/bind.hpp>
  25. #include <gazebo/gazebo.hh>
  26. #include <gazebo/physics/physics.hh>
  27. #include <gazebo/common/common.hh>
  28. #include <stdio.h>
  29.  
  30. enum {RIGHT, LEFT};
  31.  
  32. namespace gazebo
  33. {
  34.   class DiffDrive : public ModelPlugin
  35.   {
  36.     public: DiffDrive() : ModelPlugin()
  37.     {
  38.       this->wheelSpeed[LEFT] = this->wheelSpeed[RIGHT] = 0;
  39.       this->wheelSeparation = 0.34;
  40.       this->wheelRadius = 0.11;
  41.     }
  42.     public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
  43.     public: virtual void Init();
  44.  
  45.     private: void OnUpdate();
  46.  
  47.     private: void OnVelMsg(ConstPosePtr &_msg);
  48.  
  49.     private: transport::NodePtr node;
  50.     private: transport::SubscriberPtr velSub;
  51.  
  52.     private: physics::ModelPtr model;
  53.     private: physics::JointPtr leftJoint, rightJoint;
  54.     private: event::ConnectionPtr updateConnection;
  55.     private: double wheelSpeed[2];
  56.     private: double wheelSeparation;
  57.     private: double wheelRadius;
  58.     private: common::Time prevUpdateTime;
  59.  
  60.     private: physics::LinkPtr link, leftWheelLink, rightWheelLink;
  61.   };
  62.  
  63.   GZ_REGISTER_MODEL_PLUGIN(DiffDrive)
  64. }
  65.  
  66.  
  67. /////////////////////////////////////////////////
  68. void gazebo::DiffDrive::Load(physics::ModelPtr _model,
  69.                            sdf::ElementPtr _sdf)
  70. {
  71.   this->model = _model;
  72.  
  73.   this->node = transport::NodePtr(new transport::Node());
  74.   this->node->Init(this->model->GetWorld()->GetName());
  75.  
  76.   this->velSub = this->node->Subscribe(std::string("~/") +
  77.       this->model->GetName() + "/vel_cmd", &DiffDrive::OnVelMsg, this);
  78.  
  79.   if (!_sdf->HasElement("left_joint"))
  80.     gzerr << "DiffDrive plugin missing <left_joint> element\n";
  81.  
  82.   if (!_sdf->HasElement("right_joint"))
  83.     gzerr << "DiffDrive plugin missing <right_joint> element\n";
  84.  
  85.   this->leftJoint = _model->GetJoint(
  86.       _sdf->GetElement("left_joint")->Get<std::string>());
  87.   this->rightJoint = _model->GetJoint(
  88.       _sdf->GetElement("right_joint")->Get<std::string>());
  89.  
  90.   if (!this->leftJoint)
  91.     gzerr << "Unable to find left joint["
  92.          << _sdf->GetElement("left_joint")->Get<std::string>() << "]\n";
  93.  if (!this->rightJoint)
  94.     gzerr << "Unable to find right joint["
  95.          << _sdf->GetElement("right_joint")->Get<std::string>() << "]\n";
  96.  
  97.  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
  98.           boost::bind(&DiffDrive::OnUpdate, this));
  99. }
  100.  
  101. /////////////////////////////////////////////////
  102. void gazebo::DiffDrive::Init()
  103. {
  104.   this->wheelSeparation = this->leftJoint->GetAnchor(0).Distance(
  105.       this->rightJoint->GetAnchor(0));
  106.  
  107.   physics::EntityPtr parent = boost::dynamic_pointer_cast<physics::Entity>(
  108.       this->leftJoint->GetChild());
  109.  
  110.   math::Box bb = parent->GetBoundingBox();
  111.   // This assumes that the largest dimension of the wheel is the diameter
  112.   this->wheelRadius = bb.GetSize().GetMax() * 0.5;
  113. }
  114.  
  115. /////////////////////////////////////////////////
  116. void gazebo::DiffDrive::OnVelMsg(ConstPosePtr &_msg)
  117. {
  118.  double vr, va;
  119.  
  120.   // My method to calculate wheel velocities
  121.  
  122.   // vr = _msg->position().x();
  123.   // va =  msgs::ConvertIgn(_msg->orientation()).Euler().Z();
  124.  
  125.   this->wheelSpeed[LEFT] = vr + va * this->wheelSeparation / 2.0;
  126.   this->wheelSpeed[RIGHT] = vr - va * this->wheelSeparation / 2.0;
  127. }
  128.  
  129. /////////////////////////////////////////////////
  130. void gazebo::DiffDrive::OnUpdate()
  131. {
  132.   /* double d1, d2;
  133.   double dr, da;
  134.  
  135.   this->prevUpdateTime = currTime;
  136.  
  137.   // Distance travelled by front wheels
  138.   d1 = stepTime.Double() * this->wheelRadius * this->leftJoint->GetVelocity(0);
  139.   d2 = stepTime.Double() * this->wheelRadius * this->rightJoint->GetVelocity(0);
  140.  
  141.   dr = (d1 + d2) / 2;
  142.   da = (d1 - d2) / this->wheelSeparation;
  143.   common::Time currTime = this->model->GetWorld()->GetSimTime();
  144.   common::Time stepTime = currTime - this->prevUpdateTime;
  145.   */
  146.  
  147.   double leftVelDesired = (this->wheelSpeed[LEFT] / this->wheelRadius);
  148.   double rightVelDesired = (this->wheelSpeed[RIGHT] / this->wheelRadius);
  149.  
  150.   this->leftJoint->SetVelocity(0, leftVelDesired);
  151.   this->rightJoint->SetVelocity(0, rightVelDesired);
  152. }
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