diff --git a/CMakeLists.txt b/CMakeLists.txt
index 657fdbb5ca9209a75475fb94cf7241e47d1300cc..cc82036cc42d8255d0d9c69081eb707066af82ca 100644
--- a/CMakeLists.txt
+++ b/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
# ********************************************************************
diff --git a/cfg/DynamicTransformPublisher.cfg b/cfg/DynamicTransformPublisher.cfg
index ba21b11ba88915696268c05716f820f7d3f865c1..6116fd939a5fee39b1babe59c125ea21c16905ff 100755
--- a/cfg/DynamicTransformPublisher.cfg
+++ b/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)
diff --git a/include/dynamic_transform_publisher_alg_node.h b/include/dynamic_transform_publisher_alg_node.h
index 57f1886456b93c88c3efcd7023e1268c5ee97ae9..eea33916c32f270e8d2ec09b97f49e7313319fb9 100644
--- a/include/dynamic_transform_publisher_alg_node.h
+++ b/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
diff --git a/package.xml b/package.xml
index 6f0947fc7beee9fb7eb2823eeb617386897cfc3b..5c9be8a1feb83521e17e5565eefa2e57436d57ac 100644
--- a/package.xml
+++ b/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>
diff --git a/param/params.yaml b/param/params.yaml
index 495c72d0d95caf335f5882c63ac548f89e16679f..30f019a6244eddb342cc42da68ff0667abde28d6 100644
--- a/param/params.yaml
+++ b/param/params.yaml
@@ -1,10 +1,10 @@
+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
diff --git a/src/dynamic_transform_publisher_alg_node.cpp b/src/dynamic_transform_publisher_alg_node.cpp
index d02872a8918388c97eec6d6e3e5b1799a352b18e..fa9a38d8ed4db3027a410910199407ff3a12b15c 100644
--- a/src/dynamic_transform_publisher_alg_node.cpp
+++ b/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();