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