gazebo_ros_joint_trajectory.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 
19 // *************************************************************
20 // DEPRECATED
21 // This class has been renamed to gazebo_ros_joint_pose_trajectory
22 // *************************************************************
23 
24 #include <string>
25 #include <stdlib.h>
26 #include <tf/tf.h>
27 
29 
30 namespace gazebo
31 {
32 GZ_REGISTER_MODEL_PLUGIN(GazeboRosJointTrajectory);
33 
35 // Constructor
36 ROS_DEPRECATED GazeboRosJointTrajectory::GazeboRosJointTrajectory() // replaced with GazeboROSJointPoseTrajectory
37 {
38  ROS_WARN_NAMED("gazebo_ros_joint_trajectory","DEPRECATED: gazebo_ros_joint_trajectory has been renamed to gazebo_ros_joint_pose_trajectory");
39 
40  this->has_trajectory_ = false;
41  this->trajectory_index = 0;
42  this->joint_trajectory_.points.clear();
43  this->physics_engine_enabled_ = true;
44  this->disable_physics_updates_ = true;
45 }
46 
48 // Destructor
50 {
51  this->update_connection_.reset();
52  // Finalize the controller
53  this->rosnode_->shutdown();
54  this->queue_.clear();
55  this->queue_.disable();
56  this->callback_queue_thread_.join();
57  delete this->rosnode_;
58 }
59 
61 // Load the controller
62 void GazeboRosJointTrajectory::Load(physics::ModelPtr _model,
63  sdf::ElementPtr _sdf)
64 {
65  // save pointers
66  this->model_ = _model;
67  this->sdf = _sdf;
68  this->world_ = this->model_->GetWorld();
69 
70  // this->world_->SetGravity(ignition::math::Vector3d(0, 0, 0));
71 
72  // load parameters
73  this->robot_namespace_ = "";
74  if (this->sdf->HasElement("robotNamespace"))
75  this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";
76 
77  if (!this->sdf->HasElement("serviceName"))
78  {
79  // default
80  this->service_name_ = "set_joint_trajectory";
81  }
82  else
83  this->service_name_ = this->sdf->Get<std::string>("serviceName");
84 
85  if (!this->sdf->HasElement("topicName"))
86  {
87  // default
88  this->topic_name_ = "set_joint_trajectory";
89  }
90  else
91  this->topic_name_ = this->sdf->Get<std::string>("topicName");
92 
93  if (!this->sdf->HasElement("updateRate"))
94  {
95  ROS_INFO_NAMED("joint_trajectory", "joint trajectory plugin missing <updateRate>, defaults"
96  " to 0.0 (as fast as possible)");
97  this->update_rate_ = 0;
98  }
99  else
100  this->update_rate_ = this->sdf->Get<double>("updateRate");
101 
102  // ros callback queue for processing subscription
103  if (ros::isInitialized())
104  {
105  this->deferred_load_thread_ = boost::thread(
106  boost::bind(&GazeboRosJointTrajectory::LoadThread, this));
107  }
108  else
109  {
110  gzerr << "Not loading plugin since ROS hasn't been "
111  << "properly initialized. Try starting gazebo with ros plugin:\n"
112  << " gazebo -s libgazebo_ros_api_plugin.so\n";
113  }
114 }
115 
117 // Load the controller
119 {
120  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
121 
122  // resolve tf prefix
123  std::string prefix;
124  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
125 
126  if (this->topic_name_ != "")
127  {
128  ros::SubscribeOptions trajectory_so =
129  ros::SubscribeOptions::create<trajectory_msgs::JointTrajectory>(
130  this->topic_name_, 100, boost::bind(
132  ros::VoidPtr(), &this->queue_);
133  this->sub_ = this->rosnode_->subscribe(trajectory_so);
134  }
135 
136 #ifdef ENABLE_SERVICE
137  if (this->service_name_ != "")
138  {
140  ros::AdvertiseServiceOptions::create<gazebo_msgs::SetJointTrajectory>(
141  this->service_name_,
142  boost::bind(&GazeboRosJointTrajectory::SetTrajectory, this, _1, _2),
143  ros::VoidPtr(), &this->queue_);
144  this->srv_ = this->rosnode_->advertiseService(srv_aso);
145  }
146 #endif
147 
148 #if GAZEBO_MAJOR_VERSION >= 8
149  this->last_time_ = this->world_->SimTime();
150 #else
151  this->last_time_ = this->world_->GetSimTime();
152 #endif
153 
154  // start custom queue for joint trajectory plugin ros topics
155  this->callback_queue_thread_ =
156  boost::thread(boost::bind(&GazeboRosJointTrajectory::QueueThread, this));
157 
158  // New Mechanism for Updating every World Cycle
159  // Listen to the update event. This event is broadcast every
160  // simulation iteration.
161  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
162  boost::bind(&GazeboRosJointTrajectory::UpdateStates, this));
163 }
164 
166 // set joint trajectory
168  const trajectory_msgs::JointTrajectory::ConstPtr& trajectory)
169 {
170  boost::mutex::scoped_lock lock(this->update_mutex);
171 
172  this->reference_link_name_ = trajectory->header.frame_id;
173  // do this every time a new joint trajectory is supplied,
174  // use header.frame_id as the reference_link_name_
175  if (this->reference_link_name_ != "world" &&
176  this->reference_link_name_ != "/map" &&
177  this->reference_link_name_ != "map")
178  {
179  physics::EntityPtr ent =
180 #if GAZEBO_MAJOR_VERSION >= 8
181  this->world_->EntityByName(this->reference_link_name_);
182 #else
183  this->world_->GetEntity(this->reference_link_name_);
184 #endif
185  if (ent)
186  this->reference_link_ = boost::dynamic_pointer_cast<physics::Link>(ent);
187  if (!this->reference_link_)
188  {
189  ROS_ERROR_NAMED("joint_trajectory", "ros_joint_trajectory plugin needs a reference link [%s] as"
190  " frame_id, aborting.\n", this->reference_link_name_.c_str());
191  return;
192  }
193  else
194  {
195  this->model_ = this->reference_link_->GetParentModel();
196  ROS_DEBUG_NAMED("joint_trajectory", "test: update model pose by keeping link [%s] stationary"
197  " inertially", this->reference_link_->GetName().c_str());
198  }
199  }
200 
201  // copy joint configuration into a map
202  unsigned int chain_size = trajectory->joint_names.size();
203  this->joints_.resize(chain_size);
204  for (unsigned int i = 0; i < chain_size; ++i)
205  {
206  this->joints_[i] = this->model_->GetJoint(trajectory->joint_names[i]);
207  }
208 
209  unsigned int points_size = trajectory->points.size();
210  this->points_.resize(points_size);
211  for (unsigned int i = 0; i < points_size; ++i)
212  {
213  this->points_[i].positions.resize(chain_size);
214  this->points_[i].time_from_start = trajectory->points[i].time_from_start;
215  for (unsigned int j = 0; j < chain_size; ++j)
216  {
217  this->points_[i].positions[j] = trajectory->points[i].positions[j];
218  }
219  }
220 
221  // trajectory start time
222  this->trajectory_start = gazebo::common::Time(trajectory->header.stamp.sec,
223  trajectory->header.stamp.nsec);
224 #if GAZEBO_MAJOR_VERSION >= 8
225  common::Time cur_time = this->world_->SimTime();
226 #else
227  common::Time cur_time = this->world_->GetSimTime();
228 #endif
229  if (this->trajectory_start < cur_time)
230  this->trajectory_start = cur_time;
231 
232  // update the joint trajectory to play
233  this->has_trajectory_ = true;
234  // reset trajectory_index to beginning of new trajectory
235  this->trajectory_index = 0;
236 
237  if (this->disable_physics_updates_)
238  {
239 #if GAZEBO_MAJOR_VERSION >= 8
240  this->physics_engine_enabled_ = this->world_->PhysicsEnabled();
241  this->world_->SetPhysicsEnabled(false);
242 #else
243  this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
244  this->world_->EnablePhysicsEngine(false);
245 #endif
246  }
247 }
248 
249 #ifdef ENABLE_SERVICE
251  const gazebo_msgs::SetJointTrajectory::Request& req,
252  const gazebo_msgs::SetJointTrajectory::Response& res)
253 {
254  boost::mutex::scoped_lock lock(this->update_mutex);
255 
256  this->model_pose_ = req.model_pose;
257  this->set_model_pose_ = req.set_model_pose;
258 
259  this->reference_link_name_ = req.joint_trajectory.header.frame_id;
260  // do this every time a new joint_trajectory is supplied,
261  // use header.frame_id as the reference_link_name_
262  if (this->reference_link_name_ != "world" &&
263  this->reference_link_name_ != "/map" &&
264  this->reference_link_name_ != "map")
265  {
266  physics::EntityPtr ent =
267 #if GAZEBO_MAJOR_VERSION >= 8
268  this->world_->EntityByName(this->reference_link_name_);
269 #else
270  this->world_->GetEntity(this->reference_link_name_);
271 #endif
272  if (ent)
273  this->reference_link_ = boost::shared_dynamic_cast<physics::Link>(ent);
274  if (!this->reference_link_)
275  {
276  ROS_ERROR_NAMED("joint_trajectory", "ros_joint_trajectory plugin specified a reference link [%s]"
277  " that does not exist, aborting.\n",
278  this->reference_link_name_.c_str());
279  ROS_DEBUG_NAMED("joint_trajectory", "will set model [%s] configuration, keeping model root link"
280  " stationary.", this->model_->GetName().c_str());
281  return false;
282  }
283  else
284  ROS_DEBUG_NAMED("joint_trajectory", "test: update model pose by keeping link [%s] stationary"
285  " inertially", this->reference_link_->GetName().c_str());
286  }
287 
288 #if GAZEBO_MAJOR_VERSION >= 8
289  this->model_ = this->world_->ModelByName(req.model_name);
290 #else
291  this->model_ = this->world_->GetModel(req.model_name);
292 #endif
293  if (!this->model_) // look for it by frame_id name
294  {
295  this->model_ = this->reference_link_->GetParentModel();
296  if (this->model_)
297  {
298  ROS_INFO_NAMED("joint_trajectory", "found model[%s] by link name specified in frame_id[%s]",
299  this->model_->GetName().c_str(),
300  req.joint_trajectory.header.frame_id.c_str());
301  }
302  else
303  {
304  ROS_WARN_NAMED("joint_trajectory", "no model found by link name specified in frame_id[%s],"
305  " aborting.", req.joint_trajectory.header.frame_id.c_str());
306  return false;
307  }
308  }
309 
310  // copy joint configuration into a map
311  this->joint_trajectory_ = req.joint_trajectory;
312 
313  // trajectory start time
314  this->trajectory_start = gazebo::common::Time(
315  req.joint_trajectory.header.stamp.sec,
316  req.joint_trajectory.header.stamp.nsec);
317 
318  // update the joint_trajectory to play
319  this->has_trajectory_ = true;
320  // reset trajectory_index to beginning of new trajectory
321  this->trajectory_index = 0;
322  this->disable_physics_updates_ = req.disable_physics_updates;
323  if (this->disable_physics_updates_)
324  {
325 #if GAZEBO_MAJOR_VERSION >= 8
326  this->physics_engine_enabled_ = this->world_->PhysicsEnabled();
327  this->world_->SetPhysicsEnabled(false);
328 #else
329  this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
330  this->world_->EnablePhysicsEngine(false);
331 #endif
332  }
333 
334  return true;
335 }
336 #endif
337 
339 // Play the trajectory, update states
341 {
342  boost::mutex::scoped_lock lock(this->update_mutex);
343  if (this->has_trajectory_)
344  {
345 #if GAZEBO_MAJOR_VERSION >= 8
346  common::Time cur_time = this->world_->SimTime();
347 #else
348  common::Time cur_time = this->world_->GetSimTime();
349 #endif
350  // roll out trajectory via set model configuration
351  // gzerr << "i[" << trajectory_index << "] time "
352  // << trajectory_start << " now: " << cur_time << " : "<< "\n";
353  if (cur_time >= this->trajectory_start)
354  {
355  // @todo: consider a while loop until the trajectory
356  // catches up to the current time
357  // gzerr << trajectory_index << " : " << this->points_.size() << "\n";
358  if (this->trajectory_index < this->points_.size())
359  {
360  ROS_INFO_NAMED("joint_trajectory", "time [%f] updating configuration [%d/%lu]",
361  cur_time.Double(), this->trajectory_index, this->points_.size());
362 
363  // get reference link pose before updates
364 #if GAZEBO_MAJOR_VERSION >= 8
365  ignition::math::Pose3d reference_pose = this->model_->WorldPose();
366 #else
367  ignition::math::Pose3d reference_pose = this->model_->GetWorldPose().Ign();
368 #endif
369  if (this->reference_link_)
370  {
371 #if GAZEBO_MAJOR_VERSION >= 8
372  reference_pose = this->reference_link_->WorldPose();
373 #else
374  reference_pose = this->reference_link_->GetWorldPose().Ign();
375 #endif
376  }
377 
378  // trajectory roll-out based on time:
379  // set model configuration from trajectory message
380  unsigned int chain_size = this->joints_.size();
381  if (chain_size ==
382  this->points_[this->trajectory_index].positions.size())
383  {
384  for (unsigned int i = 0; i < chain_size; ++i)
385  {
386  // this is not the most efficient way to set things
387  if (this->joints_[i])
388  {
389 #if GAZEBO_MAJOR_VERSION >= 9
390  this->joints_[i]->SetPosition(0,
391  this->points_[this->trajectory_index].positions[i], true);
392 #else
393  ROS_WARN_ONCE("The joint_trajectory plugin is using the Joint::SetPosition method without preserving the link velocity.");
394  ROS_WARN_ONCE("As a result, gravity will not be simulated correctly for your model.");
395  ROS_WARN_ONCE("Please upgrade to Gazebo 9.");
396  ROS_WARN_ONCE("For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612");
397  this->joints_[i]->SetPosition(0,
398  this->points_[this->trajectory_index].positions[i]);
399 #endif
400  }
401  }
402 
403  // set model pose
404  if (this->reference_link_)
405  this->model_->SetLinkWorldPose(reference_pose,
406  this->reference_link_);
407  else
408  this->model_->SetWorldPose(reference_pose);
409  }
410  else
411  {
412  ROS_ERROR_NAMED("joint_trajectory", "point[%u] in JointTrajectory has different number of"
413  " joint names[%u] and positions[%lu].",
414  this->trajectory_index, chain_size,
415  this->points_[this->trajectory_index].positions.size());
416  }
417 
418  // this->world_->SetPaused(is_paused); // resume original pause-state
419  gazebo::common::Time duration(
420  this->points_[this->trajectory_index].time_from_start.sec,
421  this->points_[this->trajectory_index].time_from_start.nsec);
422 
423  // reset start time for next trajectory point
424  this->trajectory_start += duration;
425  this->trajectory_index++; // increment to next trajectory point
426 
427  // save last update time stamp
428  this->last_time_ = cur_time;
429  }
430  else // no more trajectory points
431  {
432  // trajectory finished
433  this->reference_link_.reset();
434  this->has_trajectory_ = false;
435  if (this->disable_physics_updates_)
436  {
437 #if GAZEBO_MAJOR_VERSION >= 8
438  this->world_->SetPhysicsEnabled(this->physics_engine_enabled_);
439 #else
440  this->world_->EnablePhysicsEngine(this->physics_engine_enabled_);
441 #endif
442  }
443  }
444  }
445  }
446 }
447 
449 // Put laser data to the interface
451 {
452  static const double timeout = 0.01;
453  while (this->rosnode_->ok())
454  {
455  this->queue_.callAvailable(ros::WallDuration(timeout));
456  }
457 }
458 }
#define ROS_INFO_NAMED(name,...)
boost::mutex update_mutex
A mutex to lock access to fields that are used in message callbacks.
trajectory_msgs::JointTrajectory joint_trajectory_
#define ROS_WARN_NAMED(name,...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL bool isInitialized()
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::vector< trajectory_msgs::JointTrajectoryPoint > points_
std::string robot_namespace_
for setting ROS name space
#define ROS_DEBUG_NAMED(name,...)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
#define ROS_WARN_ONCE(...)
physics::LinkPtr reference_link_
pose should be set relative to this link (default to "world")
#define ROS_DEPRECATED
void SetTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &trajectory)
Update the controller.
bool getParam(const std::string &key, std::string &s) const
ros::NodeHandle * rosnode_
pointer to ros node
#define ROS_ERROR_NAMED(name,...)
bool ok() const
boost::shared_ptr< void > VoidPtr
std::vector< gazebo::physics::JointPtr > joints_
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the controller.


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39