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


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 18:41:09