diff --git a/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h b/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h index 2990c4c..2c327c5 100644 --- a/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h +++ b/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h @@ -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_; + }; } diff --git a/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h b/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h index 5d0b7ac..0355c13 100644 --- a/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h +++ b/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h @@ -41,10 +41,23 @@ #include #include #include -#include "robot_state_publisher/treefksolverposfull_recursive.hpp" +#include +#include +#include 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& joint_positions, const ros::Time& time); + void publishFixedTransforms(); private: - ros::Publisher tf_publisher_; - bool flatten_tree_; - KDL::Tree tree_; - boost::scoped_ptr solver_; - std::string root_; - std::vector 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 segments_, segments_fixed_; + tf::TransformBroadcaster tf_broadcaster_, tf_broadcaster_fixed_; }; diff --git a/robot_state_publisher/src/joint_state_listener.cpp b/robot_state_publisher/src/joint_state_listener.cpp index 84f4875..1823da2 100644 --- a/robot_state_publisher/src/joint_state_listener.cpp +++ b/robot_state_publisher/src/joint_state_listener.cpp @@ -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)); - if (tree.getNrOfJoints() == 0){ - boost::shared_ptr 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); - } + + // 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); + }; @@ -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 joint_positions; - for (unsigned int i=0; iname.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 joint_positions; + for (unsigned int i=0; iname.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; } diff --git a/robot_state_publisher/src/robot_state_publisher.cpp b/robot_state_publisher/src/robot_state_publisher.cpp index 8faadd2..6202394 100644 --- a/robot_state_publisher/src/robot_state_publisher.cpp +++ b/robot_state_publisher/src/robot_state_publisher.cpp @@ -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", 5); - - // get the root segment of the tree - SegmentMap::const_iterator root = tree.getRootSegment(); - root_ = root->first; -} - - - -bool RobotStatePublisher::publishTransforms(const map& joint_positions, const Time& time) -{ - // calculate transforms - map > 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 >::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& children = segment->second.children; + for (unsigned int i=0; isecond.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& joint_positions, const Time& time) + { + std::vector tf_transforms; + + tf::StampedTransform tf_transform; + tf_transform.stamp_ = time; + + // loop over all joints + for (map::const_iterator jnt=joint_positions.begin(); jnt != joint_positions.end(); jnt++){ + std::map::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_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::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); + } + }