From d76479e17c0ed6b4c6e3691760b5374f33c98698 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 17 Dec 2024 23:18:13 +0000 Subject: [PATCH 1/4] wip api changes --- .../parameters/transform_sensor_params.hpp | 4 ++-- .../include/fuse_models/transform_sensor.hpp | 2 +- fuse_models/src/transform_sensor.cpp | 19 ++++++++++++++++--- 3 files changed, 19 insertions(+), 6 deletions(-) diff --git a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp index e6506abe..07ef8306 100644 --- a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp @@ -79,7 +79,7 @@ struct TransformSensorParams : public ParameterBase throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), throttle_use_wall_time); - fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + transforms = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "transforms"), transforms); target_frame = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame); @@ -96,7 +96,7 @@ struct TransformSensorParams : public ParameterBase bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not std::vector pose_covariance; //!< The diagonal elements of the tag pose covariance int queue_size{ 10 }; - std::string topic; + std::vector transforms; std::string target_frame; std::vector position_indices; std::vector orientation_indices; diff --git a/fuse_models/include/fuse_models/transform_sensor.hpp b/fuse_models/include/fuse_models/transform_sensor.hpp index fa8523dc..2284cd93 100644 --- a/fuse_models/include/fuse_models/transform_sensor.hpp +++ b/fuse_models/include/fuse_models/transform_sensor.hpp @@ -50,7 +50,6 @@ #include #include #include -#include "tf2_msgs/msg/tf_message.hpp" namespace fuse_models { @@ -143,6 +142,7 @@ class TransformSensor : public fuse_core::AsyncSensorModel std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; + std::set transforms_of_interest_; rclcpp::Subscription::SharedPtr sub_; diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index 6e41bcdc..14eb47a4 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -32,6 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include #include @@ -85,8 +86,15 @@ void TransformSensor::onInit() if (params_.position_indices.empty() && params_.orientation_indices.empty()) { - throw std::runtime_error("No dimensions specified, so this sensor would not do anything (data from topic " + - params_.topic + " would be ignored)."); + throw std::runtime_error("No dimensions specified, so this sensor would not do anything (tf data would be ignored)."); + } + + if (params_.transforms.empty()) { + throw std::runtime_error("No transforms specified, this sensor would not do anything (all tf data would be ignored)."); + } + + for (auto const& name : params_.transforms) { + transforms_of_interest_.insert(name); } tf_buffer_ = std::make_unique(clock_); @@ -98,7 +106,7 @@ void TransformSensor::onStart() rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; - sub_ = rclcpp::create_subscription(interfaces_, params_.topic, params_.queue_size, + sub_ = rclcpp::create_subscription(interfaces_, "/tf", params_.queue_size, std::bind(&AprilTagThrottledCallback::callback, &throttled_callback_, std::placeholders::_1), sub_options); @@ -113,6 +121,11 @@ void TransformSensor::process(MessageType const& msg) { for (auto const& transform : msg.transforms) { + std::string const& tf_name = transform.header.frame_id; + if (transforms_of_interest_.find(tf_name) == transforms_of_interest_.end()) { + // we don't care about this transform, skip it + continue; + } // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(transform.header.stamp); From 382ac3dda3ff9647def343bc395213274352d0e4 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 20 Dec 2024 00:16:10 +0000 Subject: [PATCH 2/4] fix transform sensor ignore parameter --- fuse_models/src/transform_sensor.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index 14eb47a4..bb33c68f 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -121,11 +121,13 @@ void TransformSensor::process(MessageType const& msg) { for (auto const& transform : msg.transforms) { - std::string const& tf_name = transform.header.frame_id; + std::string const& tf_name = transform.child_frame_id; if (transforms_of_interest_.find(tf_name) == transforms_of_interest_.end()) { // we don't care about this transform, skip it + RCLCPP_DEBUG(logger_, "Ignoring transform from %s to %s", transform.header.frame_id.c_str(), tf_name.c_str()); continue; } + RCLCPP_DEBUG(logger_, "Got transform of interest from %s to %s", transform.header.frame_id.c_str(), tf_name.c_str()); // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(transform.header.stamp); From 25b27d4a4814875fe30d23768e1949ecf9f82d54 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 20 Dec 2024 00:30:46 +0000 Subject: [PATCH 3/4] pre-commit --- fuse_models/src/transform_sensor.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index bb33c68f..41677719 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -86,14 +86,18 @@ void TransformSensor::onInit() if (params_.position_indices.empty() && params_.orientation_indices.empty()) { - throw std::runtime_error("No dimensions specified, so this sensor would not do anything (tf data would be ignored)."); + throw std::runtime_error( + "No dimensions specified, so this sensor would not do anything (tf data would be ignored)."); } - if (params_.transforms.empty()) { - throw std::runtime_error("No transforms specified, this sensor would not do anything (all tf data would be ignored)."); + if (params_.transforms.empty()) + { + throw std::runtime_error( + "No transforms specified, this sensor would not do anything (all tf data would be ignored)."); } - for (auto const& name : params_.transforms) { + for (auto const& name : params_.transforms) + { transforms_of_interest_.insert(name); } @@ -122,7 +126,8 @@ void TransformSensor::process(MessageType const& msg) for (auto const& transform : msg.transforms) { std::string const& tf_name = transform.child_frame_id; - if (transforms_of_interest_.find(tf_name) == transforms_of_interest_.end()) { + if (transforms_of_interest_.find(tf_name) == transforms_of_interest_.end()) + { // we don't care about this transform, skip it RCLCPP_DEBUG(logger_, "Ignoring transform from %s to %s", transform.header.frame_id.c_str(), tf_name.c_str()); continue; From 793b0074dba2988e5b02739cdebbaeb85dcbe9f8 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 20 Dec 2024 19:24:30 +0000 Subject: [PATCH 4/4] try fixing clang tidy --- .clang-tidy | 1 - .github/workflows/ci.yaml | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index b9ba1944..9f4331f8 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -64,4 +64,3 @@ CheckOptions: value: camelBack - key: readability-identifier-naming.GlobalVariableCase value: camelBack -... diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 3102f889..3cf1717a 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -93,7 +93,7 @@ jobs: **.cpp **.hpp - if: ${{ steps.changed-cpp-files.outputs.all_changed_files != '' }} - run: run-clang-tidy -j $(nproc --all) -p build/ -export-fixes clang-tidy-fixes.yaml -config-file src/fuse/.clang-tidy ${{ steps.changed-cpp-files.outputs.all_changed_files }} + run: run-clang-tidy -j $(nproc --all) -p build/ -export-fixes clang-tidy-fixes.yaml -config "$(cat src/fuse/.clang-tidy)" ${{ steps.changed-cpp-files.outputs.all_changed_files }} working-directory: /colcon_ws - uses: asarium/clang-tidy-action@v1.0 with: