From 9aea9df3d7c91374e6798383272f5902f2fda8ef Mon Sep 17 00:00:00 2001 From: 0xff <0xff.root@gmail.com> Date: Wed, 31 Jul 2013 21:18:20 +0900 Subject: [PATCH 1/6] Add dynamic reconfigure to static_tf_publisher --- tf/CMakeLists.txt | 5 ++- tf/cfg/TransformSender.cfg | 17 +++++++++++ tf/package.xml | 1 + tf/src/static_transform_publisher.cpp | 44 ++++++++++++++++++++++++++- 4 files changed, 65 insertions(+), 2 deletions(-) create mode 100755 tf/cfg/TransformSender.cfg 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..9c6eb737 --- /dev/null +++ b/tf/cfg/TransformSender.cfg @@ -0,0 +1,17 @@ +#!/usr/bin/env python +PACKAGE = "tf" + +from dynamic_reconfigure.parameter_generator_catkin import * +from math import pi + +gen = ParameterGenerator() + +gen.add("x", double_t, 0, "Translation along X-axis", 0.0) +gen.add("y", double_t, 0, "Translation along Y-axis", 0.0) +gen.add("z", double_t, 0, "Translation along Z-axis", 0.0) + +gen.add("yaw", double_t, 0, "Rotation around Z-axis", 0.0, -pi, pi) +gen.add("pitch", double_t, 0, "Rotation around Y-axis", 0.0, -pi, pi) +gen.add("roll", double_t, 0, "Rotation around X-axis", 0.0, -pi, pi) + +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..65da25e0 100644 --- a/tf/src/static_transform_publisher.cpp +++ b/tf/src/static_transform_publisher.cpp @@ -28,7 +28,9 @@ */ #include +#include #include "tf/transform_broadcaster.h" +#include "tf/TransformSenderConfig.h" class TransformSender { @@ -40,9 +42,13 @@ class TransformSender 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 ); + reconf_init(); }; 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){}; + transform_(tf::Transform(tf::Quaternion(qx,qy,qz,qw), tf::Vector3(x,y,z)), time, frame_id, child_frame_id) + { + reconf_init(); + }; //Clean up ros connections ~TransformSender() { } @@ -57,9 +63,43 @@ class TransformSender broadcaster.sendTransform(transform_); }; + // + void reconf_callback(tf::TransformSenderConfig &config, uint32_t level) + { + if(level == 0xffffffff) // sent by dynamic reconfigure at first run + { + double R, P, Y; + + // Update config with current values + config.x = transform_.getOrigin().x(); + config.y = transform_.getOrigin().y(); + config.z = transform_.getOrigin().z(); + + transform_.getBasis().getRPY(R, P, Y); + config.roll = R; + config.pitch = P; + config.yaw = Y; + } + else + { + tf::Quaternion q; + tf::Transform t; + + q.setRPY(config.roll, config.pitch, config.yaw); + t = tf::Transform(q, tf::Vector3(config.x, config.y, config.z)); + transform_ = tf::StampedTransform(t, ros::Time::now(), transform_.frame_id_, transform_.child_frame_id_); + } + } + private: tf::StampedTransform transform_; + dynamic_reconfigure::Server reconf_server_; + // Set dynamic reconfigure callback + void reconf_init() + { + reconf_server_.setCallback(boost::bind(&TransformSender::reconf_callback, this, _1, _2)); + } }; int main(int argc, char ** argv) @@ -85,6 +125,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 +149,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(); } From e2dc80fce64a014a47a103c163b0a7adc373739e Mon Sep 17 00:00:00 2001 From: 0xff <0xff.root@gmail.com> Date: Thu, 1 Aug 2013 23:04:17 +0900 Subject: [PATCH 2/6] Make use of reconfigure levels --- tf/cfg/TransformSender.cfg | 17 ++++++--- tf/src/static_transform_publisher.cpp | 53 +++++++++++++++++---------- 2 files changed, 44 insertions(+), 26 deletions(-) diff --git a/tf/cfg/TransformSender.cfg b/tf/cfg/TransformSender.cfg index 9c6eb737..c0e8d6af 100755 --- a/tf/cfg/TransformSender.cfg +++ b/tf/cfg/TransformSender.cfg @@ -4,14 +4,19 @@ PACKAGE = "tf" from dynamic_reconfigure.parameter_generator_catkin import * from math import pi +CHANGE_NOTHING = 0 +CHANGE_XYZ = 1 << 0 +CHANGE_RPY = 1 << 1 +CHANGE_ALL = 0xffffffff + gen = ParameterGenerator() -gen.add("x", double_t, 0, "Translation along X-axis", 0.0) -gen.add("y", double_t, 0, "Translation along Y-axis", 0.0) -gen.add("z", double_t, 0, "Translation along Z-axis", 0.0) +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", double_t, 0, "Rotation around Z-axis", 0.0, -pi, pi) -gen.add("pitch", double_t, 0, "Rotation around Y-axis", 0.0, -pi, pi) -gen.add("roll", double_t, 0, "Rotation around X-axis", 0.0, -pi, pi) +gen.add("yaw", double_t, CHANGE_RPY, "Rotation around Z-axis", 0.0, -pi, pi) +gen.add("pitch", double_t, CHANGE_RPY, "Rotation around Y-axis", 0.0, -pi, pi) +gen.add("roll", double_t, CHANGE_RPY, "Rotation around X-axis", 0.0, -pi, pi) exit(gen.generate(PACKAGE, "static_transform_publisher", "TransformSender")) \ No newline at end of file diff --git a/tf/src/static_transform_publisher.cpp b/tf/src/static_transform_publisher.cpp index 65da25e0..f3495ba1 100644 --- a/tf/src/static_transform_publisher.cpp +++ b/tf/src/static_transform_publisher.cpp @@ -66,32 +66,45 @@ class TransformSender // void reconf_callback(tf::TransformSenderConfig &config, uint32_t level) { - if(level == 0xffffffff) // sent by dynamic reconfigure at first run - { - double R, P, Y; - - // Update config with current values - config.x = transform_.getOrigin().x(); - config.y = transform_.getOrigin().y(); - config.z = transform_.getOrigin().z(); + tf::Quaternion q; + tf::Transform t; + double R, P, Y; - transform_.getBasis().getRPY(R, P, Y); - config.roll = R; - config.pitch = P; - config.yaw = Y; - } - else + switch(level) // sent by dynamic reconfigure at first run { - tf::Quaternion q; - tf::Transform t; - - q.setRPY(config.roll, config.pitch, config.yaw); - t = tf::Transform(q, tf::Vector3(config.x, config.y, config.z)); - transform_ = tf::StampedTransform(t, ros::Time::now(), transform_.frame_id_, transform_.child_frame_id_); + case CHANGE_ALL: + // Update config with current values + config.x = transform_.getOrigin().x(); + config.y = transform_.getOrigin().y(); + config.z = transform_.getOrigin().z(); + + transform_.getBasis().getRPY(R, P, Y); + config.roll = R; + config.pitch = P; + config.yaw = Y; + 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: + q.setRPY(config.roll, config.pitch, config.yaw); + t = tf::Transform(q, transform_.getOrigin()); + transform_ = tf::StampedTransform(t, ros::Time::now(), transform_.frame_id_, transform_.child_frame_id_); + break; } } private: + enum{ + CHANGE_NOTHING = 0, + CHANGE_XYZ = 1 << 0, + CHANGE_RPY = 1 << 1, + CHANGE_ALL = 0xffffffff + }; + tf::StampedTransform transform_; dynamic_reconfigure::Server reconf_server_; From 39a18fb19abd27e5abbee13a502657424d3f49f3 Mon Sep 17 00:00:00 2001 From: 0xff <0xff.root@gmail.com> Date: Fri, 2 Aug 2013 00:25:08 +0900 Subject: [PATCH 3/6] Add quaternion controls to reconfigure --- tf/cfg/TransformSender.cfg | 7 ++++ tf/src/static_transform_publisher.cpp | 47 ++++++++++++++++++++++++++- 2 files changed, 53 insertions(+), 1 deletion(-) diff --git a/tf/cfg/TransformSender.cfg b/tf/cfg/TransformSender.cfg index c0e8d6af..8d83b2ff 100755 --- a/tf/cfg/TransformSender.cfg +++ b/tf/cfg/TransformSender.cfg @@ -7,6 +7,7 @@ from math import pi CHANGE_NOTHING = 0 CHANGE_XYZ = 1 << 0 CHANGE_RPY = 1 << 1 +CHANGE_QUAT = 1 << 2 CHANGE_ALL = 0xffffffff gen = ParameterGenerator() @@ -19,4 +20,10 @@ gen.add("yaw", double_t, CHANGE_RPY, "Rotation around Z-axis", 0.0, -pi, pi) gen.add("pitch", double_t, CHANGE_RPY, "Rotation around Y-axis", 0.0, -pi, pi) gen.add("roll", double_t, CHANGE_RPY, "Rotation around X-axis", 0.0, -pi, pi) +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/src/static_transform_publisher.cpp b/tf/src/static_transform_publisher.cpp index f3495ba1..fcf7532b 100644 --- a/tf/src/static_transform_publisher.cpp +++ b/tf/src/static_transform_publisher.cpp @@ -63,7 +63,7 @@ class TransformSender broadcaster.sendTransform(transform_); }; - // + // Dynamic reconfigure callback void reconf_callback(tf::TransformSenderConfig &config, uint32_t level) { tf::Quaternion q; @@ -82,6 +82,11 @@ class TransformSender config.roll = R; config.pitch = P; config.yaw = 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: @@ -93,6 +98,45 @@ class TransformSender q.setRPY(config.roll, config.pitch, config.yaw); 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(); + 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 + transform_.getBasis().getRPY(R, P, Y); + config.roll = R; + config.pitch = P; + config.yaw = Y; + + // Reset checkbox + config.use_quaternion = false; break; } } @@ -102,6 +146,7 @@ class TransformSender CHANGE_NOTHING = 0, CHANGE_XYZ = 1 << 0, CHANGE_RPY = 1 << 1, + CHANGE_QUAT = 1 << 2, CHANGE_ALL = 0xffffffff }; From 3e28b357f6608de001d835915b44da4e2195a083 Mon Sep 17 00:00:00 2001 From: 0xff <0xff.root@gmail.com> Date: Fri, 2 Aug 2013 01:43:59 +0900 Subject: [PATCH 4/6] Add RPY in degrees --- tf/cfg/TransformSender.cfg | 15 ++++-- tf/src/static_transform_publisher.cpp | 78 ++++++++++++++++++++++----- 2 files changed, 76 insertions(+), 17 deletions(-) diff --git a/tf/cfg/TransformSender.cfg b/tf/cfg/TransformSender.cfg index 8d83b2ff..8ec9d667 100755 --- a/tf/cfg/TransformSender.cfg +++ b/tf/cfg/TransformSender.cfg @@ -6,8 +6,9 @@ from math import pi CHANGE_NOTHING = 0 CHANGE_XYZ = 1 << 0 -CHANGE_RPY = 1 << 1 -CHANGE_QUAT = 1 << 2 +CHANGE_RPY_RAD = 1 << 1 +CHANGE_RPY_DEG = 1 << 2 +CHANGE_QUAT = 1 << 3 CHANGE_ALL = 0xffffffff gen = ParameterGenerator() @@ -16,9 +17,13 @@ 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", double_t, CHANGE_RPY, "Rotation around Z-axis", 0.0, -pi, pi) -gen.add("pitch", double_t, CHANGE_RPY, "Rotation around Y-axis", 0.0, -pi, pi) -gen.add("roll", double_t, CHANGE_RPY, "Rotation around X-axis", 0.0, -pi, pi) +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) diff --git a/tf/src/static_transform_publisher.cpp b/tf/src/static_transform_publisher.cpp index fcf7532b..1cd4a8e8 100644 --- a/tf/src/static_transform_publisher.cpp +++ b/tf/src/static_transform_publisher.cpp @@ -78,10 +78,16 @@ class TransformSender config.y = transform_.getOrigin().y(); config.z = transform_.getOrigin().z(); + // Update RPY in radians transform_.getBasis().getRPY(R, P, Y); - config.roll = R; - config.pitch = P; - config.yaw = 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(); @@ -94,8 +100,32 @@ class TransformSender transform_ = tf::StampedTransform(t, ros::Time::now(), transform_.frame_id_, transform_.child_frame_id_); break; - case CHANGE_RPY: - q.setRPY(config.roll, config.pitch, config.yaw); + 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 @@ -103,6 +133,11 @@ class TransformSender 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: @@ -119,7 +154,6 @@ class TransformSender 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_); @@ -129,11 +163,16 @@ class TransformSender config.qy = q.y(); config.qz = q.z(); - // Update RPY + // Update RPY in radians transform_.getBasis().getRPY(R, P, Y); - config.roll = R; - config.pitch = P; - config.yaw = 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; @@ -145,8 +184,9 @@ class TransformSender enum{ CHANGE_NOTHING = 0, CHANGE_XYZ = 1 << 0, - CHANGE_RPY = 1 << 1, - CHANGE_QUAT = 1 << 2, + CHANGE_RPY_RAD = 1 << 1, + CHANGE_RPY_DEG = 1 << 2, + CHANGE_QUAT = 1 << 3, CHANGE_ALL = 0xffffffff }; @@ -158,6 +198,20 @@ class TransformSender { reconf_server_.setCallback(boost::bind(&TransformSender::reconf_callback, 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) From d1e50d8f90d82fa2966601e1c86c60f412817930 Mon Sep 17 00:00:00 2001 From: 0xff <0xff.root@gmail.com> Date: Fri, 2 Aug 2013 11:51:00 +0900 Subject: [PATCH 5/6] Rename functions according to ROS guidelines --- tf/src/static_transform_publisher.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tf/src/static_transform_publisher.cpp b/tf/src/static_transform_publisher.cpp index 1cd4a8e8..046ba6ea 100644 --- a/tf/src/static_transform_publisher.cpp +++ b/tf/src/static_transform_publisher.cpp @@ -42,12 +42,12 @@ class TransformSender 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 ); - reconf_init(); + 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) { - reconf_init(); + reconfInit(); }; //Clean up ros connections ~TransformSender() { } @@ -64,7 +64,7 @@ class TransformSender }; // Dynamic reconfigure callback - void reconf_callback(tf::TransformSenderConfig &config, uint32_t level) + void reconfCallback(tf::TransformSenderConfig &config, uint32_t level) { tf::Quaternion q; tf::Transform t; @@ -194,9 +194,9 @@ class TransformSender dynamic_reconfigure::Server reconf_server_; // Set dynamic reconfigure callback - void reconf_init() + void reconfInit() { - reconf_server_.setCallback(boost::bind(&TransformSender::reconf_callback, this, _1, _2)); + reconf_server_.setCallback(boost::bind(&TransformSender::reconfCallback, this, _1, _2)); } void toDegrees(double &r, double &p, double &y) From f36c87583bf86836ba34f2e333a2a07172ce5f2c Mon Sep 17 00:00:00 2001 From: Boris Gromov <0xff.root@gmail.com> Date: Sun, 9 Feb 2014 14:26:30 +0400 Subject: [PATCH 6/6] Add reconf_ns param to TransformSender constructor --- tf/src/static_transform_publisher.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/tf/src/static_transform_publisher.cpp b/tf/src/static_transform_publisher.cpp index 046ba6ea..ba7f3963 100644 --- a/tf/src/static_transform_publisher.cpp +++ b/tf/src/static_transform_publisher.cpp @@ -37,15 +37,19 @@ 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) : - transform_(tf::Transform(tf::Quaternion(qx,qy,qz,qw), tf::Vector3(x,y,z)), time, frame_id, child_frame_id) + 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(); };