From 5e0e8f7f9d442e5f5b467c28c2bf5f5b432c8b3f Mon Sep 17 00:00:00 2001 From: wim Date: Wed, 29 Sep 2010 17:58:03 +0000 Subject: [PATCH] Robot state publisher supports full trees and flattened trees --- .../robot_state_publisher.h | 1 + .../treefksolverposfull_recursive.hpp | 11 +++++-- .../src/robot_state_publisher.cpp | 22 +++++++++---- .../src/treefksolverposfull_recursive.cpp | 33 +++++++++++-------- 4 files changed, 43 insertions(+), 24 deletions(-) diff --git a/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h b/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h index 67c68be..5d0b7ac 100644 --- a/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h +++ b/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 solver_; std::string root_; diff --git a/robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp b/robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp index 4c3e9ea..1d11b52 100644 --- a/robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp +++ b/robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp @@ -24,6 +24,8 @@ #define KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP #include +#include + namespace KDL { @@ -34,11 +36,14 @@ public: TreeFkSolverPosFull_recursive(const Tree& _tree); ~TreeFkSolverPosFull_recursive(); - int JntToCart(const std::map& q_in, std::map& p_out); + int JntToCart(const std::map& q_in, std::map >& p_out, bool flatten_tree=true); private: - void addFrameToMap(const std::map& q_in, std::map& p_out, - const Frame& previous_frame, const SegmentMap::const_iterator this_segment); + void addFrameToMap(const std::map& q_in, + std::map >& p_out, + const tf::Stamped& previous_frame, + const SegmentMap::const_iterator this_segment, + bool flatten_tree); Tree tree; diff --git a/robot_state_publisher/src/robot_state_publisher.cpp b/robot_state_publisher/src/robot_state_publisher.cpp index ffb0405..8faadd2 100644 --- a/robot_state_publisher/src/robot_state_publisher.cpp +++ b/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", 5); @@ -65,27 +69,31 @@ RobotStatePublisher::RobotStatePublisher(const Tree& tree) bool RobotStatePublisher::publishTransforms(const map& joint_positions, const Time& time) { - // calculate transforms form root to every segment in tree - map link_poses; - solver_->JntToCart(joint_positions, link_poses); + // calculate transforms + map > 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::const_iterator f=link_poses.begin(); f!=link_poses.end(); f++){ + tf::Transform tf_frame; + for (map >::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; } } + + + diff --git a/robot_state_publisher/src/treefksolverposfull_recursive.cpp b/robot_state_publisher/src/treefksolverposfull_recursive.cpp index fd6fcd3..c4700cd 100644 --- a/robot_state_publisher/src/treefksolverposfull_recursive.cpp +++ b/robot_state_publisher/src/treefksolverposfull_recursive.cpp @@ -38,23 +38,27 @@ TreeFkSolverPosFull_recursive::~TreeFkSolverPosFull_recursive() { } -int TreeFkSolverPosFull_recursive::JntToCart(const map& q_in, map& p_out) + +int TreeFkSolverPosFull_recursive::JntToCart(const map& q_in, map >& 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::Identity(), ros::Time(), tree.getRootSegment()->second.segment.getName()), tree.getRootSegment(), flatten_tree); return 0; } - -void TreeFkSolverPosFull_recursive::addFrameToMap(const map& q_in, map& p_out, - const Frame& previous_frame, const SegmentMap::const_iterator this_segment) + +void TreeFkSolverPosFull_recursive::addFrameToMap(const map& q_in, + map >& p_out, + const tf::Stamped& previous_frame, + const SegmentMap::const_iterator this_segment, + bool flatten_tree) { // get pose of this segment - Frame this_frame; + tf::Stamped this_frame; double jnt_p = 0; if (this_segment->second.segment.getJoint().getType() != Joint::None){ map::const_iterator jnt_pos = q_in.find(this_segment->second.segment.getJoint().getName()); @@ -64,17 +68,18 @@ void TreeFkSolverPosFull_recursive::addFrameToMap(const map& 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(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(this_frame, ros::Time(), previous_frame.frame_id_))); // get poses of child segments - for (vector::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::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::Identity(), ros::Time(), this_segment->first), *child, flatten_tree); + } } - }