default_joint_state_publisher.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #include <ros/ros.h>
00038 #include <sensor_msgs/JointState.h>
00039 #include <planning_models/kinematic_state.h>
00040 #include <planning_environment/models/robot_models.h>
00041 #include <tf/transform_broadcaster.h>
00042 
00043 static const std::string JOINT_STATES_TOPIC = "/joint_states";
00044 
00045 int main(int argc, char** argv)
00046 {
00047   ros::init(argc, argv, "default_joint_state_publisher");
00048 
00049   ROS_INFO("got info");
00050  
00051   ros::NodeHandle nh;
00052 
00053   //for odom combined
00054   tf::TransformBroadcaster odom_broadcaster;
00055 
00056   std::vector<geometry_msgs::TransformStamped> trans_vector;
00057   
00058   geometry_msgs::TransformStamped odom_ident;
00059   odom_ident.header.frame_id = "odom_combined";
00060   odom_ident.child_frame_id = "base_footprint";
00061   odom_ident.transform.rotation.w = 1.0;
00062 
00063   trans_vector.push_back(odom_ident);
00064 
00065   geometry_msgs::TransformStamped map_to_odom;
00066   map_to_odom.header.frame_id = "map";
00067   map_to_odom.child_frame_id = "odom_combined";  
00068   map_to_odom.transform.translation.x = -3.0;
00069   map_to_odom.transform.rotation.w = 1.0;
00070 
00071   trans_vector.push_back(map_to_odom);
00072 
00073   geometry_msgs::TransformStamped map_to_stapler;
00074   map_to_stapler.header.frame_id = "map";
00075   map_to_stapler.child_frame_id = "map_to_stapler";  
00076   map_to_stapler.transform.translation.x = 1.0;
00077   map_to_stapler.transform.translation.y = -3.0;
00078   map_to_stapler.transform.rotation.w = 1.0;
00079 
00080   trans_vector.push_back(map_to_stapler);
00081 
00082   std::string robot_description_name = nh.resolveName("robot_description", true);
00083   
00084   ros::WallRate h(10.0);
00085 
00086   while(nh.ok() && !nh.hasParam(robot_description_name)) {
00087     ros::spinOnce();
00088     h.sleep();
00089   }
00090 
00091   ROS_INFO_STREAM("Got description");
00092 
00093   planning_environment::RobotModels rmodel(robot_description_name);
00094 
00095   ROS_INFO_STREAM("Made models");
00096 
00097   ros::Publisher joint_state_publisher = nh.advertise<sensor_msgs::JointState>(JOINT_STATES_TOPIC, 1);
00098 
00099   planning_models::KinematicState state(rmodel.getKinematicModel());
00100 
00101   state.setKinematicStateToDefault();
00102 
00103   std::map<std::string, double> joint_state_map;
00104   state.getKinematicStateValues(joint_state_map);
00105 
00106   sensor_msgs::JointState joint_state;
00107   joint_state.name.resize(joint_state_map.size());
00108   joint_state.position.resize(joint_state_map.size());
00109   joint_state.velocity.resize(joint_state_map.size());
00110   unsigned int i = 0;
00111   for(std::map<std::string, double>::iterator it = joint_state_map.begin(); it != joint_state_map.end(); it++, i++) {
00112     joint_state.name[i] = it->first;
00113     joint_state.position[i] = it->second;
00114     joint_state.velocity[i] = 0.0;
00115   }
00116 
00117   ros::WallRate r(100.0);
00118   while(nh.ok()) {
00119     ros::Time ts = ros::Time::now();
00120     
00121     joint_state.header.stamp = ts;
00122     joint_state_publisher.publish(joint_state);
00123     for(unsigned int i = 0; i < trans_vector.size(); i++) {
00124       trans_vector[i].header.stamp = ts;
00125     }
00126     odom_broadcaster.sendTransform(trans_vector);
00127     ros::spinOnce();
00128     r.sleep();
00129   }
00130 
00131   ros::shutdown();
00132 }
00133   
00134   
00135 
00136 


planning_environment
Author(s): Ioan Sucan
autogenerated on Thu Dec 12 2013 11:09:24