gazebo_ros_joint_pose_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  * Desc: 3D position interface for ground truth.
20  * Author: Sachin Chitta and John Hsu
21  * Date: 1 June 2008
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(GazeboRosJointPoseTrajectory);
33 
35 // Constructor
36 ROS_DEPRECATED GazeboRosJointPoseTrajectory::GazeboRosJointPoseTrajectory() // replaced with GazeboROSJointPoseTrajectory
37 {
38 
39  this->has_trajectory_ = false;
40  this->trajectory_index = 0;
41  this->joint_trajectory_.points.clear();
42  this->physics_engine_enabled_ = true;
43  this->disable_physics_updates_ = true;
44 }
45 
47 // Destructor
49 {
50  this->update_connection_.reset();
51  // Finalize the controller
52  this->rosnode_->shutdown();
53  this->queue_.clear();
54  this->queue_.disable();
55  this->callback_queue_thread_.join();
56  delete this->rosnode_;
57 }
58 
60 // Load the controller
61 void GazeboRosJointPoseTrajectory::Load(physics::ModelPtr _model,
62  sdf::ElementPtr _sdf)
63 {
64  // save pointers
65  this->model_ = _model;
66  this->sdf = _sdf;
67  this->world_ = this->model_->GetWorld();
68 
69  // this->world_->SetGravity(ignition::math::Vector3d(0, 0, 0));
70 
71  // load parameters
72  this->robot_namespace_ = "";
73  if (this->sdf->HasElement("robotNamespace"))
74  this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";
75 
76  if (!this->sdf->HasElement("serviceName"))
77  {
78  // default
79  this->service_name_ = "set_joint_trajectory";
80  }
81  else
82  this->service_name_ = this->sdf->Get<std::string>("serviceName");
83 
84  if (!this->sdf->HasElement("topicName"))
85  {
86  // default
87  this->topic_name_ = "set_joint_trajectory";
88  }
89  else
90  this->topic_name_ = this->sdf->Get<std::string>("topicName");
91 
92  if (!this->sdf->HasElement("updateRate"))
93  {
94  ROS_INFO_NAMED("joint_pose_trajectory", "joint trajectory plugin missing <updateRate>, defaults"
95  " to 0.0 (as fast as possible)");
96  this->update_rate_ = 0;
97  }
98  else
99  this->update_rate_ = this->sdf->Get<double>("updateRate");
100 
101  // Make sure the ROS node for Gazebo has already been initialized
102  if (!ros::isInitialized())
103  {
104  ROS_FATAL_STREAM_NAMED("joint_pose_trajectory", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
105  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
106  return;
107  }
108 
109  this->deferred_load_thread_ = boost::thread(
110  boost::bind(&GazeboRosJointPoseTrajectory::LoadThread, this));
111 
112 }
113 
115 // Load the controller
117 {
118  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
119 
120  // resolve tf prefix
121  std::string prefix;
122  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
123 
124  if (this->topic_name_ != "")
125  {
126  ros::SubscribeOptions trajectory_so =
127  ros::SubscribeOptions::create<trajectory_msgs::JointTrajectory>(
128  this->topic_name_, 100, boost::bind(
130  ros::VoidPtr(), &this->queue_);
131  this->sub_ = this->rosnode_->subscribe(trajectory_so);
132  }
133 
134 #ifdef ENABLE_SERVICE
135  if (this->service_name_ != "")
136  {
138  ros::AdvertiseServiceOptions::create<gazebo_msgs::SetJointTrajectory>(
139  this->service_name_,
140  boost::bind(&GazeboRosJointPoseTrajectory::SetTrajectory, this, _1, _2),
141  ros::VoidPtr(), &this->queue_);
142  this->srv_ = this->rosnode_->advertiseService(srv_aso);
143  }
144 #endif
145 
146 #if GAZEBO_MAJOR_VERSION >= 8
147  this->last_time_ = this->world_->SimTime();
148 #else
149  this->last_time_ = this->world_->GetSimTime();
150 #endif
151 
152  // start custom queue for joint trajectory plugin ros topics
153  this->callback_queue_thread_ =
154  boost::thread(boost::bind(&GazeboRosJointPoseTrajectory::QueueThread, this));
155 
156  // New Mechanism for Updating every World Cycle
157  // Listen to the update event. This event is broadcast every
158  // simulation iteration.
159  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
160  boost::bind(&GazeboRosJointPoseTrajectory::UpdateStates, this));
161 }
162 
164 // set joint trajectory
166  const trajectory_msgs::JointTrajectory::ConstPtr& trajectory)
167 {
168  boost::mutex::scoped_lock lock(this->update_mutex);
169 
170  this->reference_link_name_ = trajectory->header.frame_id;
171  // do this every time a new joint trajectory is supplied,
172  // use header.frame_id as the reference_link_name_
173  if (this->reference_link_name_ != "world" &&
174  this->reference_link_name_ != "/map" &&
175  this->reference_link_name_ != "map")
176  {
177  physics::EntityPtr ent =
178 #if GAZEBO_MAJOR_VERSION >= 8
179  this->world_->EntityByName(this->reference_link_name_);
180 #else
181  this->world_->GetEntity(this->reference_link_name_);
182 #endif
183  if (ent)
184  this->reference_link_ = boost::dynamic_pointer_cast<physics::Link>(ent);
185  if (!this->reference_link_)
186  {
187  ROS_ERROR_NAMED("joint_pose_trajectory", "ros_joint_trajectory plugin needs a reference link [%s] as"
188  " frame_id, aborting.\n", this->reference_link_name_.c_str());
189  return;
190  }
191  else
192  {
193  this->model_ = this->reference_link_->GetParentModel();
194  ROS_DEBUG_NAMED("joint_pose_trajectory", "test: update model pose by keeping link [%s] stationary"
195  " inertially", this->reference_link_->GetName().c_str());
196  }
197  }
198 
199  // copy joint configuration into a map
200  unsigned int chain_size = trajectory->joint_names.size();
201  this->joints_.resize(chain_size);
202  for (unsigned int i = 0; i < chain_size; ++i)
203  {
204  this->joints_[i] = this->model_->GetJoint(trajectory->joint_names[i]);
205  }
206 
207  unsigned int points_size = trajectory->points.size();
208  this->points_.resize(points_size);
209  for (unsigned int i = 0; i < points_size; ++i)
210  {
211  this->points_[i].positions.resize(chain_size);
212  this->points_[i].time_from_start = trajectory->points[i].time_from_start;
213  for (unsigned int j = 0; j < chain_size; ++j)
214  {
215  this->points_[i].positions[j] = trajectory->points[i].positions[j];
216  }
217  }
218 
219  // trajectory start time
220  this->trajectory_start = gazebo::common::Time(trajectory->header.stamp.sec,
221  trajectory->header.stamp.nsec);
222 #if GAZEBO_MAJOR_VERSION >= 8
223  common::Time cur_time = this->world_->SimTime();
224 #else
225  common::Time cur_time = this->world_->GetSimTime();
226 #endif
227  if (this->trajectory_start < cur_time)
228  this->trajectory_start = cur_time;
229 
230  // update the joint trajectory to play
231  this->has_trajectory_ = true;
232  // reset trajectory_index to beginning of new trajectory
233  this->trajectory_index = 0;
234 
235  if (this->disable_physics_updates_)
236  {
237 #if GAZEBO_MAJOR_VERSION >= 8
238  this->physics_engine_enabled_ = this->world_->PhysicsEnabled();
239  this->world_->SetPhysicsEnabled(false);
240 #else
241  this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
242  this->world_->EnablePhysicsEngine(false);
243 #endif
244  }
245 }
246 
247 #ifdef ENABLE_SERVICE
249  const gazebo_msgs::SetJointTrajectory::Request& req,
250  const gazebo_msgs::SetJointTrajectory::Response& res)
251 {
252  boost::mutex::scoped_lock lock(this->update_mutex);
253 
254  this->model_pose_ = req.model_pose;
255  this->set_model_pose_ = req.set_model_pose;
256 
257  this->reference_link_name_ = req.joint_trajectory.header.frame_id;
258  // do this every time a new joint_trajectory is supplied,
259  // use header.frame_id as the reference_link_name_
260  if (this->reference_link_name_ != "world" &&
261  this->reference_link_name_ != "/map" &&
262  this->reference_link_name_ != "map")
263  {
264  physics::EntityPtr ent =
265 #if GAZEBO_MAJOR_VERSION >= 8
266  this->world_->EntityByName(this->reference_link_name_);
267 #else
268  this->world_->GetEntity(this->reference_link_name_);
269 #endif
270  if (ent)
271  this->reference_link_ = boost::dynamic_pointer_cast<physics::Link>(ent);
272  if (!this->reference_link_)
273  {
274  ROS_ERROR_NAMED("joint_pose_trajectory", "ros_joint_trajectory plugin specified a reference link [%s]"
275  " that does not exist, aborting.\n",
276  this->reference_link_name_.c_str());
277  ROS_DEBUG_NAMED("joint_pose_trajectory", "will set model [%s] configuration, keeping model root link"
278  " stationary.", this->model_->GetName().c_str());
279  return false;
280  }
281  else
282  ROS_DEBUG_NAMED("joint_pose_trajectory", "test: update model pose by keeping link [%s] stationary"
283  " inertially", this->reference_link_->GetName().c_str());
284  }
285 
286 #if GAZEBO_MAJOR_VERSION >= 8
287  this->model_ = this->world_->ModelByName(req.model_name);
288 #else
289  this->model_ = this->world_->GetModel(req.model_name);
290 #endif
291  if (!this->model_) // look for it by frame_id name
292  {
293  this->model_ = this->reference_link_->GetParentModel();
294  if (this->model_)
295  {
296  ROS_INFO_NAMED("joint_pose_trajectory", "found model[%s] by link name specified in frame_id[%s]",
297  this->model_->GetName().c_str(),
298  req.joint_trajectory.header.frame_id.c_str());
299  }
300  else
301  {
302  ROS_WARN_NAMED("joint_pose_trajectory", "no model found by link name specified in frame_id[%s],"
303  " aborting.", req.joint_trajectory.header.frame_id.c_str());
304  return false;
305  }
306  }
307 
308  // copy joint configuration into a map
309  this->joint_trajectory_ = req.joint_trajectory;
310 
311  // trajectory start time
312  this->trajectory_start = gazebo::common::Time(
313  req.joint_trajectory.header.stamp.sec,
314  req.joint_trajectory.header.stamp.nsec);
315 
316  // update the joint_trajectory to play
317  this->has_trajectory_ = true;
318  // reset trajectory_index to beginning of new trajectory
319  this->trajectory_index = 0;
320  this->disable_physics_updates_ = req.disable_physics_updates;
321  if (this->disable_physics_updates_)
322  {
323 #if GAZEBO_MAJOR_VERSION >= 8
324  this->physics_engine_enabled_ = this->world_->PhysicsEnabled();
325  this->world_->SetPhysicsEnabled(false);
326 #else
327  this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
328  this->world_->EnablePhysicsEngine(false);
329 #endif
330  }
331 
332  return true;
333 }
334 #endif
335 
337 // Play the trajectory, update states
339 {
340  boost::mutex::scoped_lock lock(this->update_mutex);
341  if (this->has_trajectory_)
342  {
343 #if GAZEBO_MAJOR_VERSION >= 8
344  common::Time cur_time = this->world_->SimTime();
345 #else
346  common::Time cur_time = this->world_->GetSimTime();
347 #endif
348  // roll out trajectory via set model configuration
349  // gzerr << "i[" << trajectory_index << "] time "
350  // << trajectory_start << " now: " << cur_time << " : "<< "\n";
351  if (cur_time >= this->trajectory_start)
352  {
353  // @todo: consider a while loop until the trajectory
354  // catches up to the current time
355  // gzerr << trajectory_index << " : " << this->points_.size() << "\n";
356  if (this->trajectory_index < this->points_.size())
357  {
358  ROS_INFO_NAMED("joint_pose_trajectory", "time [%f] updating configuration [%d/%lu]",
359  cur_time.Double(), this->trajectory_index, this->points_.size());
360 
361  // get reference link pose before updates
362 #if GAZEBO_MAJOR_VERSION >= 8
363  ignition::math::Pose3d reference_pose = this->model_->WorldPose();
364 #else
365  ignition::math::Pose3d reference_pose = this->model_->GetWorldPose().Ign();
366 #endif
367  if (this->reference_link_)
368  {
369 #if GAZEBO_MAJOR_VERSION >= 8
370  reference_pose = this->reference_link_->WorldPose();
371 #else
372  reference_pose = this->reference_link_->GetWorldPose().Ign();
373 #endif
374  }
375 
376  // trajectory roll-out based on time:
377  // set model configuration from trajectory message
378  unsigned int chain_size = this->joints_.size();
379  if (chain_size ==
380  this->points_[this->trajectory_index].positions.size())
381  {
382  for (unsigned int i = 0; i < chain_size; ++i)
383  {
384  // this is not the most efficient way to set things
385  if (this->joints_[i])
386  {
387 #if GAZEBO_MAJOR_VERSION >= 9
388  this->joints_[i]->SetPosition(0,
389  this->points_[this->trajectory_index].positions[i], true);
390 #else
391  this->joints_[i]->SetPosition(0,
392  this->points_[this->trajectory_index].positions[i]);
393 #endif
394  }
395  }
396 
397  // set model pose
398  if (this->reference_link_)
399  this->model_->SetLinkWorldPose(reference_pose,
400  this->reference_link_);
401  else
402  this->model_->SetWorldPose(reference_pose);
403  }
404  else
405  {
406  ROS_ERROR_NAMED("joint_pose_trajectory", "point[%u] in JointTrajectory has different number of"
407  " joint names[%u] and positions[%lu].",
408  this->trajectory_index, chain_size,
409  this->points_[this->trajectory_index].positions.size());
410  }
411 
412  // this->world_->SetPaused(is_paused); // resume original pause-state
413  gazebo::common::Time duration(
414  this->points_[this->trajectory_index].time_from_start.sec,
415  this->points_[this->trajectory_index].time_from_start.nsec);
416 
417  // reset start time for next trajectory point
418  this->trajectory_start += duration;
419  this->trajectory_index++; // increment to next trajectory point
420 
421  // save last update time stamp
422  this->last_time_ = cur_time;
423  }
424  else // no more trajectory points
425  {
426  // trajectory finished
427  this->reference_link_.reset();
428  this->has_trajectory_ = false;
429  if (this->disable_physics_updates_)
430  {
431 #if GAZEBO_MAJOR_VERSION >= 8
432  this->world_->SetPhysicsEnabled(this->physics_engine_enabled_);
433 #else
434  this->world_->EnablePhysicsEngine(this->physics_engine_enabled_);
435 #endif
436  }
437  }
438  }
439  }
440 }
441 
443 // Put laser data to the interface
445 {
446  static const double timeout = 0.01;
447  while (this->rosnode_->ok())
448  {
449  this->queue_.callAvailable(ros::WallDuration(timeout));
450  }
451 }
452 }
#define ROS_INFO_NAMED(name,...)
#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()
boost::mutex update_mutex
A mutex to lock access to fields that are used in message callbacks.
std::vector< gazebo::physics::JointPtr > joints_
ros::NodeHandle * rosnode_
pointer to ros node
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
physics::LinkPtr reference_link_
pose should be set relative to this link (default to "world")
#define ROS_DEBUG_NAMED(name,...)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
std::vector< trajectory_msgs::JointTrajectoryPoint > points_
void SetTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &trajectory)
Update the controller.
#define ROS_FATAL_STREAM_NAMED(name, args)
bool getParam(const std::string &key, std::string &s) const
#define ROS_DEPRECATED
#define ROS_ERROR_NAMED(name,...)
std::string robot_namespace_
for setting ROS name space
bool ok() const
boost::shared_ptr< void > VoidPtr
trajectory_msgs::JointTrajectory joint_trajectory_
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the controller.


gazebo_plugins
Author(s): John Hsu
autogenerated on Wed Aug 24 2022 02:47:52