gazebo_ros_joint_pose_trajectory.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Open Source Robotics Foundation
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 /*
00019  * Desc: 3D position interface for ground truth.
00020  * Author: Sachin Chitta and John Hsu
00021  * Date: 1 June 2008
00022  */
00023 
00024 #include <string>
00025 #include <stdlib.h>
00026 #include <tf/tf.h>
00027 
00028 #include <gazebo_plugins/gazebo_ros_joint_pose_trajectory.h>
00029 
00030 namespace gazebo
00031 {
00032 GZ_REGISTER_MODEL_PLUGIN(GazeboRosJointPoseTrajectory);
00033 
00035 // Constructor
00036 ROS_DEPRECATED GazeboRosJointPoseTrajectory::GazeboRosJointPoseTrajectory()  // replaced with GazeboROSJointPoseTrajectory
00037 {
00038 
00039   this->has_trajectory_ = false;
00040   this->trajectory_index = 0;
00041   this->joint_trajectory_.points.clear();
00042   this->physics_engine_enabled_ = true;
00043   this->disable_physics_updates_ = true;
00044 }
00045 
00047 // Destructor
00048 GazeboRosJointPoseTrajectory::~GazeboRosJointPoseTrajectory()
00049 {
00050   event::Events::DisconnectWorldUpdateBegin(this->update_connection_);
00051   // Finalize the controller
00052   this->rosnode_->shutdown();
00053   this->queue_.clear();
00054   this->queue_.disable();
00055   this->callback_queue_thread_.join();
00056   delete this->rosnode_;
00057 }
00058 
00060 // Load the controller
00061 void GazeboRosJointPoseTrajectory::Load(physics::ModelPtr _model,
00062   sdf::ElementPtr _sdf)
00063 {
00064   // save pointers
00065   this->model_ = _model;
00066   this->sdf = _sdf;
00067   this->world_ = this->model_->GetWorld();
00068 
00069   // this->world_->GetPhysicsEngine()->SetGravity(math::Vector3(0, 0, 0));
00070 
00071   // load parameters
00072   this->robot_namespace_ = "";
00073   if (this->sdf->HasElement("robotNamespace"))
00074     this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";
00075 
00076   if (!this->sdf->HasElement("serviceName"))
00077   {
00078     // default
00079     this->service_name_ = "set_joint_trajectory";
00080   }
00081   else
00082     this->service_name_ = this->sdf->Get<std::string>("serviceName");
00083 
00084   if (!this->sdf->HasElement("topicName"))
00085   {
00086     // default
00087     this->topic_name_ = "set_joint_trajectory";
00088   }
00089   else
00090     this->topic_name_ = this->sdf->Get<std::string>("topicName");
00091 
00092   if (!this->sdf->HasElement("updateRate"))
00093   {
00094     ROS_INFO("joint trajectory plugin missing <updateRate>, defaults"
00095              " to 0.0 (as fast as possible)");
00096     this->update_rate_ = 0;
00097   }
00098   else
00099     this->update_rate_ = this->sdf->Get<double>("updateRate");
00100 
00101   // Make sure the ROS node for Gazebo has already been initialized
00102   if (!ros::isInitialized())
00103   {
00104     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00105       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00106     return;
00107   }
00108 
00109   this->deferred_load_thread_ = boost::thread(
00110     boost::bind(&GazeboRosJointPoseTrajectory::LoadThread, this));
00111 
00112 }
00113 
00115 // Load the controller
00116 void GazeboRosJointPoseTrajectory::LoadThread()
00117 {
00118   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00119 
00120   // resolve tf prefix
00121   std::string prefix;
00122   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00123 
00124   if (this->topic_name_ != "")
00125   {
00126     ros::SubscribeOptions trajectory_so =
00127       ros::SubscribeOptions::create<trajectory_msgs::JointTrajectory>(
00128       this->topic_name_, 100, boost::bind(
00129       &GazeboRosJointPoseTrajectory::SetTrajectory, this, _1),
00130       ros::VoidPtr(), &this->queue_);
00131     this->sub_ = this->rosnode_->subscribe(trajectory_so);
00132   }
00133 
00134 #ifdef ENABLE_SERVICE
00135   if (this->service_name_ != "")
00136   {
00137     ros::AdvertiseServiceOptions srv_aso =
00138       ros::AdvertiseServiceOptions::create<gazebo_msgs::SetJointTrajectory>(
00139       this->service_name_,
00140       boost::bind(&GazeboRosJointPoseTrajectory::SetTrajectory, this, _1, _2),
00141       ros::VoidPtr(), &this->queue_);
00142     this->srv_ = this->rosnode_->advertiseService(srv_aso);
00143   }
00144 #endif
00145 
00146   this->last_time_ = this->world_->GetSimTime();
00147 
00148   // start custom queue for joint trajectory plugin ros topics
00149   this->callback_queue_thread_ =
00150     boost::thread(boost::bind(&GazeboRosJointPoseTrajectory::QueueThread, this));
00151 
00152   // New Mechanism for Updating every World Cycle
00153   // Listen to the update event. This event is broadcast every
00154   // simulation iteration.
00155   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
00156       boost::bind(&GazeboRosJointPoseTrajectory::UpdateStates, this));
00157 }
00158 
00160 // set joint trajectory
00161 void GazeboRosJointPoseTrajectory::SetTrajectory(
00162   const trajectory_msgs::JointTrajectory::ConstPtr& trajectory)
00163 {
00164   boost::mutex::scoped_lock lock(this->update_mutex);
00165 
00166   this->reference_link_name_ = trajectory->header.frame_id;
00167   // do this every time a new joint trajectory is supplied,
00168   // use header.frame_id as the reference_link_name_
00169   if (this->reference_link_name_ != "world" &&
00170       this->reference_link_name_ != "/map" &&
00171       this->reference_link_name_ != "map")
00172   {
00173     physics::EntityPtr ent =
00174       this->world_->GetEntity(this->reference_link_name_);
00175     if (ent)
00176       this->reference_link_ = boost::dynamic_pointer_cast<physics::Link>(ent);
00177     if (!this->reference_link_)
00178     {
00179       ROS_ERROR("ros_joint_trajectory plugin needs a reference link [%s] as"
00180                 " frame_id, aborting.\n", this->reference_link_name_.c_str());
00181       return;
00182     }
00183     else
00184     {
00185       this->model_ = this->reference_link_->GetParentModel();
00186       ROS_DEBUG("test: update model pose by keeping link [%s] stationary"
00187                 " inertially", this->reference_link_->GetName().c_str());
00188     }
00189   }
00190 
00191   // copy joint configuration into a map
00192   unsigned int chain_size = trajectory->joint_names.size();
00193   this->joints_.resize(chain_size);
00194   for (unsigned int i = 0; i < chain_size; ++i)
00195   {
00196     this->joints_[i] = this->model_->GetJoint(trajectory->joint_names[i]);
00197   }
00198 
00199   unsigned int points_size = trajectory->points.size();
00200   this->points_.resize(points_size);
00201   for (unsigned int i = 0; i < points_size; ++i)
00202   {
00203     this->points_[i].positions.resize(chain_size);
00204     this->points_[i].time_from_start = trajectory->points[i].time_from_start;
00205     for (unsigned int j = 0; j < chain_size; ++j)
00206     {
00207       this->points_[i].positions[j] = trajectory->points[i].positions[j];
00208     }
00209   }
00210 
00211   // trajectory start time
00212   this->trajectory_start = gazebo::common::Time(trajectory->header.stamp.sec,
00213                                                 trajectory->header.stamp.nsec);
00214   common::Time cur_time = this->world_->GetSimTime();
00215   if (this->trajectory_start < cur_time)
00216     this->trajectory_start = cur_time;
00217 
00218   // update the joint trajectory to play
00219   this->has_trajectory_ = true;
00220   // reset trajectory_index to beginning of new trajectory
00221   this->trajectory_index = 0;
00222 
00223   if (this->disable_physics_updates_)
00224   {
00225     this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
00226     this->world_->EnablePhysicsEngine(false);
00227   }
00228 }
00229 
00230 #ifdef ENABLE_SERVICE
00231 bool GazeboRosJointPoseTrajectory::SetTrajectory(
00232   const gazebo_msgs::SetJointTrajectory::Request& req,
00233   const gazebo_msgs::SetJointTrajectory::Response& res)
00234 {
00235   boost::mutex::scoped_lock lock(this->update_mutex);
00236 
00237   this->model_pose_ = req.model_pose;
00238   this->set_model_pose_ = req.set_model_pose;
00239 
00240   this->reference_link_name_ = req.joint_trajectory.header.frame_id;
00241   // do this every time a new joint_trajectory is supplied,
00242   // use header.frame_id as the reference_link_name_
00243   if (this->reference_link_name_ != "world" &&
00244       this->reference_link_name_ != "/map" &&
00245       this->reference_link_name_ != "map")
00246   {
00247     physics::EntityPtr ent =
00248       this->world_->GetEntity(this->reference_link_name_);
00249     if (ent)
00250       this->reference_link_ = boost::dynamic_pointer_cast<physics::Link>(ent);
00251     if (!this->reference_link_)
00252     {
00253       ROS_ERROR("ros_joint_trajectory plugin specified a reference link [%s]"
00254                 " that does not exist, aborting.\n",
00255                 this->reference_link_name_.c_str());
00256       ROS_DEBUG("will set model [%s] configuration, keeping model root link"
00257                 " stationary.", this->model_->GetName().c_str());
00258       return false;
00259     }
00260     else
00261       ROS_DEBUG("test: update model pose by keeping link [%s] stationary"
00262                 " inertially", this->reference_link_->GetName().c_str());
00263   }
00264 
00265   this->model_ =  this->world_->GetModel(req.model_name);
00266   if (!this->model_)  // look for it by frame_id name
00267   {
00268     this->model_ = this->reference_link_->GetParentModel();
00269     if (this->model_)
00270     {
00271       ROS_INFO("found model[%s] by link name specified in frame_id[%s]",
00272         this->model_->GetName().c_str(),
00273         req.joint_trajectory.header.frame_id.c_str());
00274     }
00275     else
00276     {
00277       ROS_WARN("no model found by link name specified in frame_id[%s],"
00278                "  aborting.", req.joint_trajectory.header.frame_id.c_str());
00279       return false;
00280     }
00281   }
00282 
00283   // copy joint configuration into a map
00284   this->joint_trajectory_ = req.joint_trajectory;
00285 
00286   // trajectory start time
00287   this->trajectory_start = gazebo::common::Time(
00288     req.joint_trajectory.header.stamp.sec,
00289     req.joint_trajectory.header.stamp.nsec);
00290 
00291   // update the joint_trajectory to play
00292   this->has_trajectory_ = true;
00293   // reset trajectory_index to beginning of new trajectory
00294   this->trajectory_index = 0;
00295   this->disable_physics_updates_ = req.disable_physics_updates;
00296   if (this->disable_physics_updates_)
00297   {
00298     this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
00299     this->world_->EnablePhysicsEngine(false);
00300   }
00301 
00302   return true;
00303 }
00304 #endif
00305 
00307 // Play the trajectory, update states
00308 void GazeboRosJointPoseTrajectory::UpdateStates()
00309 {
00310   boost::mutex::scoped_lock lock(this->update_mutex);
00311   if (this->has_trajectory_)
00312   {
00313     common::Time cur_time = this->world_->GetSimTime();
00314     // roll out trajectory via set model configuration
00315     // gzerr << "i[" << trajectory_index  << "] time "
00316     //       << trajectory_start << " now: " << cur_time << " : "<< "\n";
00317     if (cur_time >= this->trajectory_start)
00318     {
00319       // @todo:  consider a while loop until the trajectory
00320       // catches up to the current time
00321       // gzerr << trajectory_index << " : "  << this->points_.size() << "\n";
00322       if (this->trajectory_index < this->points_.size())
00323       {
00324         ROS_INFO("time [%f] updating configuration [%d/%lu]",
00325           cur_time.Double(), this->trajectory_index, this->points_.size());
00326 
00327         // get reference link pose before updates
00328         math::Pose reference_pose = this->model_->GetWorldPose();
00329         if (this->reference_link_)
00330         {
00331           reference_pose = this->reference_link_->GetWorldPose();
00332         }
00333 
00334         // trajectory roll-out based on time:
00335         //  set model configuration from trajectory message
00336         unsigned int chain_size = this->joints_.size();
00337         if (chain_size ==
00338           this->points_[this->trajectory_index].positions.size())
00339         {
00340           for (unsigned int i = 0; i < chain_size; ++i)
00341           {
00342             // this is not the most efficient way to set things
00343             if (this->joints_[i])
00344             {
00345 #if GAZEBO_MAJOR_VERSION >= 4
00346               this->joints_[i]->SetPosition(0,
00347 #else
00348               this->joints_[i]->SetAngle(0,
00349 #endif
00350                 this->points_[this->trajectory_index].positions[i]);
00351             }
00352           }
00353 
00354           // set model pose
00355           if (this->reference_link_)
00356             this->model_->SetLinkWorldPose(reference_pose,
00357               this->reference_link_);
00358           else
00359             this->model_->SetWorldPose(reference_pose);
00360         }
00361         else
00362         {
00363           ROS_ERROR("point[%u] in JointTrajectory has different number of"
00364                     " joint names[%u] and positions[%lu].",
00365                     this->trajectory_index, chain_size,
00366                     this->points_[this->trajectory_index].positions.size());
00367         }
00368 
00369         // this->world_->SetPaused(is_paused);  // resume original pause-state
00370         gazebo::common::Time duration(
00371           this->points_[this->trajectory_index].time_from_start.sec,
00372           this->points_[this->trajectory_index].time_from_start.nsec);
00373 
00374         // reset start time for next trajectory point
00375         this->trajectory_start += duration;
00376         this->trajectory_index++;  // increment to next trajectory point
00377 
00378         // save last update time stamp
00379         this->last_time_ = cur_time;
00380       }
00381       else  // no more trajectory points
00382       {
00383         // trajectory finished
00384         this->reference_link_.reset();
00385         this->has_trajectory_ = false;
00386         if (this->disable_physics_updates_)
00387           this->world_->EnablePhysicsEngine(this->physics_engine_enabled_);
00388       }
00389     }
00390   }
00391 }
00392 
00394 // Put laser data to the interface
00395 void GazeboRosJointPoseTrajectory::QueueThread()
00396 {
00397   static const double timeout = 0.01;
00398   while (this->rosnode_->ok())
00399   {
00400     this->queue_.callAvailable(ros::WallDuration(timeout));
00401   }
00402 }
00403 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:22