diff --git a/tf/CMakeLists.txt b/tf/CMakeLists.txt
index 771257af..00bdc6b0 100644
--- a/tf/CMakeLists.txt
+++ b/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)
diff --git a/tf/cfg/TransformSender.cfg b/tf/cfg/TransformSender.cfg
new file mode 100755
index 00000000..8ec9d667
--- /dev/null
+++ b/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"))
\ No newline at end of file
diff --git a/tf/package.xml b/tf/package.xml
index 60d41f5a..af689384 100644
--- a/tf/package.xml
+++ b/tf/package.xml
@@ -21,6 +21,7 @@ any desired point in time.
catkin
angles
+ dynamic_reconfigure
geometry_msgs
message_filters
message_generation
diff --git a/tf/src/static_transform_publisher.cpp b/tf/src/static_transform_publisher.cpp
index 112be4cf..ba7f3963 100644
--- a/tf/src/static_transform_publisher.cpp
+++ b/tf/src/static_transform_publisher.cpp
@@ -28,21 +28,31 @@
*/
#include
+#include
#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 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();
}