00001 #include <map>
00002 #include <ros/ros.h>
00003
00004 #include <sensor_msgs/JointState.h>
00005 #include <planning_environment/models/robot_models.h>
00006
00007 class JointStateAccumulator {
00008
00009 public:
00010
00011 JointStateAccumulator()
00012 {
00013 robot_model_ = new planning_environment::RobotModels("robot_description");
00014
00015 planning_models::KinematicModel* km = robot_model_->getKinematicModel().get();
00016 unsigned int num = 0;
00017 std::vector<const planning_models::KinematicModel::Joint*> joints;
00018 km->getJoints(joints);
00019 for(unsigned int i = 0; i < joints.size(); i++) {
00020 if(joints[i]->usedParams > 0) {
00021 joint_indices_[joints[i]->name] = num++;
00022 joint_values_[joints[i]->name] = 0.0;
00023 joint_velocities_[joints[i]->name] = 0.0;
00024 }
00025 }
00026
00027 joint_state_subscriber_ = root_handle_.subscribe("joint_states", 1, &JointStateAccumulator::jointStateCallback, this);
00028
00029 joint_state_publisher_ = root_handle_.advertise<sensor_msgs::JointState>("accumulated_joint_states", 1);
00030 }
00031
00032 void jointStateCallback(const sensor_msgs::JointStateConstPtr &jointState) {
00033 if (jointState->name.size() != jointState->position.size() || jointState->name.size() !=jointState->velocity.size())
00034 {
00035 ROS_ERROR("Planning environment received invalid joint state");
00036 return;
00037 }
00038 for (unsigned int i = 0 ; i < jointState->name.size(); ++i)
00039 {
00040 joint_values_[jointState->name[i]] = jointState->position[i];
00041 joint_velocities_[jointState->name[i]] = jointState->velocity[i]+rand()*.00000000000000001;
00042 }
00043 }
00044
00045 void publishAccumlatedJointState() {
00046
00047 sensor_msgs::JointState joint_state;
00048 joint_state.header.stamp = ros::Time::now();
00049 joint_state.name.resize(joint_indices_.size());
00050 joint_state.position.resize(joint_indices_.size());
00051 joint_state.velocity.resize(joint_indices_.size());
00052 for(std::map<std::string, unsigned int>::iterator it = joint_indices_.begin(); it != joint_indices_.end(); it++) {
00053 unsigned int ind = it->second;
00054 joint_state.name[ind] = it->first;
00055 joint_state.position[ind] = joint_values_[it->first];
00056 joint_state.velocity[ind] = joint_velocities_[it->first];
00057 }
00058 joint_state_publisher_.publish(joint_state);
00059 }
00060
00061
00062 private:
00063
00064 ros::NodeHandle root_handle_;
00065 planning_environment::RobotModels* robot_model_;
00066
00067 ros::Publisher joint_state_publisher_;
00068 ros::Subscriber joint_state_subscriber_;
00069
00070 std::map<std::string, unsigned int> joint_indices_;
00071 std::map<std::string, double> joint_values_;
00072 std::map<std::string, double> joint_velocities_;
00073
00074 };
00075
00076 int main(int argc, char **argv)
00077 {
00078 ros::init(argc, argv, "joint_state_accumulator");
00079
00080 ros::AsyncSpinner spinner(2);
00081 spinner.start();
00082
00083 JointStateAccumulator jsa;
00084
00085 ros::Rate pub_rate(10.0);
00086
00087 while (ros::ok())
00088 {
00089 jsa.publishAccumlatedJointState();
00090 pub_rate.sleep();
00091 }
00092 ros::shutdown();
00093 return 0;
00094 }
