Port to tf2 and enable using static broadcaster

CMakeLists.txt

@@ -3,14 +3,14 @@ project(robot_state_publisher)
 
 find_package(orocos_kdl REQUIRED) 
 find_package(catkin REQUIRED
-  COMPONENTS roscpp rosconsole rostime tf tf_conversions kdl_parser cmake_modules
+  COMPONENTS roscpp rosconsole rostime tf tf2_ros tf2_geometry_msgs kdl_parser cmake_modules
 )
 find_package(Eigen REQUIRED)
 
 catkin_package(
   LIBRARIES ${PROJECT_NAME}_solver
   INCLUDE_DIRS include
-  DEPENDS roscpp rosconsole rostime tf tf_conversions kdl_parser orocos_kdl
+  DEPENDS roscpp rosconsole rostime tf2_ros tf2_geometry_msgs kdl_parser orocos_kdl
 )
 
 include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS})

include/robot_state_publisher/joint_state_listener.h

@@ -74,6 +74,7 @@ class JointStateListener{
   ros::Time last_callback_time_;
   std::map<std::string, ros::Time> last_publish_time_;
   MimicMap mimic_;
+  bool use_tf_static_;
 
 };
 }

include/robot_state_publisher/robot_state_publisher.h

@@ -40,7 +40,8 @@
 #include <ros/ros.h>
 #include <boost/scoped_ptr.hpp>
 #include <tf/tf.h>
-#include <tf/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.h>
 #include <kdl/frames.hpp>
 #include <kdl/segment.hpp>
 #include <kdl/tree.hpp>
@@ -74,14 +75,15 @@ class RobotStatePublisher
    * \param time The time at which the joint positions were recorded
    */
   void publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time, const std::string& tf_prefix);
-  void publishFixedTransforms(const std::string& tf_prefix);
+  void publishFixedTransforms(const std::string& tf_prefix, bool use_tf_static);
 
 private:
   void addChildren(const KDL::SegmentMap::const_iterator segment);
 
 
   std::map<std::string, SegmentPair> segments_, segments_fixed_;
-  tf::TransformBroadcaster tf_broadcaster_;
+  tf2_ros::TransformBroadcaster tf_broadcaster_;
+  tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
 };
 
 

include/robot_state_publisher/treefksolverposfull_recursive.hpp

@@ -24,7 +24,7 @@
 #define KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP
 
 #include <kdl/tree.hpp>
-#include <tf/transform_datatypes.h>
+#include <tf2/transform_datatypes.h>
 
 
 namespace KDL {
@@ -36,12 +36,12 @@ class TreeFkSolverPosFull_recursive
   TreeFkSolverPosFull_recursive(const Tree& _tree);
   ~TreeFkSolverPosFull_recursive();
 
-  int JntToCart(const std::map<std::string, double>& q_in, std::map<std::string, tf::Stamped<Frame> >& p_out, bool flatten_tree=true);
+  int JntToCart(const std::map<std::string, double>& q_in, std::map<std::string, tf2::Stamped<Frame> >& p_out, bool flatten_tree=true);
 
 private:
   void addFrameToMap(const std::map<std::string, double>& q_in, 
-		     std::map<std::string, tf::Stamped<KDL::Frame> >& p_out,
-		     const tf::Stamped<KDL::Frame>& previous_frame,
+		     std::map<std::string, tf2::Stamped<KDL::Frame> >& p_out,
+		     const tf2::Stamped<KDL::Frame>& previous_frame,
 		     const SegmentMap::const_iterator this_segment,
 		     bool flatten_tree);
 

package.xml

@@ -28,7 +28,8 @@
   <build_depend>rostime</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>tf</build_depend>
-  <build_depend>tf_conversions</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
   <build_depend>cmake_modules</build_depend>
 
   <run_depend>catkin</run_depend>
@@ -40,5 +41,6 @@
   <run_depend>rostime</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>tf</run_depend>
-  <run_depend>tf_conversions</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
 </package>

src/joint_state_listener.cpp

@@ -56,6 +56,8 @@ JointStateListener::JointStateListener(const KDL::Tree& tree, const MimicMap& m)
   // set publish frequency
   double publish_freq;
   n_tilde.param("publish_frequency", publish_freq, 50.0);
+  // set whether to use the /tf_static latched static transform broadcaster
+  n_tilde.param("use_tf_static", use_tf_static_, false);
   // get the tf_prefix parameter from the closest namespace
   std::string tf_prefix_key;
   n_tilde.searchParam("tf_prefix", tf_prefix_key);
@@ -66,7 +68,8 @@ JointStateListener::JointStateListener(const KDL::Tree& tree, const MimicMap& m)
   joint_state_sub_ = n.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);
 
   // trigger to publish fixed joints
-  timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this);
+  // if using static transform broadcaster, this will be a oneshot trigger and only run once
+  timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this, use_tf_static_);
 
 };
 
@@ -77,7 +80,7 @@ JointStateListener::~JointStateListener()
 
 void JointStateListener::callbackFixedJoint(const ros::TimerEvent& e)
 {
-  state_publisher_.publishFixedTransforms(tf_prefix_);
+  state_publisher_.publishFixedTransforms(tf_prefix_, use_tf_static_);
 }
 
 void JointStateListener::callbackJointState(const JointStateConstPtr& state)

src/robot_state_publisher.cpp

@@ -36,7 +36,8 @@
 
 #include "robot_state_publisher/robot_state_publisher.h"
 #include <kdl/frames_io.hpp>
-#include <tf_conversions/tf_kdl.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <geometry_msgs/TransformStamped.h>
 
 using namespace std;
 using namespace ros;
@@ -78,40 +79,42 @@ namespace robot_state_publisher{
   void RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time, const std::string& tf_prefix)
   {
     ROS_DEBUG("Publishing transforms for moving joints");
-    std::vector<tf::StampedTransform> tf_transforms;
-    tf::StampedTransform tf_transform;
-    tf_transform.stamp_ = time;
+    std::vector<geometry_msgs::TransformStamped> tf_transforms;
 
     // 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_ = tf::resolve(tf_prefix, seg->second.root);
-        tf_transform.child_frame_id_ = tf::resolve(tf_prefix, seg->second.tip);
+        geometry_msgs::TransformStamped tf_transform = tf2::kdlToGmTransform(seg->second.segment.pose(jnt->second));
+        tf_transform.header.stamp = time;
+        tf_transform.header.frame_id = tf::resolve(tf_prefix, seg->second.root);
+        tf_transform.child_frame_id = tf::resolve(tf_prefix, seg->second.tip);
         tf_transforms.push_back(tf_transform);
       }
     }
     tf_broadcaster_.sendTransform(tf_transforms);
   }
 
-
   // publish fixed transforms
-  void RobotStatePublisher::publishFixedTransforms(const std::string& tf_prefix)
+  void RobotStatePublisher::publishFixedTransforms(const std::string& tf_prefix, bool use_tf_static)
   {
     ROS_DEBUG("Publishing transforms for fixed joints");
-    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
+    std::vector<geometry_msgs::TransformStamped> tf_transforms;
+    geometry_msgs::TransformStamped tf_transform;
 
     // 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_ = tf::resolve(tf_prefix, seg->second.root);
-      tf_transform.child_frame_id_ = tf::resolve(tf_prefix, seg->second.tip);
+      geometry_msgs::TransformStamped tf_transform = tf2::kdlToGmTransform(seg->second.segment.pose(0));
+      tf_transform.header.stamp = ros::Time::now();
+      tf_transform.header.frame_id = tf::resolve(tf_prefix, seg->second.root);
+      tf_transform.child_frame_id = tf::resolve(tf_prefix, seg->second.tip);
       tf_transforms.push_back(tf_transform);
     }
-    tf_broadcaster_.sendTransform(tf_transforms);
+    if(use_tf_static){
+      static_tf_broadcaster_.sendTransform(tf_transforms);
+    }else{
+      tf_broadcaster_.sendTransform(tf_transforms);
+    }
   }
 
 }

src/treefksolverposfull_recursive.cpp

@@ -39,12 +39,12 @@ TreeFkSolverPosFull_recursive::~TreeFkSolverPosFull_recursive()
 }
 
 
-int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, map<string, tf::Stamped<Frame> >& p_out, bool flatten_tree)
+int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, map<string, tf2::Stamped<Frame> >& p_out, bool flatten_tree)
 {      
   // clear output
   p_out.clear();
 
-  addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), GetTreeElementSegment(tree.getRootSegment()->second).getName()),
+  addFrameToMap(q_in, p_out, tf2::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), GetTreeElementSegment(tree.getRootSegment()->second).getName()),
                 tree.getRootSegment(), flatten_tree);
 
   return 0;
@@ -53,13 +53,13 @@ int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, ma
 
 
 void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_in, 
-						  map<string, tf::Stamped<Frame> >& p_out,
-						  const tf::Stamped<KDL::Frame>& previous_frame,
+						  map<string, tf2::Stamped<Frame> >& p_out,
+						  const tf2::Stamped<KDL::Frame>& previous_frame,
 						  const SegmentMap::const_iterator this_segment,
 						  bool flatten_tree)
 {
   // get pose of this segment
-  tf::Stamped<KDL::Frame> this_frame;
+  tf2::Stamped<KDL::Frame> this_frame;
   double jnt_p = 0;
   if (GetTreeElementSegment(this_segment->second).getJoint().getType() != Joint::None){
     map<string, double>::const_iterator jnt_pos = q_in.find(GetTreeElementSegment(this_segment->second).getJoint().getName());
@@ -69,18 +69,18 @@ void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_i
     }
     jnt_p = jnt_pos->second;
   }
-  this_frame = tf::Stamped<KDL::Frame>(previous_frame * GetTreeElementSegment(this_segment->second).pose(jnt_p), ros::Time(), previous_frame.frame_id_);
+  this_frame = tf2::Stamped<KDL::Frame>(previous_frame * GetTreeElementSegment(this_segment->second).pose(jnt_p), ros::Time(), previous_frame.frame_id_);
 
   if (this_segment->first != tree.getRootSegment()->first)
-    p_out.insert(make_pair(this_segment->first, tf::Stamped<KDL::Frame>(this_frame, ros::Time(), previous_frame.frame_id_)));
+    p_out.insert(make_pair(this_segment->first, tf2::Stamped<KDL::Frame>(this_frame, ros::Time(), previous_frame.frame_id_)));
 
   // get poses of child segments
   for (vector<SegmentMap::const_iterator>::const_iterator child = GetTreeElementChildren(this_segment->second).begin();
         child != GetTreeElementChildren(this_segment->second).end(); child++){
     if (flatten_tree)
       addFrameToMap(q_in, p_out, this_frame, *child, flatten_tree);
     else
-      addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), this_segment->first), *child, flatten_tree);
+      addFrameToMap(q_in, p_out, tf2::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), this_segment->first), *child, flatten_tree);
   }      
 }