bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

use new tf broadcaster with vectors

This commit is contained in:
wim 2010-04-23 23:18:33 +00:00
parent c8cb7dce19
commit e5fc479762
2 changed files with 10 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h
View File

@ -40,7 +40,7 @@
#include <ros/ros.h>
#include <boost/scoped_ptr.hpp>
#include <tf/tf.h>
#include <tf/tfMessage.h>
#include <tf/transform_broadcaster.h>
#include "robot_state_publisher/treefksolverposfull_recursive.hpp"
namespace robot_state_publisher{
@ -68,8 +68,8 @@ private:
KDL::Tree tree_;
boost::scoped_ptr<KDL::TreeFkSolverPosFull_recursive> solver_;
std::string root_;
std::string tf_prefix_;
tf::tfMessage tf_msg_;
std::vector<tf::StampedTransform> transforms_;
tf::TransformBroadcaster tf_broadcaster_;
class empty_tree_exception: public std::exception{
virtual const char* what() const throw(){

22
robot_state_publisher/src/robot_state_publisher.cpp
View File

@ -49,12 +49,6 @@ namespace robot_state_publisher{
RobotStatePublisher::RobotStatePublisher(const Tree& tree)
:tree_(tree)
{
// get tf prefix
NodeHandle n_local("~");
std::string searched_param;
n_local.searchParam("tf_prefix", searched_param);
n_local.param(searched_param, tf_prefix_, std::string());
// build tree solver
solver_.reset(new TreeFkSolverPosFull_recursive(tree_));
@ -78,21 +72,19 @@ bool RobotStatePublisher::publishTransforms(const map<string, double>& joint_pos
ROS_ERROR("Could not compute link poses. The tree or the state is invalid.");
return false;
}
tf_msg_.transforms.resize(link_poses.size());
transforms_.resize(link_poses.size());
// send transforms to tf
geometry_msgs::TransformStamped trans;
tf::Transform tf_frame;
unsigned int i = 0;
for (map<string, Frame>::const_iterator f=link_poses.begin(); f!=link_poses.end(); f++){
tf::TransformKDLToTF(f->second, tf_frame);
trans.header.stamp = time;
trans.header.frame_id = tf::resolve(tf_prefix_, root_);
trans.child_frame_id = tf::resolve(tf_prefix_, f->first);
tf::transformTFToMsg(tf_frame, trans.transform);
tf_msg_.transforms[i++] = trans;
tf::TransformKDLToTF(f->second, transforms_[i]);
transforms_[i].stamp_ = time;
transforms_[i].frame_id_ = root_;
transforms_[i].child_frame_id_ = f->first;
i++;
}
tf_publisher_.publish(tf_msg_);
tf_broadcaster_.sendTransform(transforms_);
return true;
}