TF2 migration

CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm tf)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm tf2_ros)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -60,7 +60,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm tf
+ CATKIN_DEPENDS iri_base_algorithm tf2_ros
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 

cfg/DynamicTransformPublisher.cfg

@@ -38,10 +38,10 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
 
 #       Name                 Type      Reconf level   Description                     Default   Min   Max
+gen.add("rate",              double_t, 0,             "Main loop rate (Hz)",          10.0,     0.1,  1000.0)
 gen.add("broadcast",         bool_t,   0,             "enable broadcasting",          True)
 gen.add("parent_id",         str_t,    0,             "parent frame id",              "parent")
 gen.add("frame_id",          str_t,    0,             "child frame id",               "child")
-gen.add("period",            double_t, 0,             "period in ms",  100.0,         0.0,  10000.0)
 gen.add("reset_translation", bool_t,   0,             "reset x y z offsets to zero",  False)
 gen.add("x",                 double_t, 0,             "x offset in meters",           0.0,      -1000.0,  1000.0)
 gen.add("y",                 double_t, 0,             "y offset in meters",           0.0,      -1000.0,  1000.0)

include/dynamic_transform_publisher_alg_node.h

@@ -34,7 +34,9 @@
 
 // [action server client headers]
 
-#include <tf/transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+
 
 /**
  * \brief IRI ROS Specific Algorithm Class
@@ -54,8 +56,9 @@ class DynamicTransformPublisherAlgNode : public algorithm_base::IriBaseAlgorithm
     // [action server attributes]
 
     // [action client attributes]
-    geometry_msgs::TransformStamped transform;
-    tf::TransformBroadcaster tf_broadcaster;
+    tf2_ros::TransformBroadcaster tf2_broadcaster;
+    geometry_msgs::TransformStamped transform_msg;
+
 
    /**
     * \brief config variable

package.xml

@@ -52,7 +52,7 @@
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <depend>iri_base_algorithm</depend>
-  <depend>tf</depend>
+  <depend>tf2_ros</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
@@ -60,4 +60,4 @@
     <!-- Other tools can request additional information be placed here -->
 
   </export>
-</package>
\ No newline at end of file
+</package>

param/params.yaml

+rate: 10.0
 broadcast:    True
 parent_id:   'parent'
 frame_id:    'child'
-period:      100
 x:     0.0
 y:     0.0
 z:     1.0
 roll:  0.0
 pitch: 0.0
-yaw:   0.0
\ No newline at end of file
+yaw:   0.0

src/dynamic_transform_publisher_alg_node.cpp

@@ -38,8 +38,8 @@ void DynamicTransformPublisherAlgNode::mainNodeThread(void)
   this->alg_.lock();
   if(this->config_.broadcast)
   {
-    this->transform.header.stamp = ros::Time::now();
-    this->tf_broadcaster.sendTransform(this->transform);
+    this->transform_msg.header.stamp = ros::Time::now();
+    this->tf2_broadcaster.sendTransform(this->transform_msg);
   }
 
   this->alg_.unlock();
@@ -56,10 +56,13 @@ void DynamicTransformPublisherAlgNode::mainNodeThread(void)
 void DynamicTransformPublisherAlgNode::node_config_update(Config &config, uint32_t level)
 {
   this->alg_.lock();
-  
-  this->transform.header.stamp    = ros::Time::now();
-  this->transform.header.frame_id = config.parent_id;
-  this->transform.child_frame_id  = config.frame_id;
+
+  if(config.rate!=this->getRate())
+    this->setRate(config.rate);
+
+  this->transform_msg.header.stamp    = ros::Time::now();
+  this->transform_msg.header.frame_id = config.parent_id;
+  this->transform_msg.child_frame_id  = config.frame_id;
   
   if(config.reset_translation)
   {
@@ -79,17 +82,19 @@ void DynamicTransformPublisherAlgNode::node_config_update(Config &config, uint32
     config.pitch=0.0;
     config.yaw  =0.0;
   }
-  geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(config.roll,config.pitch,config.yaw);
-  t.rotation = q;
-  this->transform.transform  = t;
 
-  this->setRate(1000.0/config.period);
-  //this->loop_rate_ = 1000.0/config.period;
+  tf2::Quaternion quat_tf;
+  quat_tf.setRPY(config.roll, config.pitch, config.yaw);
+  geometry_msgs::Quaternion quat_msg;
+  tf2::convert(quat_tf, quat_msg);
+
+  t.rotation = quat_msg;
+  this->transform_msg.transform  = t;
   
   if(config.show_rosrun)
   {
     config.show_rosrun=false;
-    ROS_INFO("rosrun tf static_transform_publisher %f %f %f %f %f %f %s %s %f", config.x, config.y, config.z, config.yaw, config.pitch, config.roll, config.parent_id.c_str(), config.frame_id.c_str(), config.period);
+    ROS_INFO("rosrun tf2_ros static_transform_publisher %f %f %f %f %f %f %s %s", config.x, config.y, config.z, config.yaw, config.pitch, config.roll, config.parent_id.c_str(), config.frame_id.c_str());
   }
   this->config_=config;
   this->alg_.unlock();