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

make robot state publisher deal with an incomplete joint state

This commit is contained in:
meeussen 2009-10-26 19:49:56 +00:00
parent 56c049a901
commit 617c525494
3 changed files with 508 additions and 244 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
robot_state_publisher/src/robot_state_publisher.cpp
View File

@ -59,7 +59,6 @@ RobotStatePublisher::RobotStatePublisher(const Tree& tree)
// advertise tf message // advertise tf message
NodeHandle n; NodeHandle n;
tf_publisher_ = n.advertise<tf::tfMessage>("/tf", 5); tf_publisher_ = n.advertise<tf::tfMessage>("/tf", 5);
tf_msg_.transforms.resize(tree.getNrOfSegments()-1);
// get the 'real' root segment of the tree, which is the first child of "root" // get the 'real' root segment of the tree, which is the first child of "root"
SegmentMap::const_iterator root = tree.getRootSegment(); SegmentMap::const_iterator root = tree.getRootSegment();
@ -76,6 +75,11 @@ bool RobotStatePublisher::publishTransforms(const map<string, double>& joint_pos
// calculate transforms form root to every segment in tree // calculate transforms form root to every segment in tree
map<string, Frame> link_poses; map<string, Frame> link_poses;
solver_->JntToCart(joint_positions, link_poses); solver_->JntToCart(joint_positions, link_poses);
if (link_poses.size() < 2){
ROS_ERROR("Could not compute link poses. The tree or the state is invalid.");
return false;
}
tf_msg_.transforms.resize(link_poses.size()-1);
// publish the transforms to tf, converting the transforms from "root" to the 'real' root // publish the transforms to tf, converting the transforms from "root" to the 'real' root
geometry_msgs::TransformStamped trans; geometry_msgs::TransformStamped trans;

2
robot_state_publisher/src/treefksolverposfull_recursive.cpp
View File

@ -58,7 +58,7 @@ void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_i
if (this_segment->second.segment.getJoint().getType() != Joint::None){ 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()); map<string, double>::const_iterator jnt_pos = q_in.find(this_segment->second.segment.getJoint().getName());
if (jnt_pos == q_in.end()){ if (jnt_pos == q_in.end()){
printf("Could not find value for joint %s\n", this_segment->first.c_str()); printf("Warning: TreeFKSolverPosFull Could not find value for joint '%s'. Skipping this tree branch\n", this_segment->first.c_str());
return; return;
} }
jnt_p = jnt_pos->second; jnt_p = jnt_pos->second;

740
robot_state_publisher/test/pr2.urdf
View File

File diff suppressed because it is too large Load Diff