husky_plugin.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Clearpath Robotics, Inc.
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  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Clearpath Robotics, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Desc: Gazebo 1.x plugin for a Clearpath Robotics Husky A200
00032  * Adapted from the TurtleBot plugin
00033  * Author: Ryan Gariepy
00034  */ 
00035 
00036 #include <boost/thread.hpp>
00037 #include <nav_msgs/Odometry.h>
00038 #include <sensor_msgs/JointState.h>
00039 #include <geometry_msgs/Twist.h>
00040 
00041 #include <husky_plugin/husky_plugin.h>
00042 
00043 #include <ros/time.h>
00044 
00045 using namespace gazebo;
00046 
00047 enum {BL= 0, BR=1, FL=2, FR=3};
00048 
00049 HuskyPlugin::HuskyPlugin()
00050 {
00051   kill_sim = false;
00052   this->spinner_thread_ = new boost::thread( boost::bind( &HuskyPlugin::spin, this) );
00053 
00054   wheel_speed_ = new float[4];
00055   wheel_speed_[BL] = 0.0;
00056   wheel_speed_[BR] = 0.0;
00057   wheel_speed_[FL] = 0.0;
00058   wheel_speed_[FR] = 0.0;
00059 
00060   set_joints_[0] = false;
00061   set_joints_[1] = false;
00062   set_joints_[2] = false;
00063   set_joints_[3] = false;
00064 
00065   joints_[0].reset();
00066   joints_[1].reset();
00067   joints_[2].reset();
00068   joints_[3].reset();
00069 }
00070 
00071 HuskyPlugin::~HuskyPlugin()
00072 {
00073   event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
00074 
00075   rosnode_->shutdown();
00076   kill_sim = true;
00077   this->spinner_thread_->join();
00078   delete this->spinner_thread_;
00079   delete [] wheel_speed_;
00080   delete rosnode_;
00081 }
00082     
00083 void HuskyPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00084 {
00085   this->model_ = _parent;
00086   this->world_ = this->model_->GetWorld();
00087 
00088   this->node_namespace_ = "";
00089   if (_sdf->HasElement("robotNamespace"))
00090     this->node_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
00091 
00092 
00093   bl_joint_name_ = "backLeftJoint";
00094   if (_sdf->HasElement("backLeftJoint"))
00095     bl_joint_name_ = _sdf->GetElement("backLeftJoint")->Get<std::string>();
00096 
00097   br_joint_name_ = "backRightJoint";
00098   if (_sdf->HasElement("backRightJoint"))
00099     br_joint_name_ = _sdf->GetElement("backRightJoint")->Get<std::string>();
00100 
00101   fl_joint_name_ = "frontLeftJoint";
00102   if (_sdf->HasElement("frontLeftJoint"))
00103     fl_joint_name_ = _sdf->GetElement("frontLeftJoint")->Get<std::string>();
00104 
00105   fr_joint_name_ = "frontRightJoint";
00106   if (_sdf->HasElement("frontRightJoint"))
00107     fr_joint_name_ = _sdf->GetElement("frontRightJoint")->Get<std::string>();
00108 
00109   wheel_sep_ = 0.55;
00110   if (_sdf->HasElement("wheelSeparation"))
00111     wheel_sep_ = _sdf->GetElement("wheelSeparation")->Get<double>();
00112 
00113   wheel_diam_ = 0.30;
00114   if (_sdf->HasElement("wheelDiameter"))
00115     wheel_diam_ = _sdf->GetElement("wheelDiameter")->Get<double>();
00116 
00117   torque_ = 15.0;
00118   if (_sdf->HasElement("torque"))
00119     torque_ = _sdf->GetElement("torque")->Get<double>();
00120 
00121   base_geom_name_ = "base_link";
00122   if (_sdf->HasElement("baseGeom"))
00123     base_geom_name_ = _sdf->GetElement("baseGeom")->Get<std::string>();
00124   base_geom_ = model_->GetChildCollision(base_geom_name_);
00125 
00126   //base_geom_->SetContactsEnabled(true);
00127 
00128   // Get the name of the parent model
00129   std::string modelName = _sdf->GetParent()->Get<std::string>("name");
00130 
00131   // Listen to the update event. This event is broadcast every
00132   // simulation iteration.
00133   this->updateConnection = event::Events::ConnectWorldUpdateBegin(
00134       boost::bind(&HuskyPlugin::UpdateChild, this));
00135   gzdbg << "Plugin model name: " << modelName << "\n";
00136 
00137   if (!ros::isInitialized())
00138   {
00139     int argc = 0;
00140     char** argv = NULL;
00141     ros::init(argc, argv, "gazebo_husky", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00142   }
00143 
00144   rosnode_ = new ros::NodeHandle( node_namespace_ );
00145 
00146   cmd_vel_sub_ = rosnode_->subscribe("husky/cmd_vel", 1, &HuskyPlugin::OnCmdVel, this );
00147 
00148   odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("odom", 1);
00149 
00150   joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("joint_states", 1);
00151 
00152   js_.name.push_back( bl_joint_name_ );
00153   js_.position.push_back(0);
00154   js_.velocity.push_back(0);
00155   js_.effort.push_back(0);
00156 
00157   js_.name.push_back( br_joint_name_ );
00158   js_.position.push_back(0);
00159   js_.velocity.push_back(0);
00160   js_.effort.push_back(0);
00161 
00162   js_.name.push_back( fl_joint_name_ );
00163   js_.position.push_back(0);
00164   js_.velocity.push_back(0);
00165   js_.effort.push_back(0);
00166 
00167   js_.name.push_back( fr_joint_name_ );
00168   js_.position.push_back(0);
00169   js_.velocity.push_back(0);
00170   js_.effort.push_back(0);
00171 
00172   prev_update_time_ = 0;
00173   last_cmd_vel_time_ = 0;
00174 
00175   joints_[BL] = model_->GetJoint(bl_joint_name_);
00176   joints_[BR] = model_->GetJoint(br_joint_name_);
00177   joints_[FL] = model_->GetJoint(fl_joint_name_);
00178   joints_[FR] = model_->GetJoint(fr_joint_name_);
00179 
00180   if (joints_[BL]) set_joints_[BL] = true;
00181   if (joints_[BR]) set_joints_[BR] = true;
00182   if (joints_[FL]) set_joints_[FL] = true;
00183   if (joints_[FR]) set_joints_[FR] = true;
00184 
00185   //initialize time and odometry position
00186   prev_update_time_ = last_cmd_vel_time_ = this->world_->GetSimTime();
00187   odom_pose_[0] = 0.0;
00188   odom_pose_[1] = 0.0;
00189   odom_pose_[2] = 0.0;
00190 }
00191 
00192 
00193 void HuskyPlugin::UpdateChild()
00194 {
00195   common::Time time_now = this->world_->GetSimTime();
00196   common::Time step_time = time_now - prev_update_time_;
00197   prev_update_time_ = time_now;
00198 
00199   double wd, ws;
00200   double d_bl, d_br, d_fl, d_fr;
00201   double dr, da;
00202 
00203   wd = wheel_diam_;
00204   ws = wheel_sep_;
00205 
00206   d_bl = d_br = d_fl = d_fr = 0;
00207   dr = da = 0;
00208 
00209   // Distance travelled by front wheels
00210   if (set_joints_[BL])
00211     d_bl = step_time.Double() * (wd / 2) * joints_[BL]->GetVelocity(0);
00212   if (set_joints_[BR])
00213     d_br = step_time.Double() * (wd / 2) * joints_[BR]->GetVelocity(0);
00214   if (set_joints_[FL])
00215     d_fl = step_time.Double() * (wd / 2) * joints_[FL]->GetVelocity(0);
00216   if (set_joints_[FR])
00217     d_fr = step_time.Double() * (wd / 2) * joints_[FR]->GetVelocity(0);
00218 
00219   // Can see NaN values here, just zero them out if needed
00220   if (isnan(d_bl)) {
00221     ROS_WARN_THROTTLE(0.1, "Gazebo ROS Husky plugin. NaN in d_bl. Step time: %.2f. WD: %.2f. Velocity: %.2f", step_time.Double(), wd, joints_[BL]->GetVelocity(0));
00222     d_bl = 0;
00223   }
00224   if (isnan(d_br)) {
00225     ROS_WARN_THROTTLE(0.1, "Gazebo ROS Husky plugin. NaN in d_br. Step time: %.2f. WD: %.2f. Velocity: %.2f", step_time.Double(), wd, joints_[BR]->GetVelocity(0));
00226     d_br = 0;
00227   }
00228   if (isnan(d_fl)) {
00229     ROS_WARN_THROTTLE(0.1, "Gazebo ROS Husky plugin. NaN in d_fl. Step time: %.2f. WD: %.2f. Velocity: %.2f", step_time.Double(), wd, joints_[FL]->GetVelocity(0));
00230     d_fl = 0;
00231   }
00232   if (isnan(d_fr)) {
00233     ROS_WARN_THROTTLE(0.1, "Gazebo ROS Husky plugin. NaN in d_fr. Step time: %.2f. WD: %.2f. Velocity: %.2f", step_time.Double(), wd, joints_[FR]->GetVelocity(0));
00234     d_fr = 0;
00235   }
00236 
00237   dr = (d_bl + d_br + d_fl + d_fr) / 4;
00238   da = ((d_br+d_fr)/2 - (d_bl+d_fl)/2) / ws;
00239 
00240   // Compute odometric pose
00241   odom_pose_[0] += dr * cos( odom_pose_[2] );
00242   odom_pose_[1] += dr * sin( odom_pose_[2] );
00243   odom_pose_[2] += da;
00244 
00245   // Compute odometric instantaneous velocity
00246   odom_vel_[0] = dr / step_time.Double();
00247   odom_vel_[1] = 0.0;
00248   odom_vel_[2] = da / step_time.Double();
00249 
00250 
00251   if (set_joints_[BL])
00252   {
00253     joints_[BL]->SetVelocity( 0, wheel_speed_[BL] / (wd/2.0) );
00254     joints_[BL]->SetMaxForce( 0, torque_ );
00255   }
00256   if (set_joints_[BR])
00257   {
00258     joints_[BR]->SetVelocity( 0, wheel_speed_[BR] / (wd / 2.0) );
00259     joints_[BR]->SetMaxForce( 0, torque_ );
00260   }
00261   if (set_joints_[FL])
00262   {
00263     joints_[FL]->SetVelocity( 0, wheel_speed_[FL] / (wd / 2.0) );
00264     joints_[FL]->SetMaxForce( 0, torque_ );
00265   }
00266   if (set_joints_[FR])
00267   {
00268     joints_[FR]->SetVelocity( 0, wheel_speed_[FR] / (wd / 2.0) );
00269     joints_[FR]->SetMaxForce( 0, torque_ );
00270   }
00271 
00272   nav_msgs::Odometry odom;
00273   odom.header.stamp.sec = time_now.sec;
00274   odom.header.stamp.nsec = time_now.nsec;
00275   odom.header.frame_id = "odom";
00276   odom.child_frame_id = "base_footprint";
00277   odom.pose.pose.position.x = odom_pose_[0];
00278   odom.pose.pose.position.y = odom_pose_[1];
00279   odom.pose.pose.position.z = 0;
00280 
00281   tf::Quaternion qt;
00282   qt.setRPY(0,0,odom_pose_[2]);
00283 
00284   odom.pose.pose.orientation.x = qt.getX();
00285   odom.pose.pose.orientation.y = qt.getY();
00286   odom.pose.pose.orientation.z = qt.getZ();
00287   odom.pose.pose.orientation.w = qt.getW();
00288 
00289   double pose_cov[36] = { 1e-3, 0, 0, 0, 0, 0,
00290                           0, 1e-3, 0, 0, 0, 0,
00291                           0, 0, 1e6, 0, 0, 0,
00292                           0, 0, 0, 1e6, 0, 0,
00293                           0, 0, 0, 0, 1e6, 0,
00294                           0, 0, 0, 0, 0, 1e3};
00295 
00296   memcpy( &odom.pose.covariance[0], pose_cov, sizeof(double)*36 );
00297   memcpy( &odom.twist.covariance[0], pose_cov, sizeof(double)*36 );
00298 
00299   odom.twist.twist.linear.x = 0;
00300   odom.twist.twist.linear.y = 0;
00301   odom.twist.twist.linear.z = 0;
00302 
00303   odom.twist.twist.angular.x = 0;
00304   odom.twist.twist.angular.y = 0;
00305   odom.twist.twist.angular.z = 0;
00306 
00307   odom_pub_.publish( odom ); 
00308 
00309   js_.header.stamp.sec = time_now.sec;
00310   js_.header.stamp.nsec = time_now.nsec;
00311   if (this->set_joints_[BL])
00312   {
00313     js_.position[0] = joints_[BL]->GetAngle(0).Radian();
00314     js_.velocity[0] = joints_[BL]->GetVelocity(0);
00315   }
00316 
00317   if (this->set_joints_[BR])
00318   {
00319     js_.position[1] = joints_[BR]->GetAngle(0).Radian();
00320     js_.velocity[1] = joints_[BR]->GetVelocity(0);
00321   }
00322 
00323   if (this->set_joints_[FL])
00324   {
00325     js_.position[2] = joints_[FL]->GetAngle(0).Radian();
00326     js_.velocity[2] = joints_[FL]->GetVelocity(0);
00327   }
00328 
00329   if (this->set_joints_[FR])
00330   {
00331     js_.position[3] = joints_[FR]->GetAngle(0).Radian();
00332     js_.velocity[3] = joints_[FR]->GetVelocity(0);
00333   }
00334 
00335   joint_state_pub_.publish( js_ );
00336 
00337   // Timeout if we haven't received a cmd in <0.1 s
00338   common::Time time_since_last_cmd = time_now - last_cmd_vel_time_;
00339   if (time_since_last_cmd.Double() > 0.1)
00340   {
00341     wheel_speed_[BL] = 0;
00342     wheel_speed_[BR] = 0;
00343     wheel_speed_[FL] = 0;
00344     wheel_speed_[FR] = 0;
00345   }
00346 }
00347 
00348 
00349 void HuskyPlugin::OnCmdVel( const geometry_msgs::TwistConstPtr &msg)
00350 {
00351   last_cmd_vel_time_ = this->world_->GetSimTime();
00352   double vr, va;
00353   vr = msg->linear.x;
00354   va = msg->angular.z;
00355 
00356   wheel_speed_[BL] = vr - va * (wheel_sep_) / 2;
00357   wheel_speed_[BR] = vr + va * (wheel_sep_) / 2;
00358   wheel_speed_[FL] = vr - va * (wheel_sep_) / 2;
00359   wheel_speed_[FR] = vr + va * (wheel_sep_) / 2;
00360 }
00361 
00362 void HuskyPlugin::spin()
00363 {
00364   while(ros::ok() && !kill_sim) ros::spinOnce();
00365 }
00366 
00367 GZ_REGISTER_MODEL_PLUGIN(HuskyPlugin);
00368 


husky_gazebo_plugins
Author(s):
autogenerated on Wed Aug 26 2015 11:54:19