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 #include <gazebo/gazebo_config.h>
00048 
00049 namespace gazebo {
00050 
00051 enum
00052 {
00053   FRONT_LEFT,
00054   FRONT_RIGHT,
00055   MID_LEFT,
00056   MID_RIGHT,
00057   REAR_LEFT,
00058   REAR_RIGHT,
00059   NUM_WHEELS
00060 };
00061 
00062 // Constructor
00063 DiffDrivePlugin6W::DiffDrivePlugin6W()
00064 {
00065 }
00066 
00067 // Destructor
00068 DiffDrivePlugin6W::~DiffDrivePlugin6W()
00069 {
00070   event::Events::DisconnectWorldUpdateBegin(updateConnection);
00071   delete transform_broadcaster_;
00072   rosnode_->shutdown();
00073   callback_queue_thread_.join();
00074   delete rosnode_;
00075 }
00076 
00077 // Load the controller
00078 void DiffDrivePlugin6W::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00079 {
00080   world = _model->GetWorld();
00081 
00082   // default parameters
00083   namespace_.clear();
00084   topic_ = "cmd_vel";
00085   wheelSep = 0.34;
00086   wheelDiam = 0.15;
00087   torque = 10.0;
00088 
00089   // load parameters
00090   if (_sdf->HasElement("robotNamespace"))
00091     namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
00092 
00093   if (_sdf->HasElement("topicName"))
00094     topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();
00095 
00096   if (_sdf->HasElement("bodyName"))
00097   {
00098     link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
00099     link = _model->GetLink(link_name_);
00100   } else {
00101     link = _model->GetLink();
00102     link_name_ = link->GetName();
00103   }
00104 
00105   // assert that the body by link_name_ exists
00106   if (!link)
00107   {
00108     ROS_FATAL("DiffDrivePlugin6W plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00109     return;
00110   }
00111 
00112   if (_sdf->HasElement("frontLeftJoint"))  joints[FRONT_LEFT]  = _model->GetJoint(_sdf->GetElement("frontLeftJoint")->GetValue()->GetAsString());
00113   if (_sdf->HasElement("frontRightJoint")) joints[FRONT_RIGHT] = _model->GetJoint(_sdf->GetElement("frontRightJoint")->GetValue()->GetAsString());
00114   if (_sdf->HasElement("midLeftJoint"))    joints[MID_LEFT]    = _model->GetJoint(_sdf->GetElement("midLeftJoint")->GetValue()->GetAsString());
00115   if (_sdf->HasElement("midRightJoint"))   joints[MID_RIGHT]   = _model->GetJoint(_sdf->GetElement("midRightJoint")->GetValue()->GetAsString());
00116   if (_sdf->HasElement("rearLeftJoint"))   joints[REAR_LEFT]   = _model->GetJoint(_sdf->GetElement("rearLeftJoint")->GetValue()->GetAsString());
00117   if (_sdf->HasElement("rearRightJoint"))  joints[REAR_RIGHT]  = _model->GetJoint(_sdf->GetElement("rearRightJoint")->GetValue()->GetAsString());
00118 
00119   if (!joints[FRONT_LEFT])  ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get front left joint");
00120   if (!joints[FRONT_RIGHT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get front right joint");
00121   if (!joints[MID_LEFT])    ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get mid left joint");
00122   if (!joints[MID_RIGHT])   ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get mid right joint");
00123   if (!joints[REAR_LEFT])   ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get rear left joint");
00124   if (!joints[REAR_RIGHT])  ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get rear right joint");
00125 
00126   if (_sdf->HasElement("wheelSeparation"))
00127     _sdf->GetElement("wheelSeparation")->GetValue()->Get(wheelSep);
00128 
00129   if (_sdf->HasElement("wheelDiameter"))
00130     _sdf->GetElement("wheelDiameter")->GetValue()->Get(wheelDiam);
00131 
00132   if (_sdf->HasElement("torque"))
00133     _sdf->GetElement("torque")->GetValue()->Get(torque);
00134 
00135   // Make sure the ROS node for Gazebo has already been initialized
00136   if (!ros::isInitialized())
00137   {
00138     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00139       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00140     return;
00141   }
00142 
00143   rosnode_ = new ros::NodeHandle(namespace_);
00144 
00145   tf_prefix_ = tf::getPrefixParam(*rosnode_);
00146   transform_broadcaster_ = new tf::TransformBroadcaster();
00147 
00148   // ROS: Subscribe to the velocity command topic (usually "cmd_vel")
00149   ros::SubscribeOptions so =
00150       ros::SubscribeOptions::create<geometry_msgs::Twist>(topic_, 1,
00151                                                           boost::bind(&DiffDrivePlugin6W::cmdVelCallback, this, _1),
00152                                                           ros::VoidPtr(), &queue_);
00153   sub_ = rosnode_->subscribe(so);
00154   pub_ = rosnode_->advertise<nav_msgs::Odometry>("odom", 1);
00155 
00156   callback_queue_thread_ = boost::thread(boost::bind(&DiffDrivePlugin6W::QueueThread, this));
00157 
00158   Reset();
00159 
00160   // New Mechanism for Updating every World Cycle
00161   // Listen to the update event. This event is broadcast every
00162   // simulation iteration.
00163   updateConnection = event::Events::ConnectWorldUpdateBegin(
00164       boost::bind(&DiffDrivePlugin6W::Update, this));
00165 }
00166 
00167 // Initialize the controller
00168 void DiffDrivePlugin6W::Reset()
00169 {
00170   enableMotors = true;
00171 
00172   for (size_t i = 0; i < 2; ++i){
00173     wheelSpeed[i] = 0;
00174   }
00175 
00176   prevUpdateTime = world->GetSimTime();
00177 
00178   x_ = 0;
00179   rot_ = 0;
00180   alive_ = true;
00181 
00182   // Reset odometric pose
00183   odomPose[0] = 0.0;
00184   odomPose[1] = 0.0;
00185   odomPose[2] = 0.0;
00186 
00187   odomVel[0] = 0.0;
00188   odomVel[1] = 0.0;
00189   odomVel[2] = 0.0;
00190 }
00191 
00192 // Update the controller
00193 void DiffDrivePlugin6W::Update()
00194 {
00195   // TODO: Step should be in a parameter of this function
00196   double d1, d2;
00197   double dr, da;
00198   common::Time stepTime;
00199 
00200   GetPositionCmd();
00201 
00202   //stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime();
00203   stepTime = world->GetSimTime() - prevUpdateTime;
00204   prevUpdateTime = world->GetSimTime();
00205 
00206   // Distance travelled by front wheels
00207   d1 = stepTime.Double() * wheelDiam / 2 * joints[MID_LEFT]->GetVelocity(0);
00208   d2 = stepTime.Double() * wheelDiam / 2 * joints[MID_RIGHT]->GetVelocity(0);
00209 
00210   dr = (d1 + d2) / 2;
00211   da = (d1 - d2) / wheelSep;
00212 
00213   // Compute odometric pose
00214   odomPose[0] += dr * cos(odomPose[2]);
00215   odomPose[1] += dr * sin(odomPose[2]);
00216   odomPose[2] += da;
00217 
00218   // Compute odometric instantaneous velocity
00219   odomVel[0] = dr / stepTime.Double();
00220   odomVel[1] = 0.0;
00221   odomVel[2] = da / stepTime.Double();
00222 
00223   if (enableMotors)
00224   {
00225     joints[FRONT_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));
00226     joints[MID_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));
00227     joints[REAR_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));
00228 
00229     joints[FRONT_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));
00230     joints[MID_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));
00231     joints[REAR_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));
00232 
00233 #if (GAZEBO_MAJOR_VERSION > 4)
00234     joints[FRONT_LEFT]->SetEffortLimit(0, torque);
00235     joints[MID_LEFT]->SetEffortLimit(0, torque);
00236     joints[REAR_LEFT]->SetEffortLimit(0, torque);
00237 
00238     joints[FRONT_RIGHT]->SetEffortLimit(0, torque);
00239     joints[MID_RIGHT]->SetEffortLimit(0, torque);
00240     joints[REAR_RIGHT]->SetEffortLimit(0, torque);
00241 #else
00242     joints[FRONT_LEFT]->SetMaxForce(0, torque);
00243     joints[MID_LEFT]->SetMaxForce(0, torque);
00244     joints[REAR_LEFT]->SetMaxForce(0, torque);
00245 
00246     joints[FRONT_RIGHT]->SetMaxForce(0, torque);
00247     joints[MID_RIGHT]->SetMaxForce(0, torque);
00248     joints[REAR_RIGHT]->SetMaxForce(0, torque);
00249 #endif
00250   }
00251 
00252   //publish_odometry();
00253 }
00254 
00255 // NEW: Now uses the target velocities from the ROS message, not the Iface 
00256 void DiffDrivePlugin6W::GetPositionCmd()
00257 {
00258   lock.lock();
00259 
00260   double vr, va;
00261 
00262   vr = x_; //myIface->data->cmdVelocity.pos.x;
00263   va = -rot_; //myIface->data->cmdVelocity.yaw;
00264 
00265   //std::cout << "X: [" << x_ << "] ROT: [" << rot_ << "]" << std::endl;
00266 
00267   // Changed motors to be always on, which is probably what we want anyway
00268   enableMotors = true; //myIface->data->cmdEnableMotors > 0;
00269 
00270   //std::cout << enableMotors << std::endl;
00271 
00272   wheelSpeed[0] = vr + va * wheelSep / 2;
00273   wheelSpeed[1] = vr - va * wheelSep / 2;
00274 
00275   lock.unlock();
00276 }
00277 
00278 // NEW: Store the velocities from the ROS message
00279 void DiffDrivePlugin6W::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg)
00280 {
00281   //std::cout << "BEGIN CALLBACK\n";
00282 
00283   lock.lock();
00284 
00285   x_ = cmd_msg->linear.x;
00286   rot_ = cmd_msg->angular.z;
00287 
00288   lock.unlock();
00289 
00290   //std::cout << "END CALLBACK\n";
00291 }
00292 
00293 // NEW: custom callback queue thread
00294 void DiffDrivePlugin6W::QueueThread()
00295 {
00296   static const double timeout = 0.01;
00297 
00298   while (alive_ && rosnode_->ok())
00299   {
00300     //    std::cout << "CALLING STUFF\n";
00301     queue_.callAvailable(ros::WallDuration(timeout));
00302   }
00303 }
00304 
00305 // NEW: Update this to publish odometry topic
00306 void DiffDrivePlugin6W::publish_odometry()
00307 {
00308   // get current time
00309   ros::Time current_time_((world->GetSimTime()).sec, (world->GetSimTime()).nsec); 
00310 
00311   // getting data for base_footprint to odom transform
00312   math::Pose pose = link->GetWorldPose();
00313   math::Vector3 velocity = link->GetWorldLinearVel();
00314   math::Vector3 angular_velocity = link->GetWorldAngularVel();
00315 
00316   tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
00317   tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z);
00318   tf::Transform base_footprint_to_odom(qt, vt);
00319 
00320   transform_broadcaster_->sendTransform(tf::StampedTransform(base_footprint_to_odom,
00321                                                             current_time_,
00322                                                             "odom",
00323                                                             "base_footprint"));
00324 
00325   // publish odom topic
00326   odom_.pose.pose.position.x = pose.pos.x;
00327   odom_.pose.pose.position.y = pose.pos.y;
00328 
00329   odom_.pose.pose.orientation.x = pose.rot.x;
00330   odom_.pose.pose.orientation.y = pose.rot.y;
00331   odom_.pose.pose.orientation.z = pose.rot.z;
00332   odom_.pose.pose.orientation.w = pose.rot.w;
00333 
00334   odom_.twist.twist.linear.x = velocity.x;
00335   odom_.twist.twist.linear.y = velocity.y;
00336   odom_.twist.twist.angular.z = angular_velocity.z;
00337 
00338   odom_.header.frame_id = tf::resolve(tf_prefix_, "odom");
00339   odom_.child_frame_id = "base_footprint";
00340   odom_.header.stamp = current_time_;
00341 
00342   pub_.publish(odom_);
00343 }
00344 
00345 GZ_REGISTER_MODEL_PLUGIN(DiffDrivePlugin6W)
00346 
00347 } // namespace gazebo


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Mon Jun 27 2016 04:58:09