00001 #include "ros/ros.h"
00002 #include "sensor_msgs/JointState.h"
00003 #include "tf/transform_broadcaster.h"
00004
00005
00006 int main(int argc, char **argv)
00007 {
00008 ros::init(argc, argv, "cob_pseudo_joint_state_publisher");
00009
00010 ros::NodeHandle n;
00011
00012 ros::Publisher publisher = n.advertise<sensor_msgs::JointState>("/joint_states", 1);
00013 tf::TransformBroadcaster broadcaster;
00014
00015 ros::Rate loop_rate(200);
00016
00017 while (ros::ok())
00018 {
00019 sensor_msgs::JointState msg;
00020
00021
00022 msg.header.stamp = ros::Time::now();
00023
00024 XmlRpc::XmlRpcValue JointName_param_;
00025 if (n.hasParam("PseudoJoints"))
00026 {
00027 n.getParam("PseudoJoints", JointName_param_);
00028 }
00029 else
00030 {
00031 ROS_ERROR("Parameter PseudoJoints not set");
00032 }
00033
00034 for (int i = 0; i<JointName_param_.size(); i++ )
00035 {
00036 msg.name.push_back(JointName_param_[i]);
00037 if((std::string)JointName_param_[i]=="floating_rot_w")
00038 msg.position.push_back(1.0);
00039 else
00040 msg.position.push_back(0.0);
00041 msg.velocity.push_back(0.0);
00042
00043 }
00044
00045
00046 publisher.publish(msg);
00047
00048 broadcaster.sendTransform( tf::StampedTransform( tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.0, 0.0, 0.0)),
00049 ros::Time::now(),"/map", "/base_footprint"));
00050
00051 ros::spinOnce();
00052
00053 loop_rate.sleep();
00054 }
00055
00056
00057 return 0;
00058 }
00059