From a7f177aa34784f76056db09d81cad12f92e6bc40 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ren=C3=A9=20Moll?= Date: Tue, 14 Dec 2021 16:34:31 +0100 Subject: [PATCH] make the reference frame configurable --- param/xsens_mti_node.yaml | 1 + src/messagepublishers/packetcallback.h | 1 + src/messagepublishers/transformpublisher.h | 4 +++- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/param/xsens_mti_node.yaml b/param/xsens_mti_node.yaml index 3281529..21fd7fc 100644 --- a/param/xsens_mti_node.yaml +++ b/param/xsens_mti_node.yaml @@ -23,6 +23,7 @@ # TF transform frame_id (default: imu_link), you may want to change it if you use multiple devices frame_id: "imu_link" + reference_frame_id: "world" # Message publishers pub_imu: true diff --git a/src/messagepublishers/packetcallback.h b/src/messagepublishers/packetcallback.h index 28da27a..b142a1e 100644 --- a/src/messagepublishers/packetcallback.h +++ b/src/messagepublishers/packetcallback.h @@ -66,6 +66,7 @@ #include const char* DEFAULT_FRAME_ID = "imu_link"; +const char* DEFAULT_REFERENCE_FRAME_ID = "world"; class PacketCallback { diff --git a/src/messagepublishers/transformpublisher.h b/src/messagepublishers/transformpublisher.h index ef51da1..8606c86 100644 --- a/src/messagepublishers/transformpublisher.h +++ b/src/messagepublishers/transformpublisher.h @@ -70,10 +70,12 @@ struct TransformPublisher : public PacketCallback { tf2_ros::TransformBroadcaster tf_broadcaster; std::string frame_id = DEFAULT_FRAME_ID; + std::string reference_frame_id = DEFAULT_REFERENCE_FRAME_ID; TransformPublisher(rclcpp::Node &node) : tf_broadcaster(node) { node.get_parameter("frame_id", frame_id); + node.get_parameter("reference_frame_id", reference_frame_id); } void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) @@ -85,7 +87,7 @@ struct TransformPublisher : public PacketCallback XsQuaternion q = packet.orientationQuaternion(); tf.header.stamp = timestamp; - tf.header.frame_id = "world"; + tf.header.frame_id = reference_frame_id; tf.child_frame_id = frame_id; tf.transform.translation.x = 0.0; tf.transform.translation.y = 0.0;