Robot state publisher supports full trees and flattened trees

robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h

@@ -65,6 +65,7 @@ public:
 
 private:
   ros::Publisher tf_publisher_;
+  bool flatten_tree_;
   KDL::Tree tree_;
   boost::scoped_ptr<KDL::TreeFkSolverPosFull_recursive> solver_;
   std::string root_;

robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp

@@ -24,6 +24,8 @@
 #define KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP
 
 #include <kdl/tree.hpp>
+#include <tf/transform_datatypes.h>
+
 
 namespace KDL {
 
@@ -34,11 +36,14 @@ public:
   TreeFkSolverPosFull_recursive(const Tree& _tree);
   ~TreeFkSolverPosFull_recursive();
 
-  int JntToCart(const std::map<std::string, double>& q_in, std::map<std::string, Frame>& p_out);
+  int JntToCart(const std::map<std::string, double>& q_in, std::map<std::string, tf::Stamped<Frame> >& p_out, bool flatten_tree=true);
 
 private:
-  void addFrameToMap(const std::map<std::string, double>& q_in, std::map<std::string, Frame>& p_out,
-                     const Frame& previous_frame, const SegmentMap::const_iterator this_segment);
+  void addFrameToMap(const std::map<std::string, double>& q_in, 
+		     std::map<std::string, tf::Stamped<KDL::Frame> >& p_out,
+		     const tf::Stamped<KDL::Frame>& previous_frame,
+		     const SegmentMap::const_iterator this_segment,
+		     bool flatten_tree);
 
   Tree tree;
 

robot_state_publisher/src/robot_state_publisher.cpp

@@ -52,6 +52,10 @@ RobotStatePublisher::RobotStatePublisher(const Tree& tree)
   // build tree solver
   solver_.reset(new TreeFkSolverPosFull_recursive(tree_));
 
+  // get parameter to flatter tree or not
+  ros::NodeHandle n_private("~");
+  n_private.param("flatten_tree", flatten_tree_, false);
+
   // advertise tf message
   NodeHandle n;
   tf_publisher_ = n.advertise<tf::tfMessage>("/tf", 5);
@@ -65,27 +69,31 @@ RobotStatePublisher::RobotStatePublisher(const Tree& tree)
 
 bool RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time)
 {
-  // calculate transforms form root to every segment in tree
-  map<string, Frame> link_poses;
-  solver_->JntToCart(joint_positions, link_poses);
+  // calculate transforms 
+  map<string, tf::Stamped<Frame> > link_poses;
+  solver_->JntToCart(joint_positions, link_poses, flatten_tree_);
+
   if (link_poses.empty()){
     ROS_ERROR("Could not compute link poses. The tree or the state is invalid.");
     return false;
   }
   transforms_.resize(link_poses.size());
 
-  // send transforms to tf
-  tf::Transform tf_frame;
   unsigned int i = 0;
-  for (map<string, Frame>::const_iterator f=link_poses.begin(); f!=link_poses.end(); f++){
+  tf::Transform tf_frame;
+  for (map<string, tf::Stamped<Frame> >::const_iterator f=link_poses.begin(); f!=link_poses.end(); f++){
     tf::TransformKDLToTF(f->second, transforms_[i]);
     transforms_[i].stamp_ = time;
-    transforms_[i].frame_id_ = root_;
+    transforms_[i].frame_id_ = f->second.frame_id_;
     transforms_[i].child_frame_id_ = f->first;
     i++;
   }
+
   tf_broadcaster_.sendTransform(transforms_);
 
   return true;
 }
 }
+
+
+

robot_state_publisher/src/treefksolverposfull_recursive.cpp

@@ -38,23 +38,27 @@ TreeFkSolverPosFull_recursive::~TreeFkSolverPosFull_recursive()
 {
 }
 
-int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, map<string, Frame>& p_out)
+
+int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, map<string, tf::Stamped<Frame> >& p_out, bool flatten_tree)
 {      
   // clear output
   p_out.clear();
 
-  addFrameToMap(q_in, p_out, Frame::Identity(), tree.getRootSegment());
+  addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), tree.getRootSegment()->second.segment.getName()), tree.getRootSegment(), flatten_tree);
 
   return 0;
 }
 
 
-
-void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_in, map<string, Frame>& p_out,
-                                                  const Frame& previous_frame, const SegmentMap::const_iterator this_segment)
+					     
+void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_in, 
+						  map<string, tf::Stamped<Frame> >& p_out,
+						  const tf::Stamped<KDL::Frame>& previous_frame,
+						  const SegmentMap::const_iterator this_segment,
+						  bool flatten_tree)
 {
   // get pose of this segment
-  Frame this_frame;
+  tf::Stamped<KDL::Frame> this_frame;
   double jnt_p = 0;
   if (this_segment->second.segment.getJoint().getType() != Joint::None){
     map<string, double>::const_iterator jnt_pos = q_in.find(this_segment->second.segment.getJoint().getName());
@@ -64,17 +68,18 @@ void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_i
     }
     jnt_p = jnt_pos->second;
   }
-  this_frame = previous_frame * this_segment->second.segment.pose(jnt_p);
-  double r, p, y;
-  this_frame.M.GetRPY(r, p, y);
+  this_frame = tf::Stamped<KDL::Frame>(previous_frame * this_segment->second.segment.pose(jnt_p), ros::Time(), previous_frame.frame_id_);
+
   if (this_segment->first != tree.getRootSegment()->first)
-    p_out.insert(make_pair(this_segment->first, this_frame));
+    p_out.insert(make_pair(this_segment->first, tf::Stamped<KDL::Frame>(this_frame, ros::Time(), previous_frame.frame_id_)));
 
   // get poses of child segments
-  for (vector<SegmentMap::const_iterator>::const_iterator child=this_segment->second.children.begin(); child !=this_segment->second.children.end(); child++)
-    addFrameToMap(q_in, p_out, this_frame, *child);
-
+  for (vector<SegmentMap::const_iterator>::const_iterator child=this_segment->second.children.begin(); child !=this_segment->second.children.end(); child++){
+    if (flatten_tree)
+      addFrameToMap(q_in, p_out, this_frame, *child, flatten_tree);
+    else
+      addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), this_segment->first), *child, flatten_tree);
+  }      
 }
 
-
 }