diffdrive_plugin_6w.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00033 #include <algorithm>
00034 #include <assert.h>
00035 
00036 #include <hector_gazebo_plugins/diffdrive_plugin_6w.h>
00037 #include <gazebo/common/Events.hh>
00038 #include <gazebo/physics/physics.hh>
00039 
00040 #include <ros/ros.h>
00041 #include <tf/transform_broadcaster.h>
00042 #include <tf/transform_listener.h>
00043 #include <geometry_msgs/Twist.h>
00044 #include <nav_msgs/Odometry.h>
00045 #include <boost/bind.hpp>
00046 
00047 namespace gazebo {
00048 
00049 enum
00050 {
00051   FRONT_LEFT,
00052   FRONT_RIGHT,
00053   MID_LEFT,
00054   MID_RIGHT,
00055   REAR_LEFT,
00056   REAR_RIGHT,
00057   NUM_WHEELS
00058 };
00059 
00060 // Constructor
00061 DiffDrivePlugin6W::DiffDrivePlugin6W()
00062 {
00063 }
00064 
00065 // Destructor
00066 DiffDrivePlugin6W::~DiffDrivePlugin6W()
00067 {
00068   event::Events::DisconnectWorldUpdateStart(updateConnection);
00069   delete transform_broadcaster_;
00070   rosnode_->shutdown();
00071   callback_queue_thread_.join();
00072   delete rosnode_;
00073 }
00074 
00075 // Load the controller
00076 void DiffDrivePlugin6W::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00077 {
00078   world = _model->GetWorld();
00079 
00080   // load parameters
00081   if (!_sdf->HasElement("robotNamespace"))
00082     robotNamespace.clear();
00083   else
00084     robotNamespace = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00085 
00086   if (!_sdf->HasElement("topicName"))
00087     topicName = "cmd_vel";
00088   else
00089     topicName = _sdf->GetElement("topicName")->GetValueString();
00090 
00091   if (!_sdf->HasElement("bodyName"))
00092   {
00093     link = _model->GetLink();
00094     linkName = link->GetName();
00095   }
00096   else {
00097     linkName = _sdf->GetElement("bodyName")->GetValueString();
00098     link = _model->GetLink(linkName);
00099   }
00100 
00101   // assert that the body by linkName exists
00102   if (!link)
00103   {
00104     ROS_FATAL("DiffDrivePlugin6W plugin error: bodyName: %s does not exist\n", linkName.c_str());
00105     return;
00106   }
00107 
00108   if (_sdf->HasElement("frontLeftJoint"))  joints[FRONT_LEFT]  = _model->GetJoint(_sdf->GetElement("frontLeftJoint")->GetValueString());
00109   if (_sdf->HasElement("frontRightJoint")) joints[FRONT_RIGHT] = _model->GetJoint(_sdf->GetElement("frontRightJoint")->GetValueString());
00110   if (_sdf->HasElement("midLeftJoint"))    joints[MID_LEFT]    = _model->GetJoint(_sdf->GetElement("midLeftJoint")->GetValueString());
00111   if (_sdf->HasElement("midRightJoint"))   joints[MID_RIGHT]   = _model->GetJoint(_sdf->GetElement("midRightJoint")->GetValueString());
00112   if (_sdf->HasElement("rearLeftJoint"))   joints[REAR_LEFT]   = _model->GetJoint(_sdf->GetElement("rearLeftJoint")->GetValueString());
00113   if (_sdf->HasElement("rearRightJoint"))  joints[REAR_RIGHT]  = _model->GetJoint(_sdf->GetElement("rearRightJoint")->GetValueString());
00114 
00115   if (!joints[FRONT_LEFT])  ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get front left joint");
00116   if (!joints[FRONT_RIGHT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get front right joint");
00117   if (!joints[MID_LEFT])    ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get mid left joint");
00118   if (!joints[MID_RIGHT])   ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get mid right joint");
00119   if (!joints[REAR_LEFT])   ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get rear left joint");
00120   if (!joints[REAR_RIGHT])  ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get rear right joint");
00121 
00122   if (!_sdf->HasElement("wheelSeparation"))
00123     wheelSep = 0.34;
00124   else
00125     wheelSep = _sdf->GetElement("wheelSeparation")->GetValueDouble();
00126 
00127   if (!_sdf->HasElement("wheelDiameter"))
00128     wheelDiam = 0.15;
00129   else
00130     wheelDiam = _sdf->GetElement("wheelDiameter")->GetValueDouble();
00131 
00132   if (!_sdf->HasElement("torque"))
00133     torque = 10.0;
00134   else
00135     torque = _sdf->GetElement("torque")->GetValueDouble();
00136 
00137   // start ros node
00138   if (!ros::isInitialized())
00139   {
00140     int argc = 0;
00141     char** argv = NULL;
00142     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00143   }
00144 
00145   rosnode_ = new ros::NodeHandle(robotNamespace);
00146 
00147   tf_prefix_ = tf::getPrefixParam(*rosnode_);
00148   transform_broadcaster_ = new tf::TransformBroadcaster();
00149 
00150   // ROS: Subscribe to the velocity command topic (usually "cmd_vel")
00151   ros::SubscribeOptions so =
00152       ros::SubscribeOptions::create<geometry_msgs::Twist>(topicName, 1,
00153                                                           boost::bind(&DiffDrivePlugin6W::cmdVelCallback, this, _1),
00154                                                           ros::VoidPtr(), &queue_);
00155   sub_ = rosnode_->subscribe(so);
00156   pub_ = rosnode_->advertise<nav_msgs::Odometry>("odom", 1);
00157 
00158   callback_queue_thread_ = boost::thread(boost::bind(&DiffDrivePlugin6W::QueueThread, this));
00159 
00160   Reset();
00161 
00162   // New Mechanism for Updating every World Cycle
00163   // Listen to the update event. This event is broadcast every
00164   // simulation iteration.
00165   updateConnection = event::Events::ConnectWorldUpdateStart(
00166       boost::bind(&DiffDrivePlugin6W::Update, this));
00167 }
00168 
00169 // Initialize the controller
00170 void DiffDrivePlugin6W::Reset()
00171 {
00172   enableMotors = true;
00173 
00174   for (size_t i = 0; i < 2; ++i){
00175     wheelSpeed[i] = 0;
00176   }
00177 
00178   prevUpdateTime = world->GetSimTime();
00179 
00180   x_ = 0;
00181   rot_ = 0;
00182   alive_ = true;
00183 
00184   // Reset odometric pose
00185   odomPose[0] = 0.0;
00186   odomPose[1] = 0.0;
00187   odomPose[2] = 0.0;
00188 
00189   odomVel[0] = 0.0;
00190   odomVel[1] = 0.0;
00191   odomVel[2] = 0.0;
00192 }
00193 
00194 // Update the controller
00195 void DiffDrivePlugin6W::Update()
00196 {
00197   // TODO: Step should be in a parameter of this function
00198   double d1, d2;
00199   double dr, da;
00200   common::Time stepTime;
00201 
00202   GetPositionCmd();
00203 
00204   //stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime();
00205   stepTime = world->GetSimTime() - prevUpdateTime;
00206   prevUpdateTime = world->GetSimTime();
00207 
00208   // Distance travelled by front wheels
00209   d1 = stepTime.Double() * wheelDiam / 2 * joints[MID_LEFT]->GetVelocity(0);
00210   d2 = stepTime.Double() * wheelDiam / 2 * joints[MID_RIGHT]->GetVelocity(0);
00211 
00212   dr = (d1 + d2) / 2;
00213   da = (d1 - d2) / wheelSep;
00214 
00215   // Compute odometric pose
00216   odomPose[0] += dr * cos(odomPose[2]);
00217   odomPose[1] += dr * sin(odomPose[2]);
00218   odomPose[2] += da;
00219 
00220   // Compute odometric instantaneous velocity
00221   odomVel[0] = dr / stepTime.Double();
00222   odomVel[1] = 0.0;
00223   odomVel[2] = da / stepTime.Double();
00224 
00225   if (enableMotors)
00226   {
00227     joints[FRONT_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));
00228     joints[MID_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));
00229     joints[REAR_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));
00230 
00231     joints[FRONT_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));
00232     joints[MID_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));
00233     joints[REAR_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));
00234 
00235     joints[FRONT_LEFT]->SetMaxForce(0, torque);
00236     joints[MID_LEFT]->SetMaxForce(0, torque);
00237     joints[REAR_LEFT]->SetMaxForce(0, torque);
00238 
00239     joints[FRONT_RIGHT]->SetMaxForce(0, torque);
00240     joints[MID_RIGHT]->SetMaxForce(0, torque);
00241     joints[REAR_RIGHT]->SetMaxForce(0, torque);
00242   }
00243 
00244   //publish_odometry();
00245 }
00246 
00247 // NEW: Now uses the target velocities from the ROS message, not the Iface 
00248 void DiffDrivePlugin6W::GetPositionCmd()
00249 {
00250   lock.lock();
00251 
00252   double vr, va;
00253 
00254   vr = x_; //myIface->data->cmdVelocity.pos.x;
00255   va = -rot_; //myIface->data->cmdVelocity.yaw;
00256 
00257   //std::cout << "X: [" << x_ << "] ROT: [" << rot_ << "]" << std::endl;
00258 
00259   // Changed motors to be always on, which is probably what we want anyway
00260   enableMotors = true; //myIface->data->cmdEnableMotors > 0;
00261 
00262   //std::cout << enableMotors << std::endl;
00263 
00264   wheelSpeed[0] = vr + va * wheelSep / 2;
00265   wheelSpeed[1] = vr - va * wheelSep / 2;
00266 
00267   lock.unlock();
00268 }
00269 
00270 // NEW: Store the velocities from the ROS message
00271 void DiffDrivePlugin6W::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg)
00272 {
00273   //std::cout << "BEGIN CALLBACK\n";
00274 
00275   lock.lock();
00276 
00277   x_ = cmd_msg->linear.x;
00278   rot_ = cmd_msg->angular.z;
00279 
00280   lock.unlock();
00281 
00282   //std::cout << "END CALLBACK\n";
00283 }
00284 
00285 // NEW: custom callback queue thread
00286 void DiffDrivePlugin6W::QueueThread()
00287 {
00288   static const double timeout = 0.01;
00289 
00290   while (alive_ && rosnode_->ok())
00291   {
00292     //    std::cout << "CALLING STUFF\n";
00293     queue_.callAvailable(ros::WallDuration(timeout));
00294   }
00295 }
00296 
00297 // NEW: Update this to publish odometry topic
00298 void DiffDrivePlugin6W::publish_odometry()
00299 {
00300   // get current time
00301   ros::Time current_time_((world->GetSimTime()).sec, (world->GetSimTime()).nsec); 
00302 
00303   // getting data for base_footprint to odom transform
00304   math::Pose pose = link->GetWorldPose();
00305   math::Vector3 velocity = link->GetWorldLinearVel();
00306   math::Vector3 angular_velocity = link->GetWorldAngularVel();
00307 
00308   tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
00309   tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z);
00310   tf::Transform base_footprint_to_odom(qt, vt);
00311 
00312   transform_broadcaster_->sendTransform(tf::StampedTransform(base_footprint_to_odom,
00313                                                             current_time_,
00314                                                             "odom",
00315                                                             "base_footprint"));
00316 
00317   // publish odom topic
00318   odom_.pose.pose.position.x = pose.pos.x;
00319   odom_.pose.pose.position.y = pose.pos.y;
00320 
00321   odom_.pose.pose.orientation.x = pose.rot.x;
00322   odom_.pose.pose.orientation.y = pose.rot.y;
00323   odom_.pose.pose.orientation.z = pose.rot.z;
00324   odom_.pose.pose.orientation.w = pose.rot.w;
00325 
00326   odom_.twist.twist.linear.x = velocity.x;
00327   odom_.twist.twist.linear.y = velocity.y;
00328   odom_.twist.twist.angular.z = angular_velocity.z;
00329 
00330   odom_.header.frame_id = tf::resolve(tf_prefix_, "odom");
00331   odom_.child_frame_id = "base_footprint";
00332   odom_.header.stamp = current_time_;
00333 
00334   pub_.publish(odom_);
00335 }
00336 
00337 GZ_REGISTER_MODEL_PLUGIN(DiffDrivePlugin6W)
00338 
00339 } // namespace gazebo
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher and Johannes Meyer
autogenerated on Mon Jul 15 2013 16:46:30