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 JointStateDecumulator {
00008
00009 public:
00010
00011 JointStateDecumulator() : priv_handle_("~")
00012 {
00013 robot_model_ = new planning_environment::RobotModels("robot_description");
00014
00015 priv_handle_.param<bool>("publish_ungrouped_joints", publish_ungrouped_joints_, false);
00016 priv_handle_.param<std::string>("group_name", group_name_, "");
00017 priv_handle_.param<double>("publish_rate",publish_rate_,10.0);
00018
00019 if(publish_ungrouped_joints_) {
00020 const std::vector<std::string>& joint_union = robot_model_->getGroupJointUnion();
00021 for(unsigned int i = 0; i < joint_union.size(); i++) {
00022 ungrouped_joints_[joint_union[i]] = true;
00023 }
00024 } else {
00025 if(robot_model_->getPlanningGroupJoints().find(group_name_) == robot_model_->getPlanningGroupJoints().end()) {
00026 ROS_WARN_STREAM("No group " << group_name_ << " in planning groups");
00027 exit(0);
00028 }
00029 std::vector<std::string> group_joints = robot_model_->getPlanningGroupJoints().find(group_name_)->second;
00030 for(unsigned int i = 0; i < group_joints.size(); i++) {
00031 joint_values_[group_joints[i]] = 0.0;
00032 joint_velocities_[group_joints[i]] = 0.0;
00033 }
00034 }
00035 joint_state_subscriber_ = root_handle_.subscribe("joint_states", 1, &JointStateDecumulator::jointStateCallback, this);
00036
00037 joint_state_publisher_ = root_handle_.advertise<sensor_msgs::JointState>("separate_joint_states", 1);
00038 }
00039
00040 ~JointStateDecumulator()
00041 {
00042 delete robot_model_;
00043 }
00044
00045 void jointStateCallback(const sensor_msgs::JointStateConstPtr &jointState) {
00046 if (jointState->name.size() != jointState->position.size() || jointState->name.size() !=jointState->velocity.size())
00047 {
00048 ROS_ERROR("Planning environment received invalid joint state");
00049 return;
00050 }
00051 for (unsigned int i = 0 ; i < jointState->name.size(); ++i)
00052 {
00053 if(publish_ungrouped_joints_) {
00054 if(ungrouped_joints_.find(jointState->name[i]) == ungrouped_joints_.end()) {
00055 joint_values_[jointState->name[i]] = jointState->position[i];
00056 joint_velocities_[jointState->name[i]] = jointState->velocity[i];
00057 }
00058 } else if(joint_values_.find(jointState->name[i]) != joint_values_.end()) {
00059 joint_values_[jointState->name[i]] = jointState->position[i];
00060 joint_velocities_[jointState->name[i]] = jointState->velocity[i];
00061 }
00062 }
00063 }
00064
00065 void publishSeparateJointState() {
00066
00067 sensor_msgs::JointState joint_state;
00068 joint_state.header.stamp = ros::Time::now();
00069 joint_state.name.resize(joint_values_.size());
00070 joint_state.position.resize(joint_values_.size());
00071 joint_state.velocity.resize(joint_values_.size());
00072 unsigned int i = 0;
00073 for(std::map<std::string, double>::iterator it = joint_values_.begin(); it != joint_values_.end(); it++, i++) {
00074 joint_state.name[i] = it->first;
00075 joint_state.position[i] = it->second;
00076 joint_state.velocity[i] = joint_velocities_[it->first];
00077 }
00078 joint_state_publisher_.publish(joint_state);
00079 }
00080
00081 double getPublishRate() const {
00082 return publish_rate_;
00083 }
00084
00085 private:
00086
00087 ros::NodeHandle root_handle_;
00088 ros::NodeHandle priv_handle_;
00089
00090 planning_environment::RobotModels* robot_model_;
00091
00092 std::string group_name_;
00093 double publish_rate_;
00094 bool publish_ungrouped_joints_;
00095
00096 ros::Publisher joint_state_publisher_;
00097 ros::Subscriber joint_state_subscriber_;
00098
00099 std::map<std::string, double> joint_values_;
00100 std::map<std::string, double> joint_velocities_;
00101 std::map<std::string, bool> ungrouped_joints_;
00102 };
00103
00104 int main(int argc, char **argv)
00105 {
00106 ros::init(argc, argv, "joint_state_accumulator");
00107
00108 ros::AsyncSpinner spinner(2);
00109 spinner.start();
00110
00111 JointStateDecumulator jsd;
00112
00113 ros::Rate pub_rate(jsd.getPublishRate());
00114
00115 while (ros::ok())
00116 {
00117 jsd.publishSeparateJointState();
00118 pub_rate.sleep();
00119 }
00120 ros::shutdown();
00121 return 0;
00122 }
