support publishing of partial trees. Ticket #4464
This commit is contained in:
Wim Meeussen
parent
f9c4811229
commit
0cac9e781a
|
|
@ -62,11 +62,14 @@ public:
|
|||
|
||||
private:
|
||||
void callbackJointState(const JointStateConstPtr& state);
|
||||
void callbackFixedJoint(const ros::TimerEvent& e);
|
||||
|
||||
NodeHandle n_, n_tilde_;
|
||||
Rate publish_rate_;
|
||||
Duration publish_interval_;
|
||||
robot_state_publisher::RobotStatePublisher state_publisher_;
|
||||
Subscriber joint_state_sub_;
|
||||
ros::Timer timer_;
|
||||
ros::Time last_publish_time_;
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -41,10 +41,23 @@
|
|||
#include <boost/scoped_ptr.hpp>
|
||||
#include <tf/tf.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include "robot_state_publisher/treefksolverposfull_recursive.hpp"
|
||||
#include <kdl/frames.hpp>
|
||||
#include <kdl/segment.hpp>
|
||||
#include <kdl/tree.hpp>
|
||||
|
||||
namespace robot_state_publisher{
|
||||
|
||||
class SegmentPair
|
||||
{
|
||||
public:
|
||||
SegmentPair(const KDL::Segment& p_segment, const std::string& p_root, const std::string& p_tip):
|
||||
segment(p_segment), root(p_root), tip(p_tip){}
|
||||
|
||||
KDL::Segment segment;
|
||||
std::string root, tip;
|
||||
};
|
||||
|
||||
|
||||
class RobotStatePublisher
|
||||
{
|
||||
public:
|
||||
|
|
@ -62,21 +75,14 @@ public:
|
|||
* returns true on success; return false when the robot model is empty or not all the joints in the robot model are specified in the joint map.
|
||||
*/
|
||||
bool publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time);
|
||||
void publishFixedTransforms();
|
||||
|
||||
private:
|
||||
ros::Publisher tf_publisher_;
|
||||
bool flatten_tree_;
|
||||
KDL::Tree tree_;
|
||||
boost::scoped_ptr<KDL::TreeFkSolverPosFull_recursive> solver_;
|
||||
std::string root_;
|
||||
std::vector<tf::StampedTransform> transforms_;
|
||||
tf::TransformBroadcaster tf_broadcaster_;
|
||||
void addChildren(const KDL::SegmentMap::const_iterator segment);
|
||||
|
||||
class empty_tree_exception: public std::exception{
|
||||
virtual const char* what() const throw(){
|
||||
return "Tree is empty";}
|
||||
} empty_tree_ex;
|
||||
|
||||
std::map<std::string, SegmentPair> segments_, segments_fixed_;
|
||||
tf::TransformBroadcaster tf_broadcaster_, tf_broadcaster_fixed_;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -48,25 +48,24 @@ using namespace robot_state_publisher;
|
|||
|
||||
|
||||
JointStateListener::JointStateListener(const KDL::Tree& tree)
|
||||
: n_tilde_("~"), publish_rate_(0.0), state_publisher_(tree)
|
||||
: state_publisher_(tree)
|
||||
{
|
||||
ros::NodeHandle n_tilde("~");
|
||||
ros::NodeHandle n;
|
||||
|
||||
// set publish frequency
|
||||
double publish_freq;
|
||||
n_tilde_.param("publish_frequency", publish_freq, 50.0);
|
||||
publish_rate_ = Rate(publish_freq);
|
||||
n_tilde.param("publish_frequency", publish_freq, 50.0);
|
||||
publish_interval_ = ros::Duration(1.0/max(publish_freq,1.0));
|
||||
|
||||
|
||||
// subscribe to joint state
|
||||
joint_state_sub_ = n.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);
|
||||
|
||||
// trigger to publish fixed joints
|
||||
last_publish_time_ = ros::Time();
|
||||
timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this);
|
||||
|
||||
if (tree.getNrOfJoints() == 0){
|
||||
boost::shared_ptr<sensor_msgs::JointState> empty_state(new sensor_msgs::JointState);
|
||||
while (ros::ok()){
|
||||
empty_state->header.stamp = ros::Time::now();
|
||||
this->callbackJointState(empty_state);
|
||||
publish_rate_.sleep();
|
||||
}
|
||||
}
|
||||
else{
|
||||
// subscribe to mechanism state
|
||||
joint_state_sub_ = n_.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -74,6 +73,11 @@ JointStateListener::~JointStateListener()
|
|||
{};
|
||||
|
||||
|
||||
void JointStateListener::callbackFixedJoint(const ros::TimerEvent& e)
|
||||
{
|
||||
state_publisher_.publishFixedTransforms();
|
||||
}
|
||||
|
||||
void JointStateListener::callbackJointState(const JointStateConstPtr& state)
|
||||
{
|
||||
if (state->name.size() != state->position.size()){
|
||||
|
|
@ -81,12 +85,15 @@ void JointStateListener::callbackJointState(const JointStateConstPtr& state)
|
|||
return;
|
||||
}
|
||||
|
||||
// get joint positions from state message
|
||||
map<string, double> joint_positions;
|
||||
for (unsigned int i=0; i<state->name.size(); i++)
|
||||
joint_positions.insert(make_pair(state->name[i], state->position[i]));
|
||||
state_publisher_.publishTransforms(joint_positions, state->header.stamp);
|
||||
publish_rate_.sleep();
|
||||
// check if we need to publish
|
||||
if (state->header.stamp >= last_publish_time_ + publish_interval_){
|
||||
// get joint positions from state message
|
||||
map<string, double> joint_positions;
|
||||
for (unsigned int i=0; i<state->name.size(); i++)
|
||||
joint_positions.insert(make_pair(state->name[i], state->position[i]));
|
||||
state_publisher_.publishTransforms(joint_positions, state->header.stamp);
|
||||
last_publish_time_ = state->header.stamp;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -109,14 +116,8 @@ 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");
|
||||
ros::spin();
|
||||
}
|
||||
else{
|
||||
JointStateListener state_publisher(tree);
|
||||
ros::spin();
|
||||
}
|
||||
JointStateListener state_publisher(tree);
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,36 +1,36 @@
|
|||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Author: Wim Meeussen */
|
||||
|
||||
|
|
@ -40,59 +40,83 @@
|
|||
|
||||
using namespace std;
|
||||
using namespace ros;
|
||||
using namespace KDL;
|
||||
|
||||
|
||||
|
||||
namespace robot_state_publisher{
|
||||
|
||||
RobotStatePublisher::RobotStatePublisher(const Tree& tree)
|
||||
: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::tfMessage>("/tf", 5);
|
||||
|
||||
// get the root segment of the tree
|
||||
SegmentMap::const_iterator root = tree.getRootSegment();
|
||||
root_ = root->first;
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time)
|
||||
{
|
||||
// calculate transforms
|
||||
map<string, tf::Stamped<Frame> > 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());
|
||||
|
||||
unsigned int i = 0;
|
||||
tf::Transform tf_frame;
|
||||
for (map<string, tf::Stamped<Frame> >::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_ = f->second.frame_id_;
|
||||
transforms_[i].child_frame_id_ = f->first;
|
||||
i++;
|
||||
RobotStatePublisher::RobotStatePublisher(const KDL::Tree& tree)
|
||||
{
|
||||
// walk the tree and add segments to segments_
|
||||
addChildren(tree.getRootSegment());
|
||||
}
|
||||
|
||||
tf_broadcaster_.sendTransform(transforms_);
|
||||
|
||||
return true;
|
||||
}
|
||||
// add children to correct maps
|
||||
void RobotStatePublisher::addChildren(const KDL::SegmentMap::const_iterator segment)
|
||||
{
|
||||
const std::string& root = segment->second.segment.getName();
|
||||
|
||||
const std::vector<KDL::SegmentMap::const_iterator>& children = segment->second.children;
|
||||
for (unsigned int i=0; i<children.size(); i++){
|
||||
const KDL::Segment& child = children[i]->second.segment;
|
||||
SegmentPair s(children[i]->second.segment, root, child.getName());
|
||||
if (child.getJoint().getType() == KDL::Joint::None){
|
||||
segments_fixed_.insert(make_pair(child.getJoint().getName(), s));
|
||||
ROS_INFO("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
|
||||
}
|
||||
else{
|
||||
segments_.insert(make_pair(child.getJoint().getName(), s));
|
||||
ROS_INFO("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
|
||||
}
|
||||
addChildren(children[i]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// publish moving transforms
|
||||
bool RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time)
|
||||
{
|
||||
std::vector<tf::StampedTransform> tf_transforms;
|
||||
|
||||
tf::StampedTransform tf_transform;
|
||||
tf_transform.stamp_ = time;
|
||||
|
||||
// loop over all joints
|
||||
for (map<string, double>::const_iterator jnt=joint_positions.begin(); jnt != joint_positions.end(); jnt++){
|
||||
std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jnt->first);
|
||||
if (seg != segments_.end()){
|
||||
tf::TransformKDLToTF(seg->second.segment.pose(jnt->second), tf_transform);
|
||||
tf_transform.frame_id_ = seg->second.root;
|
||||
tf_transform.child_frame_id_ = seg->second.tip;
|
||||
tf_transforms.push_back(tf_transform);
|
||||
}
|
||||
}
|
||||
|
||||
tf_broadcaster_.sendTransform(tf_transforms);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
// publish fixed transforms
|
||||
void RobotStatePublisher::publishFixedTransforms()
|
||||
{
|
||||
std::vector<tf::StampedTransform> tf_transforms;
|
||||
|
||||
tf::StampedTransform tf_transform;
|
||||
tf_transform.stamp_ = ros::Time::now()+ros::Duration(0.5); // future publish by 0.5 seconds
|
||||
|
||||
// loop over all fixed segments
|
||||
for (map<string, SegmentPair>::const_iterator seg=segments_fixed_.begin(); seg != segments_fixed_.end(); seg++){
|
||||
tf::TransformKDLToTF(seg->second.segment.pose(0), tf_transform);
|
||||
tf_transform.frame_id_ = seg->second.root;
|
||||
tf_transform.child_frame_id_ = seg->second.tip;
|
||||
tf_transforms.push_back(tf_transform);
|
||||
}
|
||||
tf_broadcaster_fixed_.sendTransform(tf_transforms);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue