gazebo_ros_joint_state_publisher.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Markus Bader <markus.bader@tuwien.ac.at>
00003  *  All rights reserved.
00004  *
00005  *  Redistribution and use in source and binary forms, with or without
00006  *  modification, are permitted provided that the following conditions are met:
00007  *      * Redistributions of source code must retain the above copyright
00008  *      notice, this list of conditions and the following disclaimer.
00009  *      * Redistributions in binary form must reproduce the above copyright
00010  *      notice, this list of conditions and the following disclaimer in the
00011  *      documentation and/or other materials provided with the distribution.
00012  *      * Neither the name of the <organization> nor the
00013  *      names of its contributors may be used to endorse or promote products
00014  *      derived from this software without specific prior written permission.
00015  *
00016  *  THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
00017  *  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018  *  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019  *  DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
00020  *  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021  *  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023  *  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024  *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025  *  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  **/
00028 #include <boost/algorithm/string.hpp>
00029 #include <gazebo_plugins/gazebo_ros_joint_state_publisher.h>
00030 #include <tf/transform_broadcaster.h>
00031 #include <tf/transform_listener.h>
00032 
00033 using namespace gazebo;
00034 
00035 GazeboRosJointStatePublisher::GazeboRosJointStatePublisher() {}
00036 
00037 // Destructor
00038 GazeboRosJointStatePublisher::~GazeboRosJointStatePublisher() {
00039     rosnode_->shutdown();
00040 }
00041 
00042 void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) {
00043     // Store the pointer to the model
00044     this->parent_ = _parent;
00045     this->world_ = _parent->GetWorld();
00046 
00047     this->robot_namespace_ = parent_->GetName ();
00048     if ( !_sdf->HasElement ( "robotNamespace" ) ) {
00049         ROS_INFO ( "GazeboRosJointStatePublisher Plugin missing <robotNamespace>, defaults to \"%s\"",
00050                    this->robot_namespace_.c_str() );
00051     } else {
00052         this->robot_namespace_ = _sdf->GetElement ( "robotNamespace" )->Get<std::string>();
00053         if ( this->robot_namespace_.empty() ) this->robot_namespace_ = parent_->GetName ();
00054     }
00055     if ( !robot_namespace_.empty() ) this->robot_namespace_ += "/";
00056     rosnode_ = boost::shared_ptr<ros::NodeHandle> ( new ros::NodeHandle ( this->robot_namespace_ ) );
00057 
00058     if ( !_sdf->HasElement ( "jointName" ) ) {
00059         ROS_ASSERT ( "GazeboRosJointStatePublisher Plugin missing jointNames" );
00060     } else {
00061         sdf::ElementPtr element = _sdf->GetElement ( "jointName" ) ;
00062         std::string joint_names = element->Get<std::string>();
00063         boost::erase_all ( joint_names, " " );
00064         boost::split ( joint_names_, joint_names, boost::is_any_of ( "," ) );
00065     }
00066 
00067     this->update_rate_ = 100.0;
00068     if ( !_sdf->HasElement ( "updateRate" ) ) {
00069         ROS_WARN ( "GazeboRosJointStatePublisher Plugin (ns = %s) missing <updateRate>, defaults to %f",
00070                    this->robot_namespace_.c_str(), this->update_rate_ );
00071     } else {
00072         this->update_rate_ = _sdf->GetElement ( "updateRate" )->Get<double>();
00073     }
00074 
00075     // Initialize update rate stuff
00076     if ( this->update_rate_ > 0.0 ) {
00077         this->update_period_ = 1.0 / this->update_rate_;
00078     } else {
00079         this->update_period_ = 0.0;
00080     }
00081     last_update_time_ = this->world_->GetSimTime();
00082 
00083     for ( unsigned int i = 0; i< joint_names_.size(); i++ ) {
00084         joints_.push_back ( this->parent_->GetJoint ( joint_names_[i] ) );
00085         ROS_INFO ( "GazeboRosJointStatePublisher is going to publish joint: %s", joint_names_[i].c_str() );
00086     }
00087 
00088     ROS_INFO ( "Starting GazeboRosJointStatePublisher Plugin (ns = %s)!, parent name: %s", this->robot_namespace_.c_str(), parent_->GetName ().c_str() );
00089 
00090     tf_prefix_ = tf::getPrefixParam ( *rosnode_ );
00091     joint_state_publisher_ = rosnode_->advertise<sensor_msgs::JointState> ( "joint_states",1000 );
00092 
00093     last_update_time_ = this->world_->GetSimTime();
00094     // Listen to the update event. This event is broadcast every
00095     // simulation iteration.
00096     this->updateConnection = event::Events::ConnectWorldUpdateBegin (
00097                                  boost::bind ( &GazeboRosJointStatePublisher::OnUpdate, this, _1 ) );
00098 }
00099 
00100 void GazeboRosJointStatePublisher::OnUpdate ( const common::UpdateInfo & _info ) {
00101     // Apply a small linear velocity to the model.
00102     common::Time current_time = this->world_->GetSimTime();
00103     double seconds_since_last_update = ( current_time - last_update_time_ ).Double();
00104     if ( seconds_since_last_update > update_period_ ) {
00105 
00106         publishJointStates();
00107 
00108         last_update_time_+= common::Time ( update_period_ );
00109 
00110     }
00111 
00112 }
00113 
00114 void GazeboRosJointStatePublisher::publishJointStates() {
00115     ros::Time current_time = ros::Time::now();
00116 
00117     joint_state_.header.stamp = current_time;
00118     joint_state_.name.resize ( joints_.size() );
00119     joint_state_.position.resize ( joints_.size() );
00120 
00121     for ( int i = 0; i < joints_.size(); i++ ) {
00122         physics::JointPtr joint = joints_[i];
00123         math::Angle angle = joint->GetAngle ( 0 );
00124         joint_state_.name[i] = joint->GetName();
00125         joint_state_.position[i] = angle.Radian () ;
00126     }
00127     joint_state_publisher_.publish ( joint_state_ );
00128 }
00129 


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:22