73 if (this->
sdf->HasElement(
"robotNamespace"))
76 if (!this->
sdf->HasElement(
"serviceName"))
84 if (!this->
sdf->HasElement(
"topicName"))
92 if (!this->
sdf->HasElement(
"updateRate"))
94 ROS_INFO_NAMED(
"joint_pose_trajectory",
"joint trajectory plugin missing <updateRate>, defaults" 95 " to 0.0 (as fast as possible)");
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)");
127 ros::SubscribeOptions::create<trajectory_msgs::JointTrajectory>(
134 #ifdef ENABLE_SERVICE 138 ros::AdvertiseServiceOptions::create<gazebo_msgs::SetJointTrajectory>(
146 #if GAZEBO_MAJOR_VERSION >= 8 166 const trajectory_msgs::JointTrajectory::ConstPtr& trajectory)
177 physics::EntityPtr ent =
178 #if GAZEBO_MAJOR_VERSION >= 8 184 this->
reference_link_ = boost::dynamic_pointer_cast<physics::Link>(ent);
187 ROS_ERROR_NAMED(
"joint_pose_trajectory",
"ros_joint_trajectory plugin needs a reference link [%s] as" 194 ROS_DEBUG_NAMED(
"joint_pose_trajectory",
"test: update model pose by keeping link [%s] stationary" 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)
204 this->
joints_[i] = this->
model_->GetJoint(trajectory->joint_names[i]);
207 unsigned int points_size = trajectory->points.size();
208 this->
points_.resize(points_size);
209 for (
unsigned int i = 0; i < points_size; ++i)
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)
215 this->
points_[i].positions[j] = trajectory->points[i].positions[j];
221 trajectory->header.stamp.nsec);
222 #if GAZEBO_MAJOR_VERSION >= 8 223 common::Time cur_time = this->
world_->SimTime();
225 common::Time cur_time = this->
world_->GetSimTime();
237 #if GAZEBO_MAJOR_VERSION >= 8 239 this->
world_->SetPhysicsEnabled(
false);
242 this->
world_->EnablePhysicsEngine(
false);
247 #ifdef ENABLE_SERVICE 249 const gazebo_msgs::SetJointTrajectory::Request& req,
250 const gazebo_msgs::SetJointTrajectory::Response& res)
264 physics::EntityPtr ent =
265 #if GAZEBO_MAJOR_VERSION >= 8 271 this->
reference_link_ = boost::dynamic_pointer_cast<physics::Link>(ent);
274 ROS_ERROR_NAMED(
"joint_pose_trajectory",
"ros_joint_trajectory plugin specified a reference link [%s]" 275 " that does not exist, aborting.\n",
277 ROS_DEBUG_NAMED(
"joint_pose_trajectory",
"will set model [%s] configuration, keeping model root link" 278 " stationary.", this->
model_->GetName().c_str());
282 ROS_DEBUG_NAMED(
"joint_pose_trajectory",
"test: update model pose by keeping link [%s] stationary" 286 #if GAZEBO_MAJOR_VERSION >= 8 287 this->
model_ = this->
world_->ModelByName(req.model_name);
289 this->model_ = this->
world_->GetModel(req.model_name);
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());
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());
313 req.joint_trajectory.header.stamp.sec,
314 req.joint_trajectory.header.stamp.nsec);
323 #if GAZEBO_MAJOR_VERSION >= 8 325 this->
world_->SetPhysicsEnabled(
false);
328 this->
world_->EnablePhysicsEngine(
false);
343 #if GAZEBO_MAJOR_VERSION >= 8 344 common::Time cur_time = this->
world_->SimTime();
346 common::Time cur_time = this->
world_->GetSimTime();
356 if (this->trajectory_index < this->
points_.size())
358 ROS_INFO_NAMED(
"joint_pose_trajectory",
"time [%f] updating configuration [%d/%lu]",
362 #if GAZEBO_MAJOR_VERSION >= 8 363 ignition::math::Pose3d reference_pose = this->
model_->WorldPose();
365 ignition::math::Pose3d reference_pose = this->
model_->GetWorldPose().Ign();
369 #if GAZEBO_MAJOR_VERSION >= 8 378 unsigned int chain_size = this->
joints_.size();
382 for (
unsigned int i = 0; i < chain_size; ++i)
387 #if GAZEBO_MAJOR_VERSION >= 9 388 this->
joints_[i]->SetPosition(0,
391 ROS_WARN_ONCE(
"The joint_pose_trajectory plugin is using the Joint::SetPosition method without preserving the link velocity.");
392 ROS_WARN_ONCE(
"As a result, gravity will not be simulated correctly for your model.");
394 ROS_WARN_ONCE(
"For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612");
395 this->
joints_[i]->SetPosition(0,
403 this->
model_->SetLinkWorldPose(reference_pose,
406 this->
model_->SetWorldPose(reference_pose);
410 ROS_ERROR_NAMED(
"joint_pose_trajectory",
"point[%u] in JointTrajectory has different number of" 411 " joint names[%u] and positions[%lu].",
417 gazebo::common::Time duration(
419 this->points_[this->trajectory_index].time_from_start.nsec);
423 this->trajectory_index++;
435 #if GAZEBO_MAJOR_VERSION >= 8 450 static const double timeout = 0.01;
virtual ~GazeboRosJointPoseTrajectory()
Destructor.
event::ConnectionPtr update_connection_
#define ROS_INFO_NAMED(name,...)
bool physics_engine_enabled_
unsigned int trajectory_index
#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())
geometry_msgs::Pose model_pose_
bool disable_physics_updates_
GazeboRosJointPoseTrajectory()
Constructor.
ROSCPP_DECL bool isInitialized()
boost::thread deferred_load_thread_
boost::mutex update_mutex
A mutex to lock access to fields that are used in message callbacks.
ros::CallbackQueue queue_
std::string topic_name_
topic name
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)
boost::thread callback_queue_thread_
std::vector< trajectory_msgs::JointTrajectoryPoint > points_
void SetTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &trajectory)
Update the controller.
std::string reference_link_name_
common::Time last_time_
save last_time
#define ROS_FATAL_STREAM_NAMED(name, args)
#define ROS_WARN_ONCE(...)
std::string service_name_
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_NAMED(name,...)
common::Time trajectory_start
std::string robot_namespace_
for setting ROS name space
boost::shared_ptr< void > VoidPtr
trajectory_msgs::JointTrajectory joint_trajectory_
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the controller.