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

publish tf from root of robot

This commit is contained in:
wim 2009-11-24 21:44:17 +00:00
parent f641312f0a
commit 3117f697bc
2 changed files with 18 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
robot_state_publisher/src/robot_state_publisher.cpp
View File

@ -60,12 +60,9 @@ RobotStatePublisher::RobotStatePublisher(const Tree& tree)
NodeHandle n;
tf_publisher_ = n.advertise<tf::tfMessage>("/tf", 5);
// get the 'real' root segment of the tree, which is the first child of "root"
// get the root segment of the tree
SegmentMap::const_iterator root = tree.getRootSegment();
if (root->second.children.empty())
throw empty_tree_ex;
root_ = (*root->second.children.begin())->first;
root_ = root->first;
}
@ -79,32 +76,20 @@ 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()-1);
tf_msg_.transforms.resize(link_poses.size());
// publish the transforms to tf, converting the transforms from "root" to the 'real' root
geometry_msgs::TransformStamped trans;
map<string, Frame>::const_iterator root = link_poses.find(root_);
if (root == link_poses.end()){
ROS_ERROR("Did not find root of tree");
return false;
}
// remove root from published poses
Frame offset = root->second.Inverse();
unsigned int i = 0;
// send transforms to tf
for (map<string, Frame>::const_iterator f=link_poses.begin(); f!=link_poses.end(); f++){
if (f != root){
Frame frame = offset * f->second;
geometry_msgs::TransformStamped trans;
tf::Transform tf_frame;
tf::TransformKDLToTF(frame, 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::remap(tf_prefix_, root->first);
trans.header.frame_id = tf::remap(tf_prefix_, root_);
trans.child_frame_id = tf::remap(tf_prefix_, f->first);
tf::transformTFToMsg(tf_frame, trans.transform);
tf_msg_.transforms[i++] = trans;
}
}
tf_publisher_.publish(tf_msg_);
return true;

5
robot_state_publisher/src/robot_state_publisher_node.cpp
View File

@ -69,6 +69,11 @@ int main(int argc, char** argv)
return -1;
}
if (tree.getNrOfSegments() == 0)
ROS_WARN("Robot state publisher got an empty tree and cannot publish any state to tf");
else if (tree.getNrOfSegments() == 1)
ROS_WARN("Robot state publisher got a tree with only one segment and cannot publish any state to tf");
else
JointStateListener state_publisher(tree);
ros::spin();