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 00029 00030 #ifndef JOINT_STATE_PUBLISHER_PLUGIN_HH 00031 #define JOINT_STATE_PUBLISHER_PLUGIN_HH 00032 00033 #include <boost/bind.hpp> 00034 #include <gazebo/gazebo.hh> 00035 #include <gazebo/physics/physics.hh> 00036 #include <gazebo/common/common.hh> 00037 #include <stdio.h> 00038 00039 // ROS 00040 #include <ros/ros.h> 00041 #include <tf/transform_broadcaster.h> 00042 #include <sensor_msgs/JointState.h> 00043 00044 // Usage in URDF: 00045 // <gazebo> 00046 // <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so"> 00047 // <robotNamespace>/pioneer2dx</robotNamespace> 00048 // <jointName>chassis_swivel_joint, swivel_wheel_joint, left_hub_joint, right_hub_joint</jointName> 00049 // <updateRate>100.0</updateRate> 00050 // <alwaysOn>true</alwaysOn> 00051 // </plugin> 00052 // </gazebo> 00053 00054 00055 00056 namespace gazebo { 00057 class GazeboRosJointStatePublisher : public ModelPlugin { 00058 public: 00059 GazeboRosJointStatePublisher(); 00060 ~GazeboRosJointStatePublisher(); 00061 void Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); 00062 void OnUpdate ( const common::UpdateInfo & _info ); 00063 void publishJointStates(); 00064 // Pointer to the model 00065 private: 00066 event::ConnectionPtr updateConnection; 00067 physics::WorldPtr world_; 00068 physics::ModelPtr parent_; 00069 std::vector<physics::JointPtr> joints_; 00070 00071 // ROS STUFF 00072 boost::shared_ptr<ros::NodeHandle> rosnode_; 00073 sensor_msgs::JointState joint_state_; 00074 ros::Publisher joint_state_publisher_; 00075 std::string tf_prefix_; 00076 std::string robot_namespace_; 00077 std::vector<std::string> joint_names_; 00078 00079 // Update Rate 00080 double update_rate_; 00081 double update_period_; 00082 common::Time last_update_time_; 00083 00084 }; 00085 00086 // Register this plugin with the simulator 00087 GZ_REGISTER_MODEL_PLUGIN ( GazeboRosJointStatePublisher ) 00088 } 00089 00090 #endif //JOINT_STATE_PUBLISHER_PLUGIN_HH 00091
