gazebo_ros_joint_state_publisher.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Markus Bader <markus.bader@tuwien.ac.at>
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the <organization> nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
17  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27  **/
28 #include <boost/algorithm/string.hpp>
31 #include <tf/transform_listener.h>
32 
33 using namespace gazebo;
34 
36 
37 // Destructor
39  rosnode_->shutdown();
40 }
41 
42 void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) {
43  // Store the pointer to the model
44  this->parent_ = _parent;
45  this->world_ = _parent->GetWorld();
46 
47  this->robot_namespace_ = parent_->GetName ();
48  if ( !_sdf->HasElement ( "robotNamespace" ) ) {
49  ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin missing <robotNamespace>, defaults to \"%s\"",
50  this->robot_namespace_.c_str() );
51  } else {
52  this->robot_namespace_ = _sdf->GetElement ( "robotNamespace" )->Get<std::string>();
53  if ( this->robot_namespace_.empty() ) this->robot_namespace_ = parent_->GetName ();
54  }
55  if ( !robot_namespace_.empty() ) this->robot_namespace_ += "/";
57 
58  if ( !_sdf->HasElement ( "jointName" ) ) {
59  ROS_ASSERT ( "GazeboRosJointStatePublisher Plugin missing jointNames" );
60  } else {
61  sdf::ElementPtr element = _sdf->GetElement ( "jointName" ) ;
62  std::string joint_names = element->Get<std::string>();
63  boost::erase_all ( joint_names, " " );
64  boost::split ( joint_names_, joint_names, boost::is_any_of ( "," ) );
65  }
66 
67  this->update_rate_ = 100.0;
68  if ( !_sdf->HasElement ( "updateRate" ) ) {
69  ROS_WARN_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin (ns = %s) missing <updateRate>, defaults to %f",
70  this->robot_namespace_.c_str(), this->update_rate_ );
71  } else {
72  this->update_rate_ = _sdf->GetElement ( "updateRate" )->Get<double>();
73  }
74 
75  // Initialize update rate stuff
76  if ( this->update_rate_ > 0.0 ) {
77  this->update_period_ = 1.0 / this->update_rate_;
78  } else {
79  this->update_period_ = 0.0;
80  }
81 #if GAZEBO_MAJOR_VERSION >= 8
82  last_update_time_ = this->world_->SimTime();
83 #else
84  last_update_time_ = this->world_->GetSimTime();
85 #endif
86 
87  for ( unsigned int i = 0; i< joint_names_.size(); i++ ) {
88  joints_.push_back ( this->parent_->GetJoint ( joint_names_[i] ) );
89  ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher is going to publish joint: %s", joint_names_[i].c_str() );
90  }
91 
92  ROS_INFO_NAMED("joint_state_publisher", "Starting GazeboRosJointStatePublisher Plugin (ns = %s)!, parent name: %s", this->robot_namespace_.c_str(), parent_->GetName ().c_str() );
93 
95  joint_state_publisher_ = rosnode_->advertise<sensor_msgs::JointState> ( "joint_states",1000 );
96 
97 #if GAZEBO_MAJOR_VERSION >= 8
98  last_update_time_ = this->world_->SimTime();
99 #else
100  last_update_time_ = this->world_->GetSimTime();
101 #endif
102  // Listen to the update event. This event is broadcast every
103  // simulation iteration.
104  this->updateConnection = event::Events::ConnectWorldUpdateBegin (
105  boost::bind ( &GazeboRosJointStatePublisher::OnUpdate, this, _1 ) );
106 }
107 
108 void GazeboRosJointStatePublisher::OnUpdate ( const common::UpdateInfo & _info ) {
109  // Apply a small linear velocity to the model.
110 #if GAZEBO_MAJOR_VERSION >= 8
111  common::Time current_time = this->world_->SimTime();
112 #else
113  common::Time current_time = this->world_->GetSimTime();
114 #endif
115  if (current_time < last_update_time_)
116  {
117  ROS_WARN_NAMED("joint_state_publisher", "Negative joint state update time difference detected.");
118  last_update_time_ = current_time;
119  }
120 
121  double seconds_since_last_update = ( current_time - last_update_time_ ).Double();
122 
123  if ( seconds_since_last_update > update_period_ ) {
124 
126 
127  last_update_time_+= common::Time ( update_period_ );
128 
129  }
130 
131 }
132 
134  ros::Time current_time = ros::Time::now();
135 
136  joint_state_.header.stamp = current_time;
137  joint_state_.name.resize ( joints_.size() );
138  joint_state_.position.resize ( joints_.size() );
139  joint_state_.velocity.resize ( joints_.size() );
140 
141  for ( int i = 0; i < joints_.size(); i++ ) {
142  physics::JointPtr joint = joints_[i];
143  double velocity = joint->GetVelocity( 0 );
144 #if GAZEBO_MAJOR_VERSION >= 8
145  double position = joint->Position ( 0 );
146 #else
147  double position = joint->GetAngle ( 0 ).Radian();
148 #endif
149  joint_state_.name[i] = joint->GetName();
150  joint_state_.position[i] = position;
151  joint_state_.velocity[i] = velocity;
152  }
154 }
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
#define ROS_WARN_NAMED(name,...)
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
boost::shared_ptr< ros::NodeHandle > rosnode_
void OnUpdate(const common::UpdateInfo &_info)
static Time now()
#define ROS_ASSERT(cond)


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