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


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Sat Jun 8 2019 19:52:36