Add dynamic reconfigure to static_tf_publisher

tf/CMakeLists.txt

@@ -4,7 +4,7 @@ project(tf)
 find_package(catkin REQUIRED)
 
 find_package(Boost REQUIRED thread signals)
-find_package(catkin REQUIRED angles geometry_msgs message_filters message_generation rosconsole roscpp rostest rostime sensor_msgs std_msgs)
+find_package(catkin REQUIRED angles dynamic_reconfigure geometry_msgs message_filters message_generation rosconsole roscpp rostest rostime sensor_msgs std_msgs)
 
 catkin_python_setup()
 
@@ -19,6 +19,8 @@ add_service_files(DIRECTORY srv FILES FrameGraph.srv)
 
 generate_messages(DEPENDENCIES geometry_msgs sensor_msgs std_msgs)
 
+generate_dynamic_reconfigure_options(cfg/TransformSender.cfg)
+
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}
@@ -28,6 +30,7 @@ catkin_package(
 add_library(${PROJECT_NAME} src/tf.cpp src/transform_listener.cpp src/cache.cpp src/transform_broadcaster.cpp)
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp)
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
 
 # Debug
 add_executable(tf_empty_listener src/empty_listener.cpp)

tf/cfg/TransformSender.cfg

@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+PACKAGE = "tf"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+from math import pi
+
+CHANGE_NOTHING = 0
+CHANGE_XYZ = 1 << 0
+CHANGE_RPY_RAD = 1 << 1
+CHANGE_RPY_DEG = 1 << 2
+CHANGE_QUAT = 1 << 3
+CHANGE_ALL = 0xffffffff
+
+gen = ParameterGenerator()
+
+gen.add("x", double_t, CHANGE_XYZ, "Translation along X-axis", 0.0)
+gen.add("y", double_t, CHANGE_XYZ, "Translation along Y-axis", 0.0)
+gen.add("z", double_t, CHANGE_XYZ, "Translation along Z-axis", 0.0)
+
+gen.add("yaw_rad", double_t, CHANGE_RPY_RAD, "Rotation around Z-axis", 0.0, -pi, pi)
+gen.add("pitch_rad", double_t, CHANGE_RPY_RAD, "Rotation around Y-axis", 0.0, -pi, pi)
+gen.add("roll_rad", double_t, CHANGE_RPY_RAD, "Rotation around X-axis", 0.0, -pi, pi)
+
+gen.add("yaw_deg", double_t, CHANGE_RPY_DEG, "Rotation around Z-axis", 0.0, -180.0, 180.0)
+gen.add("pitch_deg", double_t, CHANGE_RPY_DEG, "Rotation around Y-axis", 0.0, -180.0, 180.0)
+gen.add("roll_deg", double_t, CHANGE_RPY_DEG, "Rotation around X-axis", 0.0, -180.0, 180.0)
+
+gen.add("use_quaternion", bool_t, CHANGE_QUAT, "Commit quaternion", False)
+gen.add("qw", double_t, CHANGE_NOTHING, "Quaternion W", 1.0, -1.0, 1.0)
+gen.add("qx", double_t, CHANGE_NOTHING, "Quaternion X", 0.0, -1.0, 1.0)
+gen.add("qy", double_t, CHANGE_NOTHING, "Quaternion Y", 0.0, -1.0, 1.0)
+gen.add("qz", double_t, CHANGE_NOTHING, "Quaternion Z", 0.0, -1.0, 1.0)
+
+exit(gen.generate(PACKAGE, "static_transform_publisher", "TransformSender"))

tf/package.xml

@@ -21,6 +21,7 @@ any desired point in time.
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>angles</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>message_filters</build_depend>
   <build_depend>message_generation</build_depend>

tf/src/static_transform_publisher.cpp

@@ -28,21 +28,31 @@
  */
 
 #include <cstdio>
+#include <dynamic_reconfigure/server.h>
 #include "tf/transform_broadcaster.h"
+#include "tf/TransformSenderConfig.h"
 
 class TransformSender
 {
 public:
   ros::NodeHandle node_;
   //constructor
-  TransformSender(double x, double y, double z, double yaw, double pitch, double roll, ros::Time time, const std::string& frame_id, const std::string& child_frame_id)
+  TransformSender(double x, double y, double z, double yaw, double pitch, double roll, ros::Time time, const std::string& frame_id, const std::string& child_frame_id, const std::string& reconf_ns = std::string()) :
+    node_("~"),
+    reconf_server_(ros::NodeHandle(node_, reconf_ns))
   { 
     tf::Quaternion q;
     q.setRPY(roll, pitch,yaw);
     transform_ = tf::StampedTransform(tf::Transform(q, tf::Vector3(x,y,z)), time, frame_id, child_frame_id );
+    reconfInit();
+  };
+  TransformSender(double x, double y, double z, double qx, double qy, double qz, double qw, ros::Time time, const std::string& frame_id, const std::string& child_frame_id, const std::string& reconf_ns = std::string()) :
+    node_("~"),
+    transform_(tf::Transform(tf::Quaternion(qx,qy,qz,qw), tf::Vector3(x,y,z)), time, frame_id, child_frame_id),
+    reconf_server_(ros::NodeHandle(node_, reconf_ns))
+  {
+    reconfInit();
   };
-  TransformSender(double x, double y, double z, double qx, double qy, double qz, double qw, ros::Time time, const std::string& frame_id, const std::string& child_frame_id) :
-    transform_(tf::Transform(tf::Quaternion(qx,qy,qz,qw), tf::Vector3(x,y,z)), time, frame_id, child_frame_id){};
   //Clean up ros connections
   ~TransformSender() { }
 
@@ -57,9 +67,155 @@ class TransformSender
     broadcaster.sendTransform(transform_);
   };
 
+  // Dynamic reconfigure callback
+  void reconfCallback(tf::TransformSenderConfig &config, uint32_t level)
+  {
+    tf::Quaternion q;
+    tf::Transform t;
+    double R, P, Y;
+
+    switch(level) // sent by dynamic reconfigure at first run
+    {
+      case CHANGE_ALL:
+        // Update config with current values
+        config.x = transform_.getOrigin().x();
+        config.y = transform_.getOrigin().y();
+        config.z = transform_.getOrigin().z();
+
+        // Update RPY in radians
+        transform_.getBasis().getRPY(R, P, Y);
+        config.roll_rad = R;
+        config.pitch_rad = P;
+        config.yaw_rad = Y;
+        // Update RPY in degrees
+        toDegrees(R, P, Y);
+        config.roll_deg= R;
+        config.pitch_deg= P;
+        config.yaw_deg = Y;
+
+        config.qw = transform_.getRotation().w();
+        config.qx = transform_.getRotation().x();
+        config.qy = transform_.getRotation().y();
+        config.qz = transform_.getRotation().z();
+        break;
+
+      case CHANGE_XYZ:
+        t = tf::Transform(transform_.getRotation(), tf::Vector3(config.x, config.y, config.z));
+        transform_ = tf::StampedTransform(t, ros::Time::now(), transform_.frame_id_, transform_.child_frame_id_);
+        break;
+
+      case CHANGE_RPY_DEG:
+        R = config.roll_deg;
+        P = config.pitch_deg;
+        Y = config.yaw_deg;
+        toRadians(R, P, Y);
+
+        q.setRPY(R, P, Y);
+        t = tf::Transform(q, transform_.getOrigin());
+        transform_ = tf::StampedTransform(t, ros::Time::now(), transform_.frame_id_, transform_.child_frame_id_);
+        // Update quaternion
+        config.qw = transform_.getRotation().w();
+        config.qx = transform_.getRotation().x();
+        config.qy = transform_.getRotation().y();
+        config.qz = transform_.getRotation().z();
+        // Update RPY in radians
+        config.roll_rad = R;
+        config.pitch_rad = P;
+        config.yaw_rad = Y;
+        break;
+
+      case CHANGE_RPY_RAD:
+        R = config.roll_rad;
+        P = config.pitch_rad;
+        Y = config.yaw_rad;
+
+        q.setRPY(R, P, Y);
+        t = tf::Transform(q, transform_.getOrigin());
+        transform_ = tf::StampedTransform(t, ros::Time::now(), transform_.frame_id_, transform_.child_frame_id_);
+        // Update quaternion
+        config.qw = transform_.getRotation().w();
+        config.qx = transform_.getRotation().x();
+        config.qy = transform_.getRotation().y();
+        config.qz = transform_.getRotation().z();
+        // Update RPY in degrees
+        toDegrees(R, P, Y);
+        config.roll_deg= R;
+        config.pitch_deg= P;
+        config.yaw_deg = Y;
+        break;
+
+      case CHANGE_QUAT:
+        q = tf::Quaternion(config.qx, config.qy, config.qz, config.qw);
+
+        // If new quaternion is not valid use previous one and issue error
+        if(q.length2() == 0.0){
+          q = transform_.getRotation();
+          ROS_ERROR("Reconfigure: quaternion length is 0.0. Using previous value");
+        }
+        // Check normalization
+        else if(q.length2() > 1.0 + DBL_EPSILON || q.length2() < 1.0 - DBL_EPSILON)
+        {
+          q = q.normalize();
+          ROS_WARN("Reconfigure: quaternion is not normalized. Normalizing.");
+        }
+        t = tf::Transform(q, transform_.getOrigin());
+        transform_ = tf::StampedTransform(t, ros::Time::now(), transform_.frame_id_, transform_.child_frame_id_);
+
+        // Update quaternion with corrected value
+        config.qw = q.w();
+        config.qx = q.x();
+        config.qy = q.y();
+        config.qz = q.z();
+
+        // Update RPY in radians
+        transform_.getBasis().getRPY(R, P, Y);
+        config.roll_rad = R;
+        config.pitch_rad = P;
+        config.yaw_rad = Y;
+        // Update RPY in degrees
+        toDegrees(R, P, Y);
+        config.roll_deg= R;
+        config.pitch_deg= P;
+        config.yaw_deg = Y;
+
+        // Reset checkbox
+        config.use_quaternion = false;
+        break;
+    }
+  }
+
 private:
+  enum{
+    CHANGE_NOTHING = 0,
+    CHANGE_XYZ = 1 << 0,
+    CHANGE_RPY_RAD = 1 << 1,
+    CHANGE_RPY_DEG = 1 << 2,
+    CHANGE_QUAT = 1 << 3,
+    CHANGE_ALL = 0xffffffff
+  };
+
   tf::StampedTransform transform_;
+  dynamic_reconfigure::Server<tf::TransformSenderConfig> reconf_server_;
 
+  // Set dynamic reconfigure callback
+  void reconfInit()
+  {
+    reconf_server_.setCallback(boost::bind(&TransformSender::reconfCallback, this, _1, _2));
+  }
+
+  void toDegrees(double &r, double &p, double &y)
+    {
+      r = r * 180.0 / M_PI;
+      p = p * 180.0 / M_PI;
+      y = y * 180.0 / M_PI;
+    }
+
+    void toRadians(double &r, double &p, double &y)
+    {
+      r = r / 180.0 * M_PI;
+      p = p / 180.0 * M_PI;
+      y = y / 180.0 * M_PI;
+    }
 };
 
 int main(int argc, char ** argv)
@@ -85,6 +241,7 @@ int main(int argc, char ** argv)
     {
       tf_sender.send(ros::Time::now() + sleeper);
       ROS_DEBUG("Sending transform from %s with parent %s\n", argv[8], argv[9]);
+      ros::spinOnce();
       sleeper.sleep();
     }
 
@@ -108,6 +265,7 @@ int main(int argc, char ** argv)
     {
       tf_sender.send(ros::Time::now() + sleeper);
       ROS_DEBUG("Sending transform from %s with parent %s\n", argv[7], argv[8]);
+      ros::spinOnce();
       sleeper.sleep();
     }