gazebo_ros_skid_steer_drive.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Daniel Hewlett, Antons Rebguns
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 <organization> nor the
00013  *      names of its contributors may be used to endorse or promote products
00014  *      derived from this software without specific prior written permission.
00015  *
00016  *  THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
00017  *  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018  *  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019  *  DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
00020  *  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021  *  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023  *  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024  *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025  *  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  **/
00028 
00029 /*
00030  * \file  gazebo_ros_skid_steer_drive.cpp
00031  *
00032  * \brief A skid steering drive plugin. Inspired by gazebo_ros_diff_drive and SkidSteerDrivePlugin
00033  *
00034  * \author  Zdenek Materna (imaterna@fit.vutbr.cz)
00035  *
00036  * $ Id: 06/25/2013 11:23:40 AM materna $
00037  */
00038 
00039 
00040 #include <algorithm>
00041 #include <assert.h>
00042 
00043 #include <gazebo_plugins/gazebo_ros_skid_steer_drive.h>
00044 
00045 #include <gazebo/math/gzmath.hh>
00046 #include <sdf/sdf.hh>
00047 
00048 #include <ros/ros.h>
00049 #include <tf/transform_broadcaster.h>
00050 #include <tf/transform_listener.h>
00051 #include <geometry_msgs/Twist.h>
00052 #include <nav_msgs/GetMap.h>
00053 #include <nav_msgs/Odometry.h>
00054 #include <boost/bind.hpp>
00055 #include <boost/thread/mutex.hpp>
00056 
00057 namespace gazebo {
00058 
00059   enum {
00060     RIGHT_FRONT=0,
00061     LEFT_FRONT=1,
00062     RIGHT_REAR=2,
00063     LEFT_REAR=3,
00064   };
00065 
00066   GazeboRosSkidSteerDrive::GazeboRosSkidSteerDrive() {}
00067 
00068   // Destructor
00069   GazeboRosSkidSteerDrive::~GazeboRosSkidSteerDrive() {
00070     delete rosnode_;
00071     delete transform_broadcaster_;
00072   }
00073 
00074   // Load the controller
00075   void GazeboRosSkidSteerDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
00076 
00077     this->parent = _parent;
00078     this->world = _parent->GetWorld();
00079 
00080     this->robot_namespace_ = "";
00081     if (!_sdf->HasElement("robotNamespace")) {
00082       ROS_INFO("GazeboRosSkidSteerDrive Plugin missing <robotNamespace>, defaults to \"%s\"",
00083           this->robot_namespace_.c_str());
00084     } else {
00085       this->robot_namespace_ =
00086         _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
00087     }
00088     
00089     this->broadcast_tf_ = false;
00090     if (!_sdf->HasElement("broadcastTF")) {
00091       if (!this->broadcast_tf_)
00092           ROS_INFO("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <broadcastTF>, defaults to false.",this->robot_namespace_.c_str());
00093       else ROS_INFO("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <broadcastTF>, defaults to true.",this->robot_namespace_.c_str());
00094           
00095     } else {
00096       this->broadcast_tf_ = _sdf->GetElement("broadcastTF")->Get<bool>();
00097     }
00098 
00099     // TODO write error if joint doesn't exist!
00100     this->left_front_joint_name_ = "left_front_joint";
00101     if (!_sdf->HasElement("leftFrontJoint")) {
00102       ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <leftFrontJoint>, defaults to \"%s\"",
00103           this->robot_namespace_.c_str(), this->left_front_joint_name_.c_str());
00104     } else {
00105       this->left_front_joint_name_ = _sdf->GetElement("leftFrontJoint")->Get<std::string>();
00106     }
00107 
00108     this->right_front_joint_name_ = "right_front_joint";
00109         if (!_sdf->HasElement("rightFrontJoint")) {
00110           ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <rightFrontJoint>, defaults to \"%s\"",
00111               this->robot_namespace_.c_str(), this->right_front_joint_name_.c_str());
00112         } else {
00113           this->right_front_joint_name_ = _sdf->GetElement("rightFrontJoint")->Get<std::string>();
00114         }
00115 
00116         this->left_rear_joint_name_ = "left_rear_joint";
00117         if (!_sdf->HasElement("leftRearJoint")) {
00118           ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <leftRearJoint>, defaults to \"%s\"",
00119                   this->robot_namespace_.c_str(), this->left_rear_joint_name_.c_str());
00120         } else {
00121           this->left_rear_joint_name_ = _sdf->GetElement("leftRearJoint")->Get<std::string>();
00122         }
00123 
00124     this->right_rear_joint_name_ = "right_rear_joint";
00125     if (!_sdf->HasElement("rightRearJoint")) {
00126       ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <rightRearJoint>, defaults to \"%s\"",
00127           this->robot_namespace_.c_str(), this->right_rear_joint_name_.c_str());
00128     } else {
00129       this->right_rear_joint_name_ = _sdf->GetElement("rightRearJoint")->Get<std::string>();
00130     }
00131 
00132 
00133     // This assumes that front and rear wheel spacing is identical
00134     /*this->wheel_separation_ = this->parent->GetJoint(left_front_joint_name_)->GetAnchor(0).Distance(
00135                 this->parent->GetJoint(right_front_joint_name_)->GetAnchor(0));*/
00136 
00137     this->wheel_separation_ = 0.4;
00138 
00139     if (!_sdf->HasElement("wheelSeparation")) {
00140       ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <wheelSeparation>, defaults to value from robot_description: %f",
00141           this->robot_namespace_.c_str(), this->wheel_separation_);
00142     } else {
00143       this->wheel_separation_ =
00144         _sdf->GetElement("wheelSeparation")->Get<double>();
00145     }
00146 
00147     // TODO get this from robot_description
00148     this->wheel_diameter_ = 0.15;
00149     if (!_sdf->HasElement("wheelDiameter")) {
00150       ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <wheelDiameter>, defaults to %f",
00151           this->robot_namespace_.c_str(), this->wheel_diameter_);
00152     } else {
00153       this->wheel_diameter_ = _sdf->GetElement("wheelDiameter")->Get<double>();
00154     }
00155 
00156     this->torque = 5.0;
00157     if (!_sdf->HasElement("torque")) {
00158       ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <torque>, defaults to %f",
00159           this->robot_namespace_.c_str(), this->torque);
00160     } else {
00161       this->torque = _sdf->GetElement("torque")->Get<double>();
00162     }
00163 
00164     this->command_topic_ = "cmd_vel";
00165     if (!_sdf->HasElement("commandTopic")) {
00166       ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <commandTopic>, defaults to \"%s\"",
00167           this->robot_namespace_.c_str(), this->command_topic_.c_str());
00168     } else {
00169       this->command_topic_ = _sdf->GetElement("commandTopic")->Get<std::string>();
00170     }
00171 
00172     this->odometry_topic_ = "odom";
00173     if (!_sdf->HasElement("odometryTopic")) {
00174       ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <odometryTopic>, defaults to \"%s\"",
00175           this->robot_namespace_.c_str(), this->odometry_topic_.c_str());
00176     } else {
00177       this->odometry_topic_ = _sdf->GetElement("odometryTopic")->Get<std::string>();
00178     }
00179 
00180     this->odometry_frame_ = "odom";
00181     if (!_sdf->HasElement("odometryFrame")) {
00182       ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <odometryFrame>, defaults to \"%s\"",
00183           this->robot_namespace_.c_str(), this->odometry_frame_.c_str());
00184     } else {
00185       this->odometry_frame_ = _sdf->GetElement("odometryFrame")->Get<std::string>();
00186     }
00187 
00188     this->robot_base_frame_ = "base_footprint";
00189     if (!_sdf->HasElement("robotBaseFrame")) {
00190       ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <robotBaseFrame>, defaults to \"%s\"",
00191           this->robot_namespace_.c_str(), this->robot_base_frame_.c_str());
00192     } else {
00193       this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get<std::string>();
00194     }
00195 
00196     this->update_rate_ = 100.0;
00197     if (!_sdf->HasElement("updateRate")) {
00198       ROS_WARN("GazeboRosSkidSteerDrive Plugin (ns = %s) missing <updateRate>, defaults to %f",
00199           this->robot_namespace_.c_str(), this->update_rate_);
00200     } else {
00201       this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
00202     }
00203 
00204     // Initialize update rate stuff
00205     if (this->update_rate_ > 0.0) {
00206       this->update_period_ = 1.0 / this->update_rate_;
00207     } else {
00208       this->update_period_ = 0.0;
00209     }
00210     last_update_time_ = this->world->GetSimTime();
00211 
00212     // Initialize velocity stuff
00213     wheel_speed_[RIGHT_FRONT] = 0;
00214     wheel_speed_[LEFT_FRONT] = 0;
00215     wheel_speed_[RIGHT_REAR] = 0;
00216         wheel_speed_[LEFT_REAR] = 0;
00217 
00218     x_ = 0;
00219     rot_ = 0;
00220     alive_ = true;
00221 
00222     joints[LEFT_FRONT] = this->parent->GetJoint(left_front_joint_name_);
00223     joints[RIGHT_FRONT] = this->parent->GetJoint(right_front_joint_name_);
00224     joints[LEFT_REAR] = this->parent->GetJoint(left_rear_joint_name_);
00225     joints[RIGHT_REAR] = this->parent->GetJoint(right_rear_joint_name_);
00226 
00227     if (!joints[LEFT_FRONT]) {
00228       char error[200];
00229       snprintf(error, 200,
00230           "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get left front hinge joint named \"%s\"",
00231           this->robot_namespace_.c_str(), this->left_front_joint_name_.c_str());
00232       gzthrow(error);
00233     }
00234 
00235     if (!joints[RIGHT_FRONT]) {
00236       char error[200];
00237       snprintf(error, 200,
00238           "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get right front hinge joint named \"%s\"",
00239           this->robot_namespace_.c_str(), this->right_front_joint_name_.c_str());
00240       gzthrow(error);
00241     }
00242 
00243     if (!joints[LEFT_REAR]) {
00244          char error[200];
00245          snprintf(error, 200,
00246                  "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get left rear hinge joint named \"%s\"",
00247                  this->robot_namespace_.c_str(), this->left_rear_joint_name_.c_str());
00248          gzthrow(error);
00249    }
00250 
00251    if (!joints[RIGHT_REAR]) {
00252          char error[200];
00253          snprintf(error, 200,
00254                  "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get right rear hinge joint named \"%s\"",
00255                  this->robot_namespace_.c_str(), this->right_rear_joint_name_.c_str());
00256          gzthrow(error);
00257    }
00258 
00259     joints[LEFT_FRONT]->SetMaxForce(0, torque);
00260     joints[RIGHT_FRONT]->SetMaxForce(0, torque);
00261     joints[LEFT_REAR]->SetMaxForce(0, torque);
00262     joints[RIGHT_REAR]->SetMaxForce(0, torque);
00263 
00264     // Make sure the ROS node for Gazebo has already been initialized
00265     if (!ros::isInitialized())
00266     {
00267       ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00268         << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00269       return;
00270     }
00271 
00272     rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00273 
00274     ROS_INFO("Starting GazeboRosSkidSteerDrive Plugin (ns = %s)!", this->robot_namespace_.c_str());
00275 
00276     tf_prefix_ = tf::getPrefixParam(*rosnode_);
00277     transform_broadcaster_ = new tf::TransformBroadcaster();
00278 
00279     // ROS: Subscribe to the velocity command topic (usually "cmd_vel")
00280     ros::SubscribeOptions so =
00281       ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
00282           boost::bind(&GazeboRosSkidSteerDrive::cmdVelCallback, this, _1),
00283           ros::VoidPtr(), &queue_);
00284 
00285     cmd_vel_subscriber_ = rosnode_->subscribe(so);
00286 
00287     odometry_publisher_ = rosnode_->advertise<nav_msgs::Odometry>(odometry_topic_, 1);
00288 
00289     // start custom queue for diff drive
00290     this->callback_queue_thread_ =
00291       boost::thread(boost::bind(&GazeboRosSkidSteerDrive::QueueThread, this));
00292 
00293     // listen to the update event (broadcast every simulation iteration)
00294     this->update_connection_ =
00295       event::Events::ConnectWorldUpdateBegin(
00296           boost::bind(&GazeboRosSkidSteerDrive::UpdateChild, this));
00297 
00298   }
00299 
00300   // Update the controller
00301   void GazeboRosSkidSteerDrive::UpdateChild() {
00302     common::Time current_time = this->world->GetSimTime();
00303     double seconds_since_last_update =
00304       (current_time - last_update_time_).Double();
00305     if (seconds_since_last_update > update_period_) {
00306 
00307       publishOdometry(seconds_since_last_update);
00308 
00309       // Update robot in case new velocities have been requested
00310       getWheelVelocities();
00311       joints[LEFT_FRONT]->SetVelocity(0, wheel_speed_[LEFT_FRONT] / wheel_diameter_);
00312       joints[RIGHT_FRONT]->SetVelocity(0, wheel_speed_[RIGHT_FRONT] / wheel_diameter_);
00313       joints[LEFT_REAR]->SetVelocity(0, wheel_speed_[LEFT_REAR] / wheel_diameter_);
00314       joints[RIGHT_REAR]->SetVelocity(0, wheel_speed_[RIGHT_REAR] / wheel_diameter_);
00315 
00316       last_update_time_+= common::Time(update_period_);
00317 
00318     }
00319   }
00320 
00321   // Finalize the controller
00322   void GazeboRosSkidSteerDrive::FiniChild() {
00323     alive_ = false;
00324     queue_.clear();
00325     queue_.disable();
00326     rosnode_->shutdown();
00327     callback_queue_thread_.join();
00328   }
00329 
00330   void GazeboRosSkidSteerDrive::getWheelVelocities() {
00331     boost::mutex::scoped_lock scoped_lock(lock);
00332 
00333     double vr = x_;
00334     double va = rot_;
00335 
00336     wheel_speed_[RIGHT_FRONT] = vr + va * wheel_separation_ / 2.0;
00337     wheel_speed_[RIGHT_REAR] = vr + va * wheel_separation_ / 2.0;
00338 
00339     wheel_speed_[LEFT_FRONT] = vr - va * wheel_separation_ / 2.0;
00340     wheel_speed_[LEFT_REAR] = vr - va * wheel_separation_ / 2.0;
00341 
00342   }
00343 
00344   void GazeboRosSkidSteerDrive::cmdVelCallback(
00345       const geometry_msgs::Twist::ConstPtr& cmd_msg) {
00346 
00347     boost::mutex::scoped_lock scoped_lock(lock);
00348     x_ = cmd_msg->linear.x;
00349     rot_ = cmd_msg->angular.z;
00350   }
00351 
00352   void GazeboRosSkidSteerDrive::QueueThread() {
00353     static const double timeout = 0.01;
00354 
00355     while (alive_ && rosnode_->ok()) {
00356       queue_.callAvailable(ros::WallDuration(timeout));
00357     }
00358   }
00359 
00360   void GazeboRosSkidSteerDrive::publishOdometry(double step_time) {
00361     ros::Time current_time = ros::Time::now();
00362     std::string odom_frame =
00363       tf::resolve(tf_prefix_, odometry_frame_);
00364     std::string base_footprint_frame =
00365       tf::resolve(tf_prefix_, robot_base_frame_);
00366 
00367     // TODO create some non-perfect odometry!
00368     // getting data for base_footprint to odom transform
00369     math::Pose pose = this->parent->GetWorldPose();
00370 
00371     tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
00372     tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z);
00373 
00374     tf::Transform base_footprint_to_odom(qt, vt);
00375     if (this->broadcast_tf_) {
00376 
00377         transform_broadcaster_->sendTransform(
00378         tf::StampedTransform(base_footprint_to_odom, current_time,
00379             odom_frame, base_footprint_frame));
00380 
00381     }
00382 
00383     // publish odom topic
00384     odom_.pose.pose.position.x = pose.pos.x;
00385     odom_.pose.pose.position.y = pose.pos.y;
00386 
00387     odom_.pose.pose.orientation.x = pose.rot.x;
00388     odom_.pose.pose.orientation.y = pose.rot.y;
00389     odom_.pose.pose.orientation.z = pose.rot.z;
00390     odom_.pose.pose.orientation.w = pose.rot.w;
00391     odom_.pose.covariance[0] = 0.00001;
00392     odom_.pose.covariance[7] = 0.00001;
00393     odom_.pose.covariance[14] = 1000000000000.0;
00394     odom_.pose.covariance[21] = 1000000000000.0;
00395     odom_.pose.covariance[28] = 1000000000000.0;
00396     odom_.pose.covariance[35] = 0.01;
00397 
00398     // get velocity in /odom frame
00399     math::Vector3 linear;
00400     linear = this->parent->GetWorldLinearVel();
00401     odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().z;
00402 
00403     // convert velocity to child_frame_id (aka base_footprint)
00404     float yaw = pose.rot.GetYaw();
00405     odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * linear.y;
00406     odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * linear.x;
00407 
00408     odom_.header.stamp = current_time;
00409     odom_.header.frame_id = odom_frame;
00410     odom_.child_frame_id = base_footprint_frame;
00411 
00412     odometry_publisher_.publish(odom_);
00413   }
00414 
00415   GZ_REGISTER_MODEL_PLUGIN(GazeboRosSkidSteerDrive)
00416 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Fri Aug 28 2015 10:47:25