joint_state_decumulator.cpp
Go to the documentation of this file.
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); // Use 1 thread
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 }


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43