gazebo_ros_joint_trajectory.cpp
Go to the documentation of this file.
00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /*
00022  * Desc: 3D position interface for ground truth.
00023  * Author: Sachin Chitta and John Hsu
00024  * Date: 1 June 2008
00025  * SVN info: $Id$
00026  */
00027 
00028 #include <gazebo_plugins/gazebo_ros_joint_trajectory.h>
00029 
00030 #include "tf/tf.h"
00031 
00032 namespace gazebo
00033 {
00034 
00035 
00037 // Constructor
00038 GazeboRosJointTrajectory::GazeboRosJointTrajectory()
00039 {
00040   this->has_trajectory_ = false;
00041   this->trajectory_index = 0;
00042   this->joint_trajectory_.points.clear();
00043 }
00044 
00046 // Destructor
00047 GazeboRosJointTrajectory::~GazeboRosJointTrajectory()
00048 {
00049   event::Events::DisconnectWorldUpdateStart(this->update_connection_);
00050   // Finalize the controller
00051   this->rosnode_->shutdown();
00052   this->queue_.clear();
00053   this->queue_.disable();
00054   this->callback_queue_thread_.join();
00055   delete this->rosnode_;
00056 }
00057 
00059 // Load the controller
00060 void GazeboRosJointTrajectory::Load( physics::WorldPtr _world, sdf::ElementPtr _sdf )
00061 {
00062   // Get the world name.
00063   this->world_ = _world;
00064 
00065   //this->world_->GetPhysicsEngine()->SetGravity(math::Vector3(0,0,0));
00066 
00067   // load parameters
00068   this->robot_namespace_ = "";
00069   if (_sdf->HasElement("robotNamespace"))
00070     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00071 
00072   if (!_sdf->HasElement("serviceName"))
00073   {
00074     // default
00075     this->service_name_ = "set_joint_trajectory";
00076   }
00077   else
00078     this->service_name_ = _sdf->GetElement("serviceName")->GetValueString();
00079 
00080   if (!_sdf->HasElement("topicName"))
00081   {
00082     // default
00083     this->topic_name_ = "set_joint_trajectory";
00084   }
00085   else
00086     this->topic_name_ = _sdf->GetElement("topicName")->GetValueString();
00087 
00088   if (!_sdf->HasElement("updateRate"))
00089   {
00090     ROS_INFO("joint trajectory plugin missing <updateRate>, defaults to 0.0 (as fast as possible)");
00091     this->update_rate_ = 0;
00092   }
00093   else
00094     this->update_rate_ = _sdf->GetElement("updateRate")->GetValueDouble();
00095 
00096   if (!ros::isInitialized())
00097   {
00098     ROS_ERROR("ros should have been initialized gazebo_ros_api_plugins, please load the server plugin.");
00099     int argc = 0;
00100     char** argv = NULL;
00101     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00102   }
00103 
00104   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00105 
00106   // resolve tf prefix
00107   std::string prefix;
00108   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00109   //this->tf_reference_link_name_ = tf::resolve(prefix, this->reference_link_name_);
00110 
00111   if (this->topic_name_ != "")
00112   {
00113     ros::SubscribeOptions trajectory_so = ros::SubscribeOptions::create<trajectory_msgs::JointTrajectory>(
00114       this->topic_name_,100, boost::bind( &GazeboRosJointTrajectory::SetTrajectory,this,_1),
00115       ros::VoidPtr(), &this->queue_);
00116     this->sub_ = this->rosnode_->subscribe(trajectory_so);
00117   }
00118 
00119   if (this->service_name_ != "")
00120   {
00121     ros::AdvertiseServiceOptions srv_aso = ros::AdvertiseServiceOptions::create<gazebo_msgs::SetJointTrajectory>(
00122         this->service_name_,boost::bind(&GazeboRosJointTrajectory::SetTrajectory,this,_1,_2),
00123         ros::VoidPtr(), &this->queue_);
00124     this->srv_ = this->rosnode_->advertiseService(srv_aso);
00125   }
00126   
00127   this->last_time_ = this->world_->GetSimTime();
00128 
00129   // start custom queue for joint trajectory plugin ros topics
00130   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosJointTrajectory::QueueThread,this ) );
00131   
00132   // New Mechanism for Updating every World Cycle
00133   // Listen to the update event. This event is broadcast every
00134   // simulation iteration.
00135   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
00136       boost::bind(&GazeboRosJointTrajectory::UpdateStates, this));
00137 }
00138 
00140 // set joint trajectory
00141 void GazeboRosJointTrajectory::SetTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr& trajectory)
00142 {
00143   gazebo_msgs::SetJointTrajectory::Request req;
00144   gazebo_msgs::SetJointTrajectory::Response res;
00145   // copy topic into request of service call
00146   req.joint_trajectory = *trajectory;
00147   if (!this->SetTrajectory(req,res))
00148     ROS_WARN("setting joint trajectory via topic returned failure");
00149 }
00150 
00151 bool GazeboRosJointTrajectory::SetTrajectory(const gazebo_msgs::SetJointTrajectory::Request& req, const gazebo_msgs::SetJointTrajectory::Response& res)
00152 {
00153   boost::mutex::scoped_lock lock(this->update_mutex);
00154 
00155   this->model_pose_ = req.model_pose;
00156   this->set_model_pose_ = req.set_model_pose;
00157 
00158   this->reference_link_name_ = req.joint_trajectory.header.frame_id;
00159   // do this every time a new joint_trajectory is supplied, use header.frame_id as the reference_link_name_
00160   if (this->reference_link_name_ != "world" && this->reference_link_name_ != "/map" && this->reference_link_name_ != "map")
00161   {
00162     physics::EntityPtr ent = this->world_->GetEntity(this->reference_link_name_);
00163     if (ent)
00164       this->reference_link_ = boost::shared_dynamic_cast<physics::Link>(ent);
00165     if (!this->reference_link_)
00166     {
00167       ROS_ERROR("ros_joint_trajectory plugin specified a reference link [%s] that does not exist, aborting.\n",
00168                 this->reference_link_name_.c_str());
00169       ROS_DEBUG("will set model [%s] configuration, keeping model root link stationary.",this->model_->GetName().c_str());
00170       return false;
00171     }
00172     else
00173       ROS_DEBUG("test: update model pose by keeping link [%s] stationary inertially",this->reference_link_->GetName().c_str());
00174   }
00175 
00176   this->model_ =  this->world_->GetModel(req.model_name);
00177   if (!this->model_) // look for it by frame_id name
00178   {
00179     this->model_ = this->reference_link_->GetParentModel();
00180     if (this->model_)
00181     {
00182       ROS_INFO("found model[%s] by link name specified in frame_id[%s]",this->model_->GetName().c_str(),req.joint_trajectory.header.frame_id.c_str());
00183     }
00184     else
00185     {
00186       ROS_WARN("no model found by link name specified in frame_id[%s],  aborting.",req.joint_trajectory.header.frame_id.c_str());
00187       return false;
00188     }
00189   }
00190 
00191   // copy joint configuration into a map
00192   this->joint_trajectory_ = req.joint_trajectory;
00193 
00194   // trajectory start time
00195   this->trajectory_start = gazebo::common::Time(req.joint_trajectory.header.stamp.sec,
00196                                                 req.joint_trajectory.header.stamp.nsec);
00197 
00198   // update the joint_trajectory to play
00199   this->has_trajectory_ = true;
00200   // reset trajectory_index to beginning of new trajectory
00201   this->trajectory_index = 0;
00202   this->disable_physics_updates_ = req.disable_physics_updates;
00203   if (this->disable_physics_updates_)
00204   {
00205     this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
00206     this->world_->EnablePhysicsEngine(false);
00207   }
00208 
00209   // create a joint with the world
00210   // if (this->reference_link_)
00211   //   this->FixLink(this->reference_link_);
00212 
00213   return true;
00214 }
00215 
00217 // glue a link to the world by creating a fixed joint
00218 void GazeboRosJointTrajectory::FixLink(physics::LinkPtr link)
00219 {
00220   if (this->model_)
00221   {
00222     this->joint_ = this->world_->GetPhysicsEngine()->CreateJoint("revolute",this->model_);
00223     this->joint_->SetModel(this->model_);
00224     math::Pose pose = link->GetWorldPose();
00225     //math::Pose  pose(math::Vector3(0, 0, 0.2), math::Quaternion(1, 0, 0, 0));
00226     this->joint_->Load(physics::LinkPtr(), link, pose.pos);
00227     this->joint_->SetAxis(0, math::Vector3(0, 0, 0));
00228     this->joint_->SetHighStop(0, 0);
00229     this->joint_->SetLowStop(0, 0);
00230     this->joint_->SetAnchor(0, pose.pos);
00231     this->joint_->Init();
00232   }
00233 }
00234 
00236 // unglue a link to the world by destroying the fixed joint
00237 void GazeboRosJointTrajectory::UnfixLink()
00238 {
00239   this->joint_.reset();
00240 }
00241 
00242 
00243 
00245 // Play the trajectory, update states
00246 void GazeboRosJointTrajectory::UpdateStates()
00247 {
00248   common::Time cur_time = this->world_->GetSimTime();
00249 
00250   // rate control
00251   if (this->update_rate_ > 0 && (cur_time-this->last_time_).Double() < (1.0/this->update_rate_))
00252     return;
00253 
00254   {
00255 
00256     if (this->has_trajectory_ && this->model_)
00257     {
00258       boost::mutex::scoped_lock lock(this->update_mutex);
00259       // roll out trajectory via set model configuration
00260       if (cur_time >= this->trajectory_start)
00261       {
00262         // @todo:  consider a while loop until the trajectory "catches up" to the current time?
00263         if (this->trajectory_index < this->joint_trajectory_.points.size())
00264         {
00265           ROS_INFO("time [%f] updating configuration [%d/%lu]",cur_time.Double(),this->trajectory_index,this->joint_trajectory_.points.size());
00266           // make the service call to pause gazebo
00267           //bool is_paused = this->world_->IsPaused();
00268           //if (!is_paused) this->world_->SetPaused(true);
00269 
00270           // get reference link pose before updates
00271           math::Pose reference_pose(math::Vector3(this->model_pose_.position.x,
00272                                                   this->model_pose_.position.y,
00273                                                   this->model_pose_.position.z),
00274                                  math::Quaternion(this->model_pose_.orientation.w,
00275                                                   this->model_pose_.orientation.x,
00276                                                   this->model_pose_.orientation.y,
00277                                                   this->model_pose_.orientation.z));
00278           // if (this->reference_link_)
00279           // {
00280           //   reference_pose = this->reference_link_->GetWorldPose();
00281           // }
00282 
00283           // trajectory roll-out based on time:  set model configuration from trajectory message
00284           std::map<std::string, double> joint_position_map;
00285           unsigned int chain_size = this->joint_trajectory_.joint_names.size();
00286           if (chain_size == this->joint_trajectory_.points[this->trajectory_index].positions.size())
00287           {
00288             for (unsigned int i = 0; i < chain_size; ++i)
00289             {
00290               joint_position_map[this->joint_trajectory_.joint_names[i]] = 
00291                 this->joint_trajectory_.points[this->trajectory_index].positions[i];
00292             }
00293             this->model_->SetJointPositions(joint_position_map);
00294 
00295             // set model pose?
00296             if (this->set_model_pose_)
00297             {
00298               this->model_->SetWorldPose(reference_pose);
00299             }
00300             // test: fix reference link location
00301             // if (this->reference_link_)
00302             //   this->model_->SetLinkWorldPose(reference_pose, this->reference_link_);
00303           }
00304           else
00305           {
00306             ROS_ERROR("point[%u] in JointTrajectory has different number of joint names[%u] and positions[%lu].",
00307                       this->trajectory_index, chain_size,
00308                       this->joint_trajectory_.points[this->trajectory_index].positions.size());
00309           }
00310 
00311 
00312           //this->world_->SetPaused(is_paused); // resume original pause-state
00313           gazebo::common::Time duration(this->joint_trajectory_.points[this->trajectory_index].time_from_start.sec,
00314                                         this->joint_trajectory_.points[this->trajectory_index].time_from_start.nsec);
00315           this->trajectory_start += duration; // reset start time for next trajectory point
00316           this->trajectory_index++; // increment to next trajectory point
00317 
00318           // save last update time stamp
00319           this->last_time_ = cur_time;
00320         }
00321         else // no more trajectory points
00322         {
00323           // trajectory finished
00324           this->reference_link_.reset();
00325           this->has_trajectory_ = false;
00326           if (this->disable_physics_updates_)
00327             this->world_->EnablePhysicsEngine(this->physics_engine_enabled_);
00328         }
00329       }
00330     }
00331   } // mutex lock
00332 
00333 }
00334 
00336 // Put laser data to the interface
00337 void GazeboRosJointTrajectory::QueueThread()
00338 {
00339   static const double timeout = 0.01;
00340 
00341   while (this->rosnode_->ok())
00342   {
00343     this->queue_.callAvailable(ros::WallDuration(timeout));
00344   }
00345 }
00346 
00347 GZ_REGISTER_WORLD_PLUGIN(GazeboRosJointTrajectory);
00348 
00349 }


gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Mon Oct 6 2014 12:15:44