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

39
robot_state_publisher/src/robot_state_publisher.cpp
View File

@ -60,12 +60,9 @@ RobotStatePublisher::RobotStatePublisher(const Tree& tree)
NodeHandle n; NodeHandle n;
tf_publisher_ = n.advertise<tf::tfMessage>("/tf", 5); 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(); SegmentMap::const_iterator root = tree.getRootSegment();
if (root->second.children.empty()) root_ = root->first;
throw empty_tree_ex;
root_ = (*root->second.children.begin())->first;
} }
@ -79,31 +76,19 @@ bool RobotStatePublisher::publishTransforms(const map<string, double>& joint_pos
ROS_ERROR("Could not compute link poses. The tree or the state is invalid."); ROS_ERROR("Could not compute link poses. The tree or the state is invalid.");
return false; 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 // 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++){ for (map<string, Frame>::const_iterator f=link_poses.begin(); f!=link_poses.end(); f++){
if (f != root){ tf::TransformKDLToTF(f->second, tf_frame);
Frame frame = offset * f->second; trans.header.stamp = time;
tf::Transform tf_frame; trans.header.frame_id = tf::remap(tf_prefix_, root_);
tf::TransformKDLToTF(frame, tf_frame); trans.child_frame_id = tf::remap(tf_prefix_, f->first);
trans.header.stamp = time; tf::transformTFToMsg(tf_frame, trans.transform);
trans.header.frame_id = tf::remap(tf_prefix_, root->first); tf_msg_.transforms[i++] = trans;
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_); tf_publisher_.publish(tf_msg_);

7
robot_state_publisher/src/robot_state_publisher_node.cpp
View File

@ -69,7 +69,12 @@ int main(int argc, char** argv)
return -1; return -1;
} }
JointStateListener state_publisher(tree); 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(); ros::spin();
return 0; return 0;