From 5c462655d75fa00435b37c447ecaddbb7c1f101f Mon Sep 17 00:00:00 2001 From: methylDragon Date: Sat, 14 Jan 2023 00:23:22 -0800 Subject: [PATCH] fuse -> ROS 2 fuse_models: Port fuse_models (#304) * Port messages Signed-off-by: methylDragon * Port fuse_models Signed-off-by: methylDragon * Fix alloc error and some bugs Signed-off-by: methylDragon * Wait on result Signed-off-by: methylDragon Signed-off-by: methylDragon --- doc/Variables.md | 2 +- .../include/fuse_core/async_motion_model.hpp | 12 +- .../include/fuse_core/async_sensor_model.hpp | 10 +- fuse_core/include/fuse_core/console.hpp | 7 +- fuse_core/include/fuse_core/motion_model.hpp | 4 +- fuse_core/include/fuse_core/sensor_model.hpp | 1 + fuse_core/src/async_motion_model.cpp | 43 ++- fuse_core/src/async_publisher.cpp | 6 +- fuse_core/src/async_sensor_model.cpp | 39 +- fuse_core/src/ceres_options.cpp | 8 +- fuse_core/src/fuse_echo.cpp | 8 +- fuse_core/test/test_async_motion_model.cpp | 16 +- fuse_core/test/test_async_sensor_model.cpp | 16 +- fuse_core/test/test_throttled_callback.cpp | 2 +- fuse_graphs/fuse_plugins.xml | 2 +- fuse_loss/fuse_plugins.xml | 2 +- fuse_models/CMakeLists.txt | 341 +++++------------- fuse_models/COLCON_IGNORE | 0 fuse_models/benchmark/CMakeLists.txt | 21 ++ fuse_models/fuse_plugins.xml | 4 +- .../include/fuse_models/acceleration_2d.h | 42 ++- .../include/fuse_models/common/sensor_proc.h | 95 +++-- .../include/fuse_models/graph_ignition.h | 48 ++- fuse_models/include/fuse_models/imu_2d.h | 48 ++- fuse_models/include/fuse_models/odometry_2d.h | 49 ++- .../fuse_models/odometry_2d_publisher.h | 61 ++-- .../parameters/acceleration_2d_params.h | 43 +-- .../parameters/graph_ignition_params.h | 29 +- .../fuse_models/parameters/imu_2d_params.h | 68 ++-- .../parameters/odometry_2d_params.h | 64 ++-- .../parameters/odometry_2d_publisher_params.h | 60 +-- .../fuse_models/parameters/parameter_base.h | 29 +- .../fuse_models/parameters/pose_2d_params.h | 50 +-- .../parameters/transaction_params.h | 18 +- .../fuse_models/parameters/twist_2d_params.h | 46 +-- .../parameters/unicycle_2d_ignition_params.h | 45 ++- fuse_models/include/fuse_models/pose_2d.h | 48 ++- fuse_models/include/fuse_models/transaction.h | 28 +- fuse_models/include/fuse_models/twist_2d.h | 41 ++- fuse_models/include/fuse_models/unicycle_2d.h | 23 +- .../fuse_models/unicycle_2d_ignition.h | 64 ++-- .../include/fuse_models/unicycle_2d_predict.h | 2 +- fuse_models/package.xml | 78 ++-- fuse_models/src/acceleration_2d.cpp | 67 +++- fuse_models/src/graph_ignition.cpp | 98 +++-- fuse_models/src/imu_2d.cpp | 175 +++++---- fuse_models/src/odometry_2d.cpp | 106 ++++-- fuse_models/src/odometry_2d_publisher.cpp | 133 ++++--- fuse_models/src/pose_2d.cpp | 79 ++-- fuse_models/src/transaction.cpp | 33 +- fuse_models/src/twist_2d.cpp | 67 +++- fuse_models/src/unicycle_2d.cpp | 55 ++- fuse_models/src/unicycle_2d_ignition.cpp | 137 ++++--- ...unicycle_2d_state_kinematic_constraint.cpp | 2 +- fuse_models/test/CMakeLists.txt | 16 + fuse_models/test/example_variable_stamped.h | 2 +- fuse_models/test/graph_ignition.test | 4 - fuse_models/test/test_graph_ignition.cpp | 122 ++++--- fuse_models/test/test_sensor_proc.cpp | 6 - fuse_models/test/test_unicycle_2d.cpp | 6 - .../test/test_unicycle_2d_ignition.cpp | 181 ++++++---- fuse_models/test/test_unicycle_2d_predict.cpp | 8 +- .../test_unicycle_2d_state_cost_function.cpp | 15 +- fuse_models/test/unicycle_2d_ignition.test | 4 - fuse_msgs/CMakeLists.txt | 11 +- fuse_msgs/package.xml | 1 + {fuse_models => fuse_msgs}/srv/SetGraph.srv | 0 {fuse_models => fuse_msgs}/srv/SetPose.srv | 0 .../srv/SetPoseDeprecated.srv | 0 fuse_tutorials/fuse_plugins.xml | 2 +- .../fuse_tutorials/range_sensor_model.h | 4 +- fuse_tutorials/src/beacon_publisher.cpp | 2 +- fuse_tutorials/src/range_sensor_model.cpp | 8 +- fuse_tutorials/src/range_sensor_simulator.cpp | 16 +- 74 files changed, 1767 insertions(+), 1216 deletions(-) delete mode 100644 fuse_models/COLCON_IGNORE create mode 100644 fuse_models/benchmark/CMakeLists.txt create mode 100644 fuse_models/test/CMakeLists.txt delete mode 100644 fuse_models/test/graph_ignition.test delete mode 100644 fuse_models/test/unicycle_2d_ignition.test rename {fuse_models => fuse_msgs}/srv/SetGraph.srv (100%) rename {fuse_models => fuse_msgs}/srv/SetPose.srv (100%) rename {fuse_models => fuse_msgs}/srv/SetPoseDeprecated.srv (100%) diff --git a/doc/Variables.md b/doc/Variables.md index 2d55cd914..4e41fcbf1 100644 --- a/doc/Variables.md +++ b/doc/Variables.md @@ -297,7 +297,7 @@ void print(std::ostream& stream = std::cout) const override { stream << type() << ":\n" << " uuid: " << uuid_ << "\n" - << " stamp: " << stamp_ << "\n" + << " stamp: " << stamp_.nanoseconds() << "\n" << " device_id: " << device_id_ << "\n" << " size: " << data_.size() << "\n" << " data:\n" diff --git a/fuse_core/include/fuse_core/async_motion_model.hpp b/fuse_core/include/fuse_core/async_motion_model.hpp index 6f7685c6e..35e9bcf8f 100644 --- a/fuse_core/include/fuse_core/async_motion_model.hpp +++ b/fuse_core/include/fuse_core/async_motion_model.hpp @@ -142,7 +142,9 @@ class AsyncMotionModel : public MotionModel * @param[in] name A unique name to give this plugin instance * @throws runtime_error if already initialized */ - void initialize(const std::string & name) override; + void initialize( + node_interfaces::NodeInterfaces interfaces, + const std::string & name) override; /** * @brief Get the unique name of this motion model @@ -191,13 +193,15 @@ class AsyncMotionModel : public MotionModel std::shared_ptr callback_queue_; std::string name_; //!< The unique name for this motion model instance - rclcpp::Node::SharedPtr node_; //!< The node for this motion model + + //! The node interfaces + node_interfaces::NodeInterfaces interfaces_; rclcpp::CallbackGroup::SharedPtr cb_group_; //!< Internal re-entrant callback group //! A single/multi-threaded executor assigned to the local callback queue - rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + rclcpp::Executor::SharedPtr executor_; - size_t executor_thread_count_; + size_t executor_thread_count_{1}; std::thread spinner_; //!< Internal thread for spinning the executor std::atomic initialized_ = false; //!< True if instance has been fully initialized diff --git a/fuse_core/include/fuse_core/async_sensor_model.hpp b/fuse_core/include/fuse_core/async_sensor_model.hpp index a0808c767..0fb353a93 100644 --- a/fuse_core/include/fuse_core/async_sensor_model.hpp +++ b/fuse_core/include/fuse_core/async_sensor_model.hpp @@ -40,6 +40,7 @@ #include #include +#include #include #include @@ -127,6 +128,7 @@ class AsyncSensorModel : public SensorModel * @throws runtime_error if already initialized */ void initialize( + node_interfaces::NodeInterfaces interfaces, const std::string & name, TransactionCallback transaction_callback) override; @@ -193,15 +195,17 @@ class AsyncSensorModel : public SensorModel std::shared_ptr callback_queue_; std::string name_; //!< The unique name for this sensor model instance - rclcpp::Node::SharedPtr node_; //!< The node for this sensor model + + //! The node interfaces + node_interfaces::NodeInterfaces interfaces_; rclcpp::CallbackGroup::SharedPtr cb_group_; //!< Internal re-entrant callback group //! A single/multi-threaded executor assigned to the local callback queue - rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + rclcpp::Executor::SharedPtr executor_; //! The function to be executed every time a Transaction is "published" TransactionCallback transaction_callback_; - size_t executor_thread_count_; + size_t executor_thread_count_{1}; std::thread spinner_; //!< Internal thread for spinning the executor std::atomic initialized_ = false; //!< True if instance has been fully initialized diff --git a/fuse_core/include/fuse_core/console.hpp b/fuse_core/include/fuse_core/console.hpp index fb607da91..c30e6737b 100644 --- a/fuse_core/include/fuse_core/console.hpp +++ b/fuse_core/include/fuse_core/console.hpp @@ -82,7 +82,8 @@ class DelayedThrottleFilter */ bool isEnabled() { - const auto now = time_point_cast(std::chrono::system_clock::now()); + const auto now = std::chrono::time_point_cast( + std::chrono::system_clock::now()); if (last_hit_.time_since_epoch().count() < 0.0) { last_hit_ = now; @@ -102,8 +103,8 @@ class DelayedThrottleFilter */ void reset() { - last_hit_ = - time_point_cast(std::chrono::system_clock::from_time_t(-1)); + last_hit_ = std::chrono::time_point_cast( + std::chrono::system_clock::from_time_t(-1)); } private: diff --git a/fuse_core/include/fuse_core/motion_model.hpp b/fuse_core/include/fuse_core/motion_model.hpp index a14eb206e..b833b0b33 100644 --- a/fuse_core/include/fuse_core/motion_model.hpp +++ b/fuse_core/include/fuse_core/motion_model.hpp @@ -100,7 +100,9 @@ class MotionModel * * @param[in] name A unique name to give this plugin instance */ - virtual void initialize(const std::string & name) = 0; + virtual void initialize( + node_interfaces::NodeInterfaces interfaces, + const std::string & name) = 0; /** * @brief Get the unique name of this motion model diff --git a/fuse_core/include/fuse_core/sensor_model.hpp b/fuse_core/include/fuse_core/sensor_model.hpp index eed0c3c82..11f5a9639 100644 --- a/fuse_core/include/fuse_core/sensor_model.hpp +++ b/fuse_core/include/fuse_core/sensor_model.hpp @@ -106,6 +106,7 @@ class SensorModel * @param[in] transaction_callback The function to call every time a transaction is published */ virtual void initialize( + node_interfaces::NodeInterfaces interfaces, const std::string & name, TransactionCallback transaction_callback) = 0; diff --git a/fuse_core/src/async_motion_model.cpp b/fuse_core/src/async_motion_model.cpp index daf4606f2..c2be0ad85 100644 --- a/fuse_core/src/async_motion_model.cpp +++ b/fuse_core/src/async_motion_model.cpp @@ -75,7 +75,9 @@ bool AsyncMotionModel::apply(Transaction & transaction) return result.get(); } -void AsyncMotionModel::initialize(const std::string & name) +void AsyncMotionModel::initialize( + node_interfaces::NodeInterfaces interfaces, + const std::string & name) { if (initialized_) { throw std::runtime_error("Calling initialize on an already initialized AsyncMotionModel!"); @@ -83,36 +85,33 @@ void AsyncMotionModel::initialize(const std::string & name) // Initialize internal state name_ = name; - std::string node_namespace = ""; - - // TODO(CH3): Pass in the context or a node to get the context from - rclcpp::Context::SharedPtr ros_context = rclcpp::contexts::get_global_default_context(); - auto node_options = rclcpp::NodeOptions(); - node_options.context(ros_context); // set a context to generate the node in - - // TODO(CH3): Potentially pass in the optimizer node instead of spinning a new one - node_ = rclcpp::Node::make_shared(name_, node_namespace, node_options); + interfaces_ = interfaces; + auto context = interfaces_.get_node_base_interface()->get_context(); auto executor_options = rclcpp::ExecutorOptions(); - executor_options.context = ros_context; - executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( - executor_options, - executor_thread_count_ - ); + executor_options.context = context; + + if (executor_thread_count_ == 1) { + executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(executor_options); + } else { + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( + executor_options, executor_thread_count_); + } - callback_queue_ = std::make_shared(ros_context); + callback_queue_ = std::make_shared(context); // This callback group MUST be re-entrant in order to support parallelization - cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - node_->get_node_waitables_interface()->add_waitable( - callback_queue_, cb_group_); + cb_group_ = interfaces_.get_node_base_interface()->create_callback_group( + rclcpp::CallbackGroupType::Reentrant, false); + interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, cb_group_); // Call the derived onInit() function to perform implementation-specific initialization onInit(); // Make sure the executor will service the given node - // TODO(sloretz) add just the callback group here when using Optimizer's Node - executor_->add_node(node_); + // We can add this without any guards because the callback group was set to not get automatically + // added to executors + executor_->add_callback_group(cb_group_, interfaces_.get_node_base_interface()); // Start the executor spinner_ = std::thread( @@ -155,7 +154,7 @@ void AsyncMotionModel::stop() { // Prefer to call onStop in executor's thread so downstream users don't have // to worry about threads in ROS callbacks when there's only 1 thread. - if (node_->get_node_base_interface()->get_context()->is_valid()) { + if (interfaces_.get_node_base_interface()->get_context()->is_valid()) { auto callback = std::make_shared>( std::bind(&AsyncMotionModel::onStop, this) ); diff --git a/fuse_core/src/async_publisher.cpp b/fuse_core/src/async_publisher.cpp index 5ad30fd93..3caeadb53 100644 --- a/fuse_core/src/async_publisher.cpp +++ b/fuse_core/src/async_publisher.cpp @@ -52,14 +52,13 @@ void AsyncPublisher::initialize( node_interfaces::NodeInterfaces interfaces, const std::string & name) { - interfaces_ = interfaces; - if (initialized_) { throw std::runtime_error("Calling initialize on an already initialized AsyncPublisher!"); } // Initialize internal state name_ = name; // NOTE(methylDragon): Used in derived classes + interfaces_ = interfaces; auto context = interfaces_.get_node_base_interface()->get_context(); auto executor_options = rclcpp::ExecutorOptions(); @@ -72,8 +71,7 @@ void AsyncPublisher::initialize( executor_options, executor_thread_count_); } - callback_queue_ = - std::make_shared(context); + callback_queue_ = std::make_shared(context); // This callback group MUST be re-entrant in order to support parallelization cb_group_ = interfaces_.get_node_base_interface()->create_callback_group( diff --git a/fuse_core/src/async_sensor_model.cpp b/fuse_core/src/async_sensor_model.cpp index bbebc2494..80f926448 100644 --- a/fuse_core/src/async_sensor_model.cpp +++ b/fuse_core/src/async_sensor_model.cpp @@ -57,6 +57,7 @@ AsyncSensorModel::~AsyncSensorModel() } void AsyncSensorModel::initialize( + node_interfaces::NodeInterfaces interfaces, const std::string & name, TransactionCallback transaction_callback) { @@ -66,28 +67,25 @@ void AsyncSensorModel::initialize( // Initialize internal state name_ = name; - std::string node_namespace = ""; - - // TODO(CH3): Pass in the context or a node to get the context from - rclcpp::Context::SharedPtr ros_context = rclcpp::contexts::get_global_default_context(); - auto node_options = rclcpp::NodeOptions(); - node_options.context(ros_context); // set a context to generate the node in - - // TODO(CH3): Potentially pass in the optimizer node instead of spinning a new one - node_ = rclcpp::Node::make_shared(name_, node_namespace, node_options); + interfaces_ = interfaces; + auto context = interfaces_.get_node_base_interface()->get_context(); auto executor_options = rclcpp::ExecutorOptions(); - executor_options.context = ros_context; - executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( - executor_options, - executor_thread_count_); + executor_options.context = context; + + if (executor_thread_count_ == 1) { + executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(executor_options); + } else { + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( + executor_options, executor_thread_count_); + } - callback_queue_ = std::make_shared(ros_context); + callback_queue_ = std::make_shared(context); // This callback group MUST be re-entrant in order to support parallelization - cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - node_->get_node_waitables_interface()->add_waitable( - callback_queue_, cb_group_); + cb_group_ = interfaces_.get_node_base_interface()->create_callback_group( + rclcpp::CallbackGroupType::Reentrant, false); + interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, cb_group_); transaction_callback_ = transaction_callback; @@ -95,8 +93,9 @@ void AsyncSensorModel::initialize( onInit(); // Make sure the executor will service the given node - // TODO(sloretz) add just the callback group here when using Optimizer's Node - executor_->add_node(node_); + // We can add this without any guards because the callback group was set to not get automatically + // added to executors + executor_->add_callback_group(cb_group_, interfaces_.get_node_base_interface()); // Start the executor spinner_ = std::thread( @@ -144,7 +143,7 @@ void AsyncSensorModel::stop() { // Prefer to call onStop in executor's thread so downstream users don't have // to worry about threads in ROS callbacks when there's only 1 thread. - if (node_->get_node_base_interface()->get_context()->is_valid()) { + if (interfaces_.get_node_base_interface()->get_context()->is_valid()) { auto callback = std::make_shared>( std::bind(&AsyncSensorModel::onStop, this) ); diff --git a/fuse_core/src/ceres_options.cpp b/fuse_core/src/ceres_options.cpp index 3777fbd2a..37d5c8705 100644 --- a/fuse_core/src/ceres_options.cpp +++ b/fuse_core/src/ceres_options.cpp @@ -53,7 +53,7 @@ namespace fuse_core { // Helper function to get a namespace string with a '.' suffix, but only if not empty -static std::string get_well_formatted_namespace_string(std::string ns) +static std::string get_well_formatted_param_namespace_string(std::string ns) { return ns.empty() || ns.back() == '.' ? ns : ns + "."; } @@ -70,7 +70,7 @@ void loadCovarianceOptionsFromROS( { rcl_interfaces::msg::ParameterDescriptor tmp_descr; - std::string ns = get_well_formatted_namespace_string(namespace_string); + std::string ns = get_well_formatted_param_namespace_string(namespace_string); // The sparse_linear_algebra_library_type field was added to ceres::Covariance::Options in version // 1.13.0, see https://github.com/ceres-solver/ceres- @@ -127,7 +127,7 @@ void loadProblemOptionsFromROS( { rcl_interfaces::msg::ParameterDescriptor tmp_descr; - std::string ns = get_well_formatted_namespace_string(namespace_string); + std::string ns = get_well_formatted_param_namespace_string(namespace_string); tmp_descr.description = "trades memory for faster Problem::RemoveResidualBlock()"; problem_options.enable_fast_removal = fuse_core::getParam( @@ -162,7 +162,7 @@ void loadSolverOptionsFromROS( { rcl_interfaces::msg::ParameterDescriptor tmp_descr; - std::string ns = get_well_formatted_namespace_string(namespace_string); + std::string ns = get_well_formatted_param_namespace_string(namespace_string); // Minimizer options solver_options.minimizer_type = diff --git a/fuse_core/src/fuse_echo.cpp b/fuse_core/src/fuse_echo.cpp index 0286700b0..9cf3f4a01 100644 --- a/fuse_core/src/fuse_echo.cpp +++ b/fuse_core/src/fuse_echo.cpp @@ -54,12 +54,12 @@ class FuseEcho : public rclcpp::Node : Node("fuse_echo", options) { // Subscribe to the constraint topic - graph_subscriber_ = this->create_subscription( + graph_sub_ = this->create_subscription( "graph", rclcpp::QoS(100), std::bind(&FuseEcho::graphCallback, this, std::placeholders::_1) ); - transaction_subscriber_ = this->create_subscription( + transaction_sub_ = this->create_subscription( "transaction", rclcpp::QoS(100), std::bind(&FuseEcho::transactionCallback, this, std::placeholders::_1) @@ -69,8 +69,8 @@ class FuseEcho : public rclcpp::Node private: fuse_core::GraphDeserializer graph_deserializer_; fuse_core::TransactionDeserializer transaction_deserializer_; - rclcpp::Subscription::SharedPtr graph_subscriber_; - rclcpp::Subscription::SharedPtr transaction_subscriber_; + rclcpp::Subscription::SharedPtr graph_sub_; + rclcpp::Subscription::SharedPtr transaction_sub_; void graphCallback(const fuse_msgs::msg::SerializedGraph & msg) { diff --git a/fuse_core/test/test_async_motion_model.cpp b/fuse_core/test/test_async_motion_model.cpp index 038ea4413..5d5452957 100644 --- a/fuse_core/test/test_async_motion_model.cpp +++ b/fuse_core/test/test_async_motion_model.cpp @@ -43,7 +43,7 @@ class MyMotionModel : public fuse_core::AsyncMotionModel { public: MyMotionModel() - : fuse_core::AsyncMotionModel(0), + : fuse_core::AsyncMotionModel(1), initialized(false) { } @@ -91,7 +91,8 @@ TEST_F(TestAsyncMotionModel, OnInit) { for (int i = 0; i < 50; i++) { MyMotionModel motion_model; - motion_model.initialize("my_motion_model_" + std::to_string(i)); + auto node = rclcpp::Node::make_shared("test_async_motion_model_node"); + motion_model.initialize(node, "my_motion_model_" + std::to_string(i)); EXPECT_TRUE(motion_model.initialized); } } @@ -99,15 +100,17 @@ TEST_F(TestAsyncMotionModel, OnInit) TEST_F(TestAsyncMotionModel, DoubleInit) { MyMotionModel motion_model; - motion_model.initialize("my_motion_model"); + auto node = rclcpp::Node::make_shared("test_async_motion_model_node"); + motion_model.initialize(node, "my_motion_model"); EXPECT_TRUE(motion_model.initialized); - EXPECT_THROW(motion_model.initialize("test"), std::runtime_error); + EXPECT_THROW(motion_model.initialize(node, "test"), std::runtime_error); } TEST_F(TestAsyncMotionModel, OnGraphUpdate) { MyMotionModel motion_model; - motion_model.initialize("my_motion_model"); + auto node = rclcpp::Node::make_shared("test_async_motion_model_node"); + motion_model.initialize(node, "my_motion_model"); // Execute the graph callback in this thread. This should push a call to // MyMotionModel::onGraphUpdate() into MyMotionModel's callback queue, which will get executed by @@ -133,7 +136,8 @@ TEST_F(TestAsyncMotionModel, OnGraphUpdate) TEST_F(TestAsyncMotionModel, ApplyCallback) { MyMotionModel motion_model; - motion_model.initialize("my_motion_model"); + auto node = rclcpp::Node::make_shared("test_async_motion_model_node"); + motion_model.initialize(node, "my_motion_model"); // Call the motion model base class "apply()" method to send a transaction to the derived model. // The AsyncMotionModel will then inject a call to applyCallback() into the motion model's diff --git a/fuse_core/test/test_async_sensor_model.cpp b/fuse_core/test/test_async_sensor_model.cpp index 8271d00c4..8e534ba93 100644 --- a/fuse_core/test/test_async_sensor_model.cpp +++ b/fuse_core/test/test_async_sensor_model.cpp @@ -57,7 +57,7 @@ class MySensor : public fuse_core::AsyncSensorModel { public: MySensor() - : fuse_core::AsyncSensorModel(0), + : fuse_core::AsyncSensorModel(1), initialized(false) { } @@ -97,7 +97,8 @@ TEST_F(TestAsyncSensorModel, OnInit) { for (int i = 0; i < 50; i++) { MySensor sensor; - sensor.initialize("my_sensor_" + std::to_string(i), &transactionCallback); + auto node = rclcpp::Node::make_shared("test_async_sensor_model_node"); + sensor.initialize(node, "my_sensor_" + std::to_string(i), &transactionCallback); EXPECT_TRUE(sensor.initialized); } } @@ -105,15 +106,17 @@ TEST_F(TestAsyncSensorModel, OnInit) TEST_F(TestAsyncSensorModel, DoubleInit) { MySensor sensor_model; - sensor_model.initialize("my_sensor_model", &transactionCallback); + auto node = rclcpp::Node::make_shared("test_async_sensor_model_node"); + sensor_model.initialize(node, "my_sensor_model", &transactionCallback); EXPECT_TRUE(sensor_model.initialized); - EXPECT_THROW(sensor_model.initialize("test", &transactionCallback), std::runtime_error); + EXPECT_THROW(sensor_model.initialize(node, "test", &transactionCallback), std::runtime_error); } TEST_F(TestAsyncSensorModel, OnGraphUpdate) { MySensor sensor; - sensor.initialize("my_sensor", &transactionCallback); + auto node = rclcpp::Node::make_shared("test_async_sensor_model_node"); + sensor.initialize(node, "my_sensor", &transactionCallback); // Execute the graph callback in this thread. This should push a call to MySensor::onGraphUpdate() // into MySensor's callback queue, which will get executed by MySensor's async spinner. @@ -138,7 +141,8 @@ TEST_F(TestAsyncSensorModel, OnGraphUpdate) TEST_F(TestAsyncSensorModel, SendTransaction) { MySensor sensor; - sensor.initialize("my_sensor", &transactionCallback); + auto node = rclcpp::Node::make_shared("test_async_sensor_model_node"); + sensor.initialize(node, "my_sensor", &transactionCallback); // Use the sensor "sendTransaction()" method to execute the transaction callback. This will get // executed immediately. diff --git a/fuse_core/test/test_throttled_callback.cpp b/fuse_core/test/test_throttled_callback.cpp index 8548a45fd..b12931b3f 100644 --- a/fuse_core/test/test_throttled_callback.cpp +++ b/fuse_core/test/test_throttled_callback.cpp @@ -265,7 +265,7 @@ TEST_F(TestThrottledCallback, NoDroppedMessagesIfThrottlePeriodIsZero) TEST_F(TestThrottledCallback, DropMessagesIfThrottlePeriodIsGreaterThanPublishPeriod) { // Start sensor model to listen to messages: - const rclcpp::Duration throttled_period(0, RCUTILS_S_TO_NS(0.2)); + const rclcpp::Duration throttled_period(0, static_cast(RCUTILS_S_TO_NS(0.2))); auto sensor_model = std::make_shared(throttled_period); executor_->add_node(sensor_model); diff --git a/fuse_graphs/fuse_plugins.xml b/fuse_graphs/fuse_plugins.xml index 90743b283..0b7be8ae4 100644 --- a/fuse_graphs/fuse_plugins.xml +++ b/fuse_graphs/fuse_plugins.xml @@ -1,4 +1,4 @@ - + This is a concrete implementation of the Graph interface using hashmaps to store the constraints and variables. diff --git a/fuse_loss/fuse_plugins.xml b/fuse_loss/fuse_plugins.xml index 4ebb5e478..d9227cafd 100644 --- a/fuse_loss/fuse_plugins.xml +++ b/fuse_loss/fuse_plugins.xml @@ -1,4 +1,4 @@ - + - benchmark - rostest + benchmark + ament_cmake_gtest + ament_cmake_gmock + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/fuse_models/src/acceleration_2d.cpp b/fuse_models/src/acceleration_2d.cpp index 18954ae44..8e44a761f 100644 --- a/fuse_models/src/acceleration_2d.cpp +++ b/fuse_models/src/acceleration_2d.cpp @@ -37,9 +37,9 @@ #include #include -#include -#include -#include +#include +#include +#include // Register this sensor model with ROS as a plugin. @@ -51,61 +51,94 @@ namespace fuse_models Acceleration2D::Acceleration2D() : fuse_core::AsyncSensorModel(1), device_id_(fuse_core::uuid::NIL), - tf_listener_(tf_buffer_), + logger_(rclcpp::get_logger("uninitialized")), throttled_callback_(std::bind(&Acceleration2D::process, this, std::placeholders::_1)) { } +void Acceleration2D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + void Acceleration2D::onInit() { + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + // Read settings from the parameter sever - device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + device_id_ = fuse_variables::loadDeviceId(interfaces_); - params_.loadFromROS(private_node_handle_); + params_.loadFromROS(interfaces_, name_); throttled_callback_.setThrottlePeriod(params_.throttle_period); if (!params_.throttle_use_wall_time) { - throttled_callback_.setClock(node_->get_clock()); + throttled_callback_.setClock(clock_); } if (params_.indices.empty()) { - RCLCPP_WARN_STREAM(node_->get_logger(), + RCLCPP_WARN_STREAM(logger_, "No dimensions were specified. Data from topic " - << ros::names::resolve(params_.topic) << " will be ignored."); + << fuse_core::joinTopicName(name_, params_.topic) << " will be ignored."); } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); } void Acceleration2D::onStart() { if (!params_.indices.empty()) { - subscriber_ = node_handle_.subscribe( - ros::names::resolve(params_.topic), params_.queue_size, &AccelerationThrottledCallback::callback, - &throttled_callback_, ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + fuse_core::joinTopicName(name_, params_.topic), + params_.queue_size, + std::bind( + &AccelerationThrottledCallback::callback< + const geometry_msgs::msg::AccelWithCovarianceStamped &>, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); } } void Acceleration2D::onStop() { - subscriber_.shutdown(); + sub_.reset(); } -void Acceleration2D::process(const geometry_msgs::AccelWithCovarianceStamped::ConstPtr& msg) +void Acceleration2D::process(const geometry_msgs::msg::AccelWithCovarianceStamped& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); - transaction->stamp(msg->header.stamp); + transaction->stamp(msg.header.stamp); common::processAccelWithCovariance( name(), device_id_, - *msg, + msg, params_.loss, params_.target_frame, params_.indices, - tf_buffer_, + *tf_buffer_, !params_.disable_checks, *transaction, params_.tf_timeout); diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index 9e8947189..1c26d55a9 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -34,9 +34,9 @@ #include -#include +#include -#include +#include #include #include @@ -48,26 +48,62 @@ PLUGINLIB_EXPORT_CLASS(fuse_models::GraphIgnition, fuse_core::SensorModel); namespace fuse_models { -GraphIgnition::GraphIgnition() : fuse_core::AsyncSensorModel(1), started_(false) +GraphIgnition::GraphIgnition() : + fuse_core::AsyncSensorModel(1), + started_(false), + logger_(rclcpp::get_logger("uninitialized")) { } +void GraphIgnition::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + void GraphIgnition::onInit() { + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + // Read settings from the parameter sever - params_.loadFromROS(private_node_handle_); + params_.loadFromROS(interfaces_, name_); // Connect to the reset service if (!params_.reset_service.empty()) { - reset_client_ = node_handle_.serviceClient(ros::names::resolve(params_.reset_service)); + reset_client_ = rclcpp::create_client( + interfaces_.get_node_base_interface(), + interfaces_.get_node_graph_interface(), + interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.reset_service), + rclcpp::ServicesQoS(), + cb_group_ + ); } // Advertise - subscriber_ = node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, - &GraphIgnition::subscriberCallback, this); - set_graph_service_ = node_handle_.advertiseService(ros::names::resolve(params_.set_graph_service), - &GraphIgnition::setGraphServiceCallback, this); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + sub_ = rclcpp::create_subscription( + interfaces_, + fuse_core::joinTopicName(name_, params_.topic), + params_.queue_size, + std::bind(&GraphIgnition::subscriberCallback, this, std::placeholders::_1), + sub_options + ); + + set_graph_service_ = rclcpp::create_service( + interfaces_.get_node_base_interface(), + interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_graph_service), + std::bind( + &GraphIgnition::setGraphServiceCallback, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS(), + cb_group_ + ); } void GraphIgnition::start() @@ -80,35 +116,37 @@ void GraphIgnition::stop() started_ = false; } -void GraphIgnition::subscriberCallback(const fuse_msgs::SerializedGraph::ConstPtr& msg) +void GraphIgnition::subscriberCallback(const fuse_msgs::msg::SerializedGraph& msg) { try { - process(*msg); + process(msg); } catch (const std::exception& e) { - RCLCPP_ERROR_STREAM(node_->get_logger(), e.what() << " Ignoring message."); + RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); } } -bool GraphIgnition::setGraphServiceCallback(fuse_models::SetGraph::Request& req, fuse_models::SetGraph::Response& res) +bool GraphIgnition::setGraphServiceCallback( + const fuse_msgs::srv::SetGraph::Request::SharedPtr req, + fuse_msgs::srv::SetGraph::Response::SharedPtr res) { try { - process(req.graph); - res.success = true; + process(req->graph); + res->success = true; } catch (const std::exception& e) { - res.success = false; - res.message = e.what(); - RCLCPP_ERROR_STREAM(node_->get_logger(), e.what() << " Ignoring request."); + res->success = false; + res->message = e.what(); + RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); } return true; } -void GraphIgnition::process(const fuse_msgs::SerializedGraph& msg) +void GraphIgnition::process(const fuse_msgs::msg::SerializedGraph& msg) { // Verify we are in the correct state to process set graph requests if (!started_) @@ -134,18 +172,17 @@ void GraphIgnition::process(const fuse_msgs::SerializedGraph& msg) if (!params_.reset_service.empty()) { // Wait for the reset service - while (!reset_client_.waitForExistence(rclcpp::Duration::from_seconds(10.0)) && ros::ok()) + while (!reset_client_->wait_for_service(std::chrono::seconds(10)) + && interfaces_.get_node_base_interface()->get_context()->is_valid()) { - RCLCPP_WARN_STREAM(node_->get_logger(), - "Waiting for '" << reset_client_.getService() << "' service to become avaiable."); + RCLCPP_WARN_STREAM(logger_, + "Waiting for '" << reset_client_->wait_for_service() << "' service to become avaiable."); } - auto srv = std_srvs::Empty(); - if (!reset_client_.call(srv)) - { - // The reset() service failed. Propagate that failure to the caller of this service. - throw std::runtime_error("Failed to call the '" + reset_client_.getService() + "' service."); - } + auto srv = std::make_shared(); + // No need to spin since node is optimizer node, which should be spinning + auto result_future = reset_client_->async_send_request(srv); + result_future.wait(); } // Now that the optimizer has been reset, actually send the initial state constraints to the optimizer @@ -187,8 +224,9 @@ void GraphIgnition::sendGraph(const fuse_core::Graph& graph, const rclcpp::Time& // Send the transaction to the optimizer. sendTransaction(transaction); - RCLCPP_INFO_STREAM(node_->get_logger(), - "Received a set_graph request (stamp: " << transaction->stamp() << ", constraints: " + RCLCPP_INFO_STREAM(logger_, + "Received a set_graph request (stamp: " + << transaction->stamp().nanoseconds() << ", constraints: " << boost::size(transaction->addedConstraints()) << ", variables: " << boost::size(transaction->addedVariables()) << ")"); } diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp index 200ac9f27..7094eef05 100644 --- a/fuse_models/src/imu_2d.cpp +++ b/fuse_models/src/imu_2d.cpp @@ -37,12 +37,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -57,32 +57,53 @@ namespace fuse_models Imu2D::Imu2D() : fuse_core::AsyncSensorModel(1), device_id_(fuse_core::uuid::NIL), - tf_listener_(tf_buffer_), + logger_(rclcpp::get_logger("uninitialized")), throttled_callback_(std::bind(&Imu2D::process, this, std::placeholders::_1)) { } +void Imu2D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + void Imu2D::onInit() { + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + // Read settings from the parameter sever - device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + device_id_ = fuse_variables::loadDeviceId(interfaces_); - params_.loadFromROS(private_node_handle_); + params_.loadFromROS(interfaces_, name_); throttled_callback_.setThrottlePeriod(params_.throttle_period); if (!params_.throttle_use_wall_time) { - throttled_callback_.setClock(node_->get_clock()); + throttled_callback_.setClock(clock_); } if (params_.orientation_indices.empty() && params_.linear_acceleration_indices.empty() && params_.angular_velocity_indices.empty()) { - RCLCPP_WARN_STREAM(node_->get_logger(), - "No dimensions were specified. Data from topic " << ros::names::resolve(params_.topic) + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << fuse_core::joinTopicName(name_, params_.topic) << " will be ignored."); } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); } void Imu2D::onStart() @@ -92,49 +113,61 @@ void Imu2D::onStart() !params_.angular_velocity_indices.empty()) { previous_pose_.reset(); - subscriber_ = node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, - &ImuThrottledCallback::callback, &throttled_callback_, - ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + fuse_core::joinTopicName(name_, params_.topic), + params_.queue_size, + std::bind( + &ImuThrottledCallback::callback, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); } } void Imu2D::onStop() { - subscriber_.shutdown(); + sub_.reset(); } -void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg) +void Imu2D::process(const sensor_msgs::msg::Imu& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); - transaction->stamp(msg->header.stamp); + transaction->stamp(msg.header.stamp); // Handle the orientation data (treat it as a pose, but with only orientation indices used) - auto pose = std::make_unique(); - pose->header = msg->header; - pose->pose.pose.orientation = msg->orientation; - pose->pose.covariance[21] = msg->orientation_covariance[0]; - pose->pose.covariance[22] = msg->orientation_covariance[1]; - pose->pose.covariance[23] = msg->orientation_covariance[2]; - pose->pose.covariance[27] = msg->orientation_covariance[3]; - pose->pose.covariance[28] = msg->orientation_covariance[4]; - pose->pose.covariance[29] = msg->orientation_covariance[5]; - pose->pose.covariance[33] = msg->orientation_covariance[6]; - pose->pose.covariance[34] = msg->orientation_covariance[7]; - pose->pose.covariance[35] = msg->orientation_covariance[8]; - - geometry_msgs::TwistWithCovarianceStamped twist; - twist.header = msg->header; - twist.twist.twist.angular = msg->angular_velocity; - twist.twist.covariance[21] = msg->angular_velocity_covariance[0]; - twist.twist.covariance[22] = msg->angular_velocity_covariance[1]; - twist.twist.covariance[23] = msg->angular_velocity_covariance[2]; - twist.twist.covariance[27] = msg->angular_velocity_covariance[3]; - twist.twist.covariance[28] = msg->angular_velocity_covariance[4]; - twist.twist.covariance[29] = msg->angular_velocity_covariance[5]; - twist.twist.covariance[33] = msg->angular_velocity_covariance[6]; - twist.twist.covariance[34] = msg->angular_velocity_covariance[7]; - twist.twist.covariance[35] = msg->angular_velocity_covariance[8]; + auto pose = std::make_unique(); + pose->header = msg.header; + pose->pose.pose.orientation = msg.orientation; + pose->pose.covariance[21] = msg.orientation_covariance[0]; + pose->pose.covariance[22] = msg.orientation_covariance[1]; + pose->pose.covariance[23] = msg.orientation_covariance[2]; + pose->pose.covariance[27] = msg.orientation_covariance[3]; + pose->pose.covariance[28] = msg.orientation_covariance[4]; + pose->pose.covariance[29] = msg.orientation_covariance[5]; + pose->pose.covariance[33] = msg.orientation_covariance[6]; + pose->pose.covariance[34] = msg.orientation_covariance[7]; + pose->pose.covariance[35] = msg.orientation_covariance[8]; + + geometry_msgs::msg::TwistWithCovarianceStamped twist; + twist.header = msg.header; + twist.twist.twist.angular = msg.angular_velocity; + twist.twist.covariance[21] = msg.angular_velocity_covariance[0]; + twist.twist.covariance[22] = msg.angular_velocity_covariance[1]; + twist.twist.covariance[23] = msg.angular_velocity_covariance[2]; + twist.twist.covariance[27] = msg.angular_velocity_covariance[3]; + twist.twist.covariance[28] = msg.angular_velocity_covariance[4]; + twist.twist.covariance[29] = msg.angular_velocity_covariance[5]; + twist.twist.covariance[33] = msg.angular_velocity_covariance[6]; + twist.twist.covariance[34] = msg.angular_velocity_covariance[7]; + twist.twist.covariance[35] = msg.angular_velocity_covariance[8]; const bool validate = !params_.disable_checks; @@ -152,7 +185,7 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg) params_.orientation_target_frame, {}, params_.orientation_indices, - tf_buffer_, + *tf_buffer_, validate, *transaction, params_.tf_timeout); @@ -168,33 +201,33 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg) params_.twist_target_frame, {}, params_.angular_velocity_indices, - tf_buffer_, + *tf_buffer_, validate, *transaction, params_.tf_timeout); // Handle the acceleration data - geometry_msgs::AccelWithCovarianceStamped accel; - accel.header = msg->header; - accel.accel.accel.linear = msg->linear_acceleration; - accel.accel.covariance[0] = msg->linear_acceleration_covariance[0]; - accel.accel.covariance[1] = msg->linear_acceleration_covariance[1]; - accel.accel.covariance[2] = msg->linear_acceleration_covariance[2]; - accel.accel.covariance[6] = msg->linear_acceleration_covariance[3]; - accel.accel.covariance[7] = msg->linear_acceleration_covariance[4]; - accel.accel.covariance[8] = msg->linear_acceleration_covariance[5]; - accel.accel.covariance[12] = msg->linear_acceleration_covariance[6]; - accel.accel.covariance[13] = msg->linear_acceleration_covariance[7]; - accel.accel.covariance[14] = msg->linear_acceleration_covariance[8]; + geometry_msgs::msg::AccelWithCovarianceStamped accel; + accel.header = msg.header; + accel.accel.accel.linear = msg.linear_acceleration; + accel.accel.covariance[0] = msg.linear_acceleration_covariance[0]; + accel.accel.covariance[1] = msg.linear_acceleration_covariance[1]; + accel.accel.covariance[2] = msg.linear_acceleration_covariance[2]; + accel.accel.covariance[6] = msg.linear_acceleration_covariance[3]; + accel.accel.covariance[7] = msg.linear_acceleration_covariance[4]; + accel.accel.covariance[8] = msg.linear_acceleration_covariance[5]; + accel.accel.covariance[12] = msg.linear_acceleration_covariance[6]; + accel.accel.covariance[13] = msg.linear_acceleration_covariance[7]; + accel.accel.covariance[14] = msg.linear_acceleration_covariance[8]; // Optionally remove the acceleration due to gravity if (params_.remove_gravitational_acceleration) { - geometry_msgs::Vector3 accel_gravity; + geometry_msgs::msg::Vector3 accel_gravity; accel_gravity.z = params_.gravitational_acceleration; - geometry_msgs::TransformStamped orientation_trans; + geometry_msgs::msg::TransformStamped orientation_trans; tf2::Quaternion imu_orientation; - tf2::fromMsg(msg->orientation, imu_orientation); + tf2::fromMsg(msg.orientation, imu_orientation); orientation_trans.transform.rotation = tf2::toMsg(imu_orientation.inverse()); tf2::doTransform(accel_gravity, accel_gravity, orientation_trans); // Doesn't use the stamp accel.accel.accel.linear.x -= accel_gravity.x; @@ -209,7 +242,7 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg) params_.linear_acceleration_loss, params_.acceleration_target_frame, params_.linear_acceleration_indices, - tf_buffer_, + *tf_buffer_, validate, *transaction, params_.tf_timeout); @@ -218,18 +251,18 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg) sendTransaction(transaction); } -void Imu2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped& pose, - const geometry_msgs::TwistWithCovarianceStamped& twist, const bool validate, +void Imu2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, fuse_core::Transaction& transaction) { - auto transformed_pose = std::make_unique(); + auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = params_.orientation_target_frame.empty() ? pose.header.frame_id : params_.orientation_target_frame; - if (!common::transformMessage(tf_buffer_, pose, *transformed_pose)) + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 5.0 * 1000, - "Cannot transform pose message with stamp " << pose.header.stamp + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " << rclcpp::Time(pose.header.stamp).nanoseconds() << " to orientation target frame " << params_.orientation_target_frame); return; } @@ -242,14 +275,14 @@ void Imu2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped& if (params_.use_twist_covariance) { - geometry_msgs::TwistWithCovarianceStamped transformed_twist; + geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; transformed_twist.header.frame_id = params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - if (!common::transformMessage(tf_buffer_, twist, transformed_twist)) + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 5.0 * 1000, - "Cannot transform twist message with stamp " << twist.header.stamp + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " << rclcpp::Time(twist.header.stamp).nanoseconds() << " to twist target frame " << params_.twist_target_frame); } else diff --git a/fuse_models/src/odometry_2d.cpp b/fuse_models/src/odometry_2d.cpp index f04c50450..55efa607b 100644 --- a/fuse_models/src/odometry_2d.cpp +++ b/fuse_models/src/odometry_2d.cpp @@ -37,11 +37,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -56,22 +56,34 @@ namespace fuse_models Odometry2D::Odometry2D() : fuse_core::AsyncSensorModel(1), device_id_(fuse_core::uuid::NIL), - tf_listener_(tf_buffer_), + logger_(rclcpp::get_logger("uninitialized")), throttled_callback_(std::bind(&Odometry2D::process, this, std::placeholders::_1)) { } +void Odometry2D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + void Odometry2D::onInit() { + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + // Read settings from the parameter sever - device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + device_id_ = fuse_variables::loadDeviceId(interfaces_); - params_.loadFromROS(private_node_handle_); + params_.loadFromROS(interfaces_, name_); throttled_callback_.setThrottlePeriod(params_.throttle_period); if (!params_.throttle_use_wall_time) { - throttled_callback_.setClock(node_->get_clock()); + throttled_callback_.setClock(clock_); } if (params_.position_indices.empty() && @@ -79,10 +91,19 @@ void Odometry2D::onInit() params_.linear_velocity_indices.empty() && params_.angular_velocity_indices.empty()) { - RCLCPP_WARN_STREAM(node_->get_logger(), - "No dimensions were specified. Data from topic " << ros::names::resolve(params_.topic) + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << fuse_core::joinTopicName(name_, params_.topic) << " will be ignored."); } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); } void Odometry2D::onStart() @@ -93,32 +114,45 @@ void Odometry2D::onStart() !params_.angular_velocity_indices.empty()) { previous_pose_.reset(); - subscriber_ = node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, - &OdometryThrottledCallback::callback, &throttled_callback_, - ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + fuse_core::joinTopicName(name_, params_.topic), + params_.queue_size, + std::bind( + &OdometryThrottledCallback::callback< + const nav_msgs::msg::Odometry &>, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); } } void Odometry2D::onStop() { - subscriber_.shutdown(); + sub_.reset(); } -void Odometry2D::process(const nav_msgs::Odometry::ConstPtr& msg) +void Odometry2D::process(const nav_msgs::msg::Odometry& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); - transaction->stamp(msg->header.stamp); + transaction->stamp(msg.header.stamp); // Handle the pose data - auto pose = std::make_unique(); - pose->header = msg->header; - pose->pose = msg->pose; + auto pose = std::make_unique(); + pose->header = msg.header; + pose->pose = msg.pose; - geometry_msgs::TwistWithCovarianceStamped twist; - twist.header = msg->header; - twist.header.frame_id = msg->child_frame_id; - twist.twist = msg->twist; + geometry_msgs::msg::TwistWithCovarianceStamped twist; + twist.header = msg.header; + twist.header.frame_id = msg.child_frame_id; + twist.twist = msg.twist; const bool validate = !params_.disable_checks; @@ -136,7 +170,7 @@ void Odometry2D::process(const nav_msgs::Odometry::ConstPtr& msg) params_.pose_target_frame, params_.position_indices, params_.orientation_indices, - tf_buffer_, + *tf_buffer_, validate, *transaction, params_.tf_timeout); @@ -152,7 +186,7 @@ void Odometry2D::process(const nav_msgs::Odometry::ConstPtr& msg) params_.twist_target_frame, params_.linear_velocity_indices, params_.angular_velocity_indices, - tf_buffer_, + *tf_buffer_, validate, *transaction, params_.tf_timeout); @@ -161,19 +195,19 @@ void Odometry2D::process(const nav_msgs::Odometry::ConstPtr& msg) sendTransaction(transaction); } -void Odometry2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped& pose, - const geometry_msgs::TwistWithCovarianceStamped& twist, const bool validate, +void Odometry2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, fuse_core::Transaction& transaction) { - auto transformed_pose = std::make_unique(); + auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; - if (!common::transformMessage(tf_buffer_, pose, *transformed_pose)) + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 5.0 * 1000, + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, "Cannot transform pose message with stamp " - << pose.header.stamp << " to pose target frame " << params_.pose_target_frame); + << rclcpp::Time(pose.header.stamp).nanoseconds() << " to pose target frame " << params_.pose_target_frame); return; } @@ -185,14 +219,14 @@ void Odometry2D::processDifferential(const geometry_msgs::PoseWithCovarianceStam if (params_.use_twist_covariance) { - geometry_msgs::TwistWithCovarianceStamped transformed_twist; + geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; transformed_twist.header.frame_id = params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - if (!common::transformMessage(tf_buffer_, twist, transformed_twist)) + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 5.0 * 1000, - "Cannot transform twist message with stamp " << twist.header.stamp + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " << rclcpp::Time(twist.header.stamp).nanoseconds() << " to twist target frame " << params_.twist_target_frame); } else diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp index bb87ad169..48e034992 100644 --- a/fuse_models/src/odometry_2d_publisher.cpp +++ b/fuse_models/src/odometry_2d_publisher.cpp @@ -39,15 +39,16 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include +#include #include #include #include @@ -63,31 +64,63 @@ namespace fuse_models Odometry2DPublisher::Odometry2DPublisher() : fuse_core::AsyncPublisher(1), device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")), latest_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)), - latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)), - publish_timer_spinner_(1, &publish_timer_callback_queue_) + latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { } +void Odometry2DPublisher::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name) +{ + interfaces_ = interfaces; + fuse_core::AsyncPublisher::initialize(interfaces, name); +} + void Odometry2DPublisher::onInit() { + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + tf_broadcaster_ = std::make_shared(interfaces_); + // Read settings from the parameter sever - device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + device_id_ = fuse_variables::loadDeviceId(interfaces_); - params_.loadFromROS(private_node_handle_); + params_.loadFromROS(interfaces_, name_); if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { - tf_buffer_ = std::make_unique(params_.tf_cache_time); - tf_listener_ = std::make_unique(*tf_buffer_, node_handle_); + tf_buffer_ = std::make_unique( + clock_, + params_.tf_cache_time.to_chrono() + // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h + // TODO(methylDragon): See above ^ + ); + + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } - odom_pub_ = node_handle_.advertise(ros::names::resolve(params_.topic), params_.queue_size); - acceleration_pub_ = node_handle_.advertise( - ros::names::resolve(params_.acceleration_topic), params_.queue_size); - - publish_timer_node_handle_.setCallbackQueue(&publish_timer_callback_queue_); - publish_timer_spinner_.start(); + // Advertise the topics + rclcpp::PublisherOptions pub_options; + pub_options.callback_group = cb_group_; + + odom_pub_ = rclcpp::create_publisher( + interfaces_, + fuse_core::joinTopicName(name_, params_.topic), + params_.queue_size, + pub_options); + acceleration_pub_ = rclcpp::create_publisher( + interfaces_, + fuse_core::joinTopicName(name_, params_.acceleration_topic), + params_.queue_size, + pub_options); } void Odometry2DPublisher::notifyCallback( @@ -103,7 +136,7 @@ void Odometry2DPublisher::notifyCallback( latest_stamp_ = latest_stamp; } - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 10.0 * 1000, + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, "Failed to find a matching set of state variables with device id '" << device_id_ << "'."); return; @@ -116,8 +149,8 @@ void Odometry2DPublisher::notifyCallback( fuse_core::UUID velocity_angular_uuid; fuse_core::UUID acceleration_linear_uuid; - nav_msgs::Odometry odom_output; - geometry_msgs::AccelWithCovarianceStamped acceleration_output; + nav_msgs::msg::Odometry odom_output; + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; if (!getState( *graph, latest_stamp, @@ -145,10 +178,10 @@ void Odometry2DPublisher::notifyCallback( // Don't waste CPU computing the covariance if nobody is listening rclcpp::Time latest_covariance_stamp = latest_covariance_stamp_; bool latest_covariance_valid = latest_covariance_valid_; - if (odom_pub_.getNumSubscribers() > 0 || acceleration_pub_.getNumSubscribers() > 0) + if (odom_pub_->get_subscription_count() > 0 || acceleration_pub_->get_subscription_count() > 0) { // Throttle covariance computation - if (params_.covariance_throttle_period.isZero() || + if (params_.covariance_throttle_period.nanoseconds() == 0 || latest_stamp - latest_covariance_stamp > params_.covariance_throttle_period) { latest_covariance_stamp = latest_stamp; @@ -196,8 +229,8 @@ void Odometry2DPublisher::notifyCallback( } catch (const std::exception& e) { - RCLCPP_WARN_STREAM(node_->get_logger(), - "An error occurred computing the covariance information for " << latest_stamp + RCLCPP_WARN_STREAM(logger_, + "An error occurred computing the covariance information for " << latest_stamp.nanoseconds() << ". The covariance will be set to zero.\n" << e.what()); std::fill(odom_output.pose.covariance.begin(), odom_output.pose.covariance.end(), 0.0); std::fill(odom_output.twist.covariance.begin(), odom_output.twist.covariance.end(), 0.0); @@ -234,13 +267,16 @@ void Odometry2DPublisher::onStart() synchronizer_ = Synchronizer(device_id_); latest_stamp_ = latest_covariance_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); latest_covariance_valid_ = false; - odom_output_ = nav_msgs::Odometry(); - acceleration_output_ = geometry_msgs::AccelWithCovarianceStamped(); + odom_output_ = nav_msgs::msg::Odometry(); + acceleration_output_ = geometry_msgs::msg::AccelWithCovarianceStamped(); // TODO(CH3): Add this to a separate callback group for async behavior - publish_timer_ = this->node_.create_timer( - rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency), - std::bind(&Odometry2DPublisher::publishTimerCallback, this) + publish_timer_ = rclcpp::create_timer( + interfaces_, + clock_, + std::chrono::duration(1.0 / params_.publish_frequency), + std::move(std::bind(&Odometry2DPublisher::publishTimerCallback, this)), + cb_group_ ); delayed_throttle_filter_.reset(); @@ -248,7 +284,7 @@ void Odometry2DPublisher::onStart() void Odometry2DPublisher::onStop() { - publish_timer_.cancel(); + publish_timer_->cancel(); } bool Odometry2DPublisher::getState( @@ -260,8 +296,8 @@ bool Odometry2DPublisher::getState( fuse_core::UUID& velocity_linear_uuid, fuse_core::UUID& velocity_angular_uuid, fuse_core::UUID& acceleration_linear_uuid, - nav_msgs::Odometry& odometry, - geometry_msgs::AccelWithCovarianceStamped& acceleration) + nav_msgs::msg::Odometry& odometry, + geometry_msgs::msg::AccelWithCovarianceStamped& acceleration) { try { @@ -305,14 +341,14 @@ bool Odometry2DPublisher::getState( } catch (const std::exception& e) { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 10.0 * 1000, - "Failed to find a state at time " << stamp << ". Error: " << e.what()); + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); return false; } catch (...) { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 10.0 * 1000, - "Failed to find a state at time " << stamp << ". Error: unknown"); + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: unknown"); return false; } @@ -324,8 +360,8 @@ void Odometry2DPublisher::publishTimerCallback() rclcpp::Time latest_stamp; rclcpp::Time latest_covariance_stamp; bool latest_covariance_valid; - nav_msgs::Odometry odom_output; - geometry_msgs::AccelWithCovarianceStamped acceleration_output; + nav_msgs::msg::Odometry odom_output; + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; { std::lock_guard lock(mutex_); @@ -339,7 +375,7 @@ void Odometry2DPublisher::publishTimerCallback() if (!fuse_core::is_valid(latest_stamp)) { RCLCPP_WARN_STREAM_EXPRESSION( - node_->get_logger(), delayed_throttle_filter_.isEnabled(), + logger_, delayed_throttle_filter_.isEnabled(), "No valid state data yet. Delaying tf broadcast."); return; } @@ -350,10 +386,11 @@ void Odometry2DPublisher::publishTimerCallback() // If requested, we need to project our state forward in time using the 2D kinematic model if (params_.predict_to_current_time) { + rclcpp::Time timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); tf2_2d::Vector2 velocity_linear; tf2::fromMsg(odom_output.twist.twist.linear, velocity_linear); - const double dt = event.current_real.seconds() - odom_output.header.stamp.seconds(); + const double dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); fuse_core::Matrix8d jacobian; @@ -391,8 +428,8 @@ void Odometry2DPublisher::publishTimerCallback() acceleration_output.accel.accel.linear.y = acceleration_linear.y(); } - odom_output.header.stamp = event.current_real; - acceleration_output.header.stamp = event.current_real; + odom_output.header.stamp = timer_now; + acceleration_output.header.stamp = timer_now; // Either the last covariance computation was skipped because there was no subscriber, // or it failed @@ -470,8 +507,8 @@ void Odometry2DPublisher::publishTimerCallback() } } - odom_pub_.publish(odom_output); - acceleration_pub_.publish(acceleration_output); + odom_pub_->publish(odom_output); + acceleration_pub_->publish(acceleration_output); if (params_.publish_tf) { @@ -484,7 +521,7 @@ void Odometry2DPublisher::publishTimerCallback() std::swap(frame_id, child_frame_id); } - geometry_msgs::TransformStamped trans; + geometry_msgs::msg::TransformStamped trans; trans.header.stamp = odom_output.header.stamp; trans.header.frame_id = frame_id; trans.child_frame_id = child_frame_id; @@ -503,7 +540,7 @@ void Odometry2DPublisher::publishTimerCallback() trans.header.stamp, params_.tf_timeout); - geometry_msgs::TransformStamped map_to_odom; + geometry_msgs::msg::TransformStamped map_to_odom; tf2::doTransform(base_to_odom, map_to_odom, trans); map_to_odom.child_frame_id = params_.odom_frame_id; trans = map_to_odom; @@ -511,7 +548,7 @@ void Odometry2DPublisher::publishTimerCallback() catch (const std::exception& e) { RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5.0 * 1000, + logger_, *clock_, 5.0 * 1000, "Could not lookup the " << params_.base_link_frame_id << "->" << params_.odom_frame_id<< " transform. Error: " << e.what()); @@ -519,7 +556,7 @@ void Odometry2DPublisher::publishTimerCallback() } } - tf_broadcaster_.sendTransform(trans); + tf_broadcaster_->sendTransform(trans); } } diff --git a/fuse_models/src/pose_2d.cpp b/fuse_models/src/pose_2d.cpp index fc5dda05e..d2b82b6cd 100644 --- a/fuse_models/src/pose_2d.cpp +++ b/fuse_models/src/pose_2d.cpp @@ -37,9 +37,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -54,31 +54,52 @@ namespace fuse_models Pose2D::Pose2D() : fuse_core::AsyncSensorModel(1), device_id_(fuse_core::uuid::NIL), - tf_listener_(tf_buffer_), + logger_(rclcpp::get_logger("uninitialized")), throttled_callback_(std::bind(&Pose2D::process, this, std::placeholders::_1)) { } +void Pose2D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + void Pose2D::onInit() { + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + // Read settings from the parameter sever - device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + device_id_ = fuse_variables::loadDeviceId(interfaces_); - params_.loadFromROS(private_node_handle_); + params_.loadFromROS(interfaces_, name_); throttled_callback_.setThrottlePeriod(params_.throttle_period); if (!params_.throttle_use_wall_time) { - throttled_callback_.setClock(node_->get_clock()); + throttled_callback_.setClock(clock_); } if (params_.position_indices.empty() && params_.orientation_indices.empty()) { - RCLCPP_WARN_STREAM(node_->get_logger(), - "No dimensions were specified. Data from topic " << ros::names::resolve(params_.topic) + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << fuse_core::joinTopicName(name_, params_.topic) << " will be ignored."); } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); } void Pose2D::onStart() @@ -86,40 +107,52 @@ void Pose2D::onStart() if (!params_.position_indices.empty() || !params_.orientation_indices.empty()) { - subscriber_ = node_handle_.subscribe( - ros::names::resolve(params_.topic), params_.queue_size, &PoseThrottledCallback::callback, &throttled_callback_, - ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + fuse_core::joinTopicName(name_, params_.topic), + params_.queue_size, + std::bind( + &PoseThrottledCallback::callback< + const geometry_msgs::msg::PoseWithCovarianceStamped &>, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); } } void Pose2D::onStop() { - subscriber_.shutdown(); + sub_.reset(); } -void Pose2D::process(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) +void Pose2D::process(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); - transaction->stamp(msg->header.stamp); + transaction->stamp(msg.header.stamp); const bool validate = !params_.disable_checks; if (params_.differential) { - processDifferential(*msg, validate, *transaction); + processDifferential(msg, validate, *transaction); } else { common::processAbsolutePoseWithCovariance( name(), device_id_, - *msg, + msg, params_.loss, params_.target_frame, params_.position_indices, params_.orientation_indices, - tf_buffer_, + *tf_buffer_, validate, *transaction, params_.tf_timeout); @@ -129,16 +162,16 @@ void Pose2D::process(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& m sendTransaction(transaction); } -void Pose2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped& pose, const bool validate, +void Pose2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, const bool validate, fuse_core::Transaction& transaction) { - auto transformed_pose = std::make_unique(); + auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = params_.target_frame.empty() ? pose.header.frame_id : params_.target_frame; - if (!common::transformMessage(tf_buffer_, pose, *transformed_pose)) + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 5.0 * 1000, - "Cannot transform pose message with stamp " << pose.header.stamp + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " << rclcpp::Time(pose.header.stamp).nanoseconds() << " to target frame " << params_.target_frame); return; } diff --git a/fuse_models/src/transaction.cpp b/fuse_models/src/transaction.cpp index fa4b80d16..e43814c9b 100644 --- a/fuse_models/src/transaction.cpp +++ b/fuse_models/src/transaction.cpp @@ -34,8 +34,8 @@ #include -#include -#include +#include +#include // Register this sensor model with ROS as a plugin. PLUGINLIB_EXPORT_CLASS(fuse_models::Transaction, fuse_core::SensorModel) @@ -47,27 +47,44 @@ Transaction::Transaction() : fuse_core::AsyncSensorModel(1) { } +void Transaction::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + void Transaction::onInit() { // Read settings from the parameter sever - params_.loadFromROS(private_node_handle_); + params_.loadFromROS(interfaces_, name_); } void Transaction::onStart() { - subscriber_ = - node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, &Transaction::process, this); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + fuse_core::joinTopicName(name_, params_.topic), + params_.queue_size, + std::bind(&Transaction::process, this, std::placeholders::_1), + sub_options + ); } void Transaction::onStop() { - subscriber_.shutdown(); + sub_.reset(); } -void Transaction::process(const fuse_msgs::SerializedTransaction::ConstPtr& msg) +void Transaction::process(const fuse_msgs::msg::SerializedTransaction& msg) { // Deserialize and send the transaction to the plugin's parent - sendTransaction(transaction_deserializer_.deserialize(msg).clone()); + sendTransaction(transaction_deserializer_.deserialize(msg)->clone()); } } // namespace fuse_models diff --git a/fuse_models/src/twist_2d.cpp b/fuse_models/src/twist_2d.cpp index 571f97ff1..6e4beb46f 100644 --- a/fuse_models/src/twist_2d.cpp +++ b/fuse_models/src/twist_2d.cpp @@ -37,9 +37,9 @@ #include #include -#include -#include -#include +#include +#include +#include // Register this sensor model with ROS as a plugin. @@ -51,31 +51,52 @@ namespace fuse_models Twist2D::Twist2D() : fuse_core::AsyncSensorModel(1), device_id_(fuse_core::uuid::NIL), - tf_listener_(tf_buffer_), + logger_(rclcpp::get_logger("uninitialized")), throttled_callback_(std::bind(&Twist2D::process, this, std::placeholders::_1)) { } +void Twist2D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + void Twist2D::onInit() { + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + // Read settings from the parameter sever - device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + device_id_ = fuse_variables::loadDeviceId(interfaces_); - params_.loadFromROS(private_node_handle_); + params_.loadFromROS(interfaces_, name_); throttled_callback_.setThrottlePeriod(params_.throttle_period); if (!params_.throttle_use_wall_time) { - throttled_callback_.setClock(node_->get_clock()); + throttled_callback_.setClock(clock_); } if (params_.linear_indices.empty() && params_.angular_indices.empty()) { - RCLCPP_WARN_STREAM(node_->get_logger(), - "No dimensions were specified. Data from topic " << ros::names::resolve(params_.topic) + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << fuse_core::joinTopicName(name_, params_.topic) << " will be ignored."); } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); } void Twist2D::onStart() @@ -83,33 +104,45 @@ void Twist2D::onStart() if (!params_.linear_indices.empty() || !params_.angular_indices.empty()) { - subscriber_ = node_handle_.subscribe( - ros::names::resolve(params_.topic), params_.queue_size, &TwistThrottledCallback::callback, &throttled_callback_, - ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + fuse_core::joinTopicName(name_, params_.topic), + params_.queue_size, + std::bind( + &TwistThrottledCallback::callback< + const geometry_msgs::msg::TwistWithCovarianceStamped &>, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); } } void Twist2D::onStop() { - subscriber_.shutdown(); + sub_.reset(); } -void Twist2D::process(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg) +void Twist2D::process(const geometry_msgs::msg::TwistWithCovarianceStamped& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); - transaction->stamp(msg->header.stamp); + transaction->stamp(msg.header.stamp); common::processTwistWithCovariance( name(), device_id_, - *msg, + msg, params_.linear_loss, params_.angular_loss, params_.target_frame, params_.linear_indices, params_.angular_indices, - tf_buffer_, + *tf_buffer_, !params_.disable_checks, *transaction, params_.tf_timeout); diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 6fb060338..6d78aa991 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -35,10 +35,12 @@ #include #include #include +#include #include #include #include +#include #include #include #include @@ -48,8 +50,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -117,6 +119,7 @@ namespace fuse_models Unicycle2D::Unicycle2D() : fuse_core::AsyncMotionModel(1), + logger_(rclcpp::get_logger("uninitialized")), buffer_length_(rclcpp::Duration::max()), device_id_(fuse_core::uuid::NIL), timestamp_manager_(&Unicycle2D::generateMotionModel, this, rclcpp::Duration::max()) @@ -128,7 +131,7 @@ void Unicycle2D::print(std::ostream& stream) const stream << "state history:\n"; for (const auto& state : state_history_) { - stream << "- stamp: " << state.first << "\n"; + stream << "- stamp: " << state.first.nanoseconds() << "\n"; state.second.print(stream); } } @@ -180,7 +183,7 @@ bool Unicycle2D::applyCallback(fuse_core::Transaction& transaction) } catch (const std::exception& e) { - RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, "An error occurred while completing the motion model query. Error: " << e.what()); return false; } @@ -192,10 +195,23 @@ void Unicycle2D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) updateStateHistoryEstimates(*graph, state_history_, buffer_length_); } +void Unicycle2D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name) +{ + interfaces_ = interfaces; + fuse_core::AsyncMotionModel::initialize(interfaces, name); +} + void Unicycle2D::onInit() { + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + std::string ns = fuse_models::parameters::get_well_formatted_param_namespace_string(name_); + std::vector process_noise_diagonal; - private_node_handle_.param("process_noise_diagonal", process_noise_diagonal, process_noise_diagonal); + process_noise_diagonal = fuse_core::getParam(interfaces_, ns + "process_noise_diagonal", process_noise_diagonal); if (process_noise_diagonal.size() != 8) { @@ -204,23 +220,25 @@ void Unicycle2D::onInit() process_noise_covariance_ = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal(); - private_node_handle_.param("scale_process_noise", scale_process_noise_, scale_process_noise_); - private_node_handle_.param("velocity_norm_min", velocity_norm_min_, velocity_norm_min_); + scale_process_noise_ = fuse_core::getParam(interfaces_, ns + "scale_process_noise", scale_process_noise_); + velocity_norm_min_ = fuse_core::getParam(interfaces_, ns + "velocity_norm_min", velocity_norm_min_); - private_node_handle_.param("disable_checks", disable_checks_, disable_checks_); + disable_checks_ = fuse_core::getParam(interfaces_, ns + "disable_checks", disable_checks_); double buffer_length = 3.0; - private_node_handle_.param("buffer_length", buffer_length, buffer_length); + buffer_length = fuse_core::getParam(interfaces_, ns + "buffer_length", buffer_length); if (buffer_length < 0.0) { - throw std::runtime_error("Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); + throw std::runtime_error( + "Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); } - buffer_length_ = (buffer_length == 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); + buffer_length_ = + (buffer_length == 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); timestamp_manager_.bufferLength(buffer_length_); - device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + device_id_ = fuse_variables::loadDeviceId(interfaces_); } void Unicycle2D::onStart() @@ -245,9 +263,9 @@ void Unicycle2D::generateMotionModel( auto base_state_pair_it = state_history_.upper_bound(beginning_stamp); if (base_state_pair_it == state_history_.begin()) { - RCLCPP_WARN_STREAM_EXPRESSION(node_->get_logger(), !state_history_.empty(), - "UnicycleModel", "Unable to locate a state in this history with stamp <= " - << beginning_stamp << ". Variables will all be initialized to 0."); + RCLCPP_WARN_STREAM_EXPRESSION(logger_, !state_history_.empty(), + "Unable to locate a state in this history with stamp <= " + << beginning_stamp.nanoseconds() << ". Variables will all be initialized to 0."); base_time = beginning_stamp; } else @@ -369,7 +387,7 @@ void Unicycle2D::generateMotionModel( } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, "Invalid '" << name_ << "' motion model: " << ex.what()); return; } @@ -417,11 +435,12 @@ void Unicycle2D::updateStateHistoryEstimates( // Compute the expiration time carefully, as ROS can't handle negative times const auto& ending_stamp = state_history.rbegin()->first; + rclcpp::Time expiration_time; if (ending_stamp.seconds() > buffer_length.seconds()) { - auto expiration_time = ending_stamp - buffer_length; + expiration_time = ending_stamp - buffer_length; } else { // NOTE(CH3): Uninitialized. But okay because it's just used for comparison. - auto expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type); + expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type()); } // Remove state history elements before the expiration time. diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 6f54b4be1..d72bca8e0 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -40,8 +40,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -49,12 +49,12 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include -#include +#include #include @@ -72,37 +72,72 @@ Unicycle2DIgnition::Unicycle2DIgnition() : fuse_core::AsyncSensorModel(1), started_(false), initial_transaction_sent_(false), - device_id_(fuse_core::uuid::NIL) + device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")) { } +void Unicycle2DIgnition::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + void Unicycle2DIgnition::onInit() { + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + // Read settings from the parameter sever - device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + device_id_ = fuse_variables::loadDeviceId(interfaces_); - params_.loadFromROS(private_node_handle_); + params_.loadFromROS(interfaces_, name_); // Connect to the reset service if (!params_.reset_service.empty()) { - reset_client_ = node_handle_.serviceClient(ros::names::resolve(params_.reset_service)); + reset_client_ = rclcpp::create_client( + interfaces_.get_node_base_interface(), + interfaces_.get_node_graph_interface(), + interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.reset_service), + rclcpp::ServicesQoS(), + cb_group_ + ); } // Advertise - subscriber_ = node_handle_.subscribe( - ros::names::resolve(params_.topic), + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + sub_ = rclcpp::create_subscription( + interfaces_, + fuse_core::joinTopicName(name_, params_.topic), params_.queue_size, - &Unicycle2DIgnition::subscriberCallback, - this); - set_pose_service_ = node_handle_.advertiseService( - ros::names::resolve(params_.set_pose_service), - &Unicycle2DIgnition::setPoseServiceCallback, - this); - set_pose_deprecated_service_ = node_handle_.advertiseService( - ros::names::resolve(params_.set_pose_deprecated_service), - &Unicycle2DIgnition::setPoseDeprecatedServiceCallback, - this); + std::bind(&Unicycle2DIgnition::subscriberCallback, this, std::placeholders::_1), + sub_options + ); + + set_pose_service_ = rclcpp::create_service( + interfaces_.get_node_base_interface(), + interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_service), + std::bind( + &Unicycle2DIgnition::setPoseServiceCallback, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS(), + cb_group_ + ); + set_pose_deprecated_service_ = rclcpp::create_service( + interfaces_.get_node_base_interface(), + interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_deprecated_service), + std::bind( + &Unicycle2DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS(), + cb_group_ + ); } void Unicycle2DIgnition::start() @@ -114,8 +149,8 @@ void Unicycle2DIgnition::start() // Send an initial state transaction immediately, if requested if (params_.publish_on_startup && !initial_transaction_sent_) { - auto pose = geometry_msgs::PoseWithCovarianceStamped(); - pose.header.stamp = this->get_node_clock_interface()->now(); + auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); + pose.header.stamp = clock_->now(); pose.pose.pose.position.x = params_.initial_state[0]; pose.pose.pose.position.y = params_.initial_state[1]; pose.pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), params_.initial_state[2])); @@ -132,51 +167,53 @@ void Unicycle2DIgnition::stop() started_ = false; } -void Unicycle2DIgnition::subscriberCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) +void Unicycle2DIgnition::subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { try { - process(*msg); + process(msg); } catch (const std::exception& e) { - RCLCPP_ERROR_STREAM(node_->get_logger(), e.what() << " Ignoring message."); + RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); } } -bool Unicycle2DIgnition::setPoseServiceCallback(fuse_models::SetPose::Request& req, fuse_models::SetPose::Response& res) +bool Unicycle2DIgnition::setPoseServiceCallback( + const fuse_msgs::srv::SetPose::Request::SharedPtr req, + fuse_msgs::srv::SetPose::Response::SharedPtr res) { try { - process(req.pose); - res.success = true; + process(req->pose); + res->success = true; } catch (const std::exception& e) { - res.success = false; - res.message = e.what(); - RCLCPP_ERROR_STREAM(node_->get_logger(), e.what() << " Ignoring request."); + res->success = false; + res->message = e.what(); + RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); } return true; } bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( - fuse_models::SetPoseDeprecated::Request& req, - fuse_models::SetPoseDeprecated::Response&) + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req, + fuse_msgs::srv::SetPoseDeprecated::Response::SharedPtr) { try { - process(req.pose); + process(req->pose); return true; } catch (const std::exception& e) { - RCLCPP_ERROR_STREAM(node_->get_logger(), e.what() << " Ignoring request."); + RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); return false; } } -void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& pose) +void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose) { // Verify we are in the correct state to process set pose requests if (!started_) @@ -227,18 +264,17 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& if (!params_.reset_service.empty()) { // Wait for the reset service - while (!reset_client_.waitForExistence(rclcpp::Duration::from_seconds(10.0)) && ros::ok()) + while (!reset_client_->wait_for_service(std::chrono::seconds(10)) + && interfaces_.get_node_base_interface()->get_context()->is_valid()) { - RCLCPP_WARN_STREAM(node_->get_logger(), - "Waiting for '" << reset_client_.getService() << "' service to become avaiable."); + RCLCPP_WARN_STREAM(logger_, + "Waiting for '" << reset_client_->get_service_name() << "' service to become avaiable."); } - auto srv = std_srvs::Empty(); - if (!reset_client_.call(srv)) - { - // The reset() service failed. Propagate that failure to the caller of this service. - throw std::runtime_error("Failed to call the '" + reset_client_.getService() + "' service."); - } + auto srv = std::make_shared(); + // No need to spin since node is optimizer node, which should be spinning + auto result_future = reset_client_->async_send_request(srv); + result_future.wait(); } // Now that the pose has been validated and the optimizer has been reset, actually send the initial state constraints @@ -246,7 +282,7 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& sendPrior(pose); } -void Unicycle2DIgnition::sendPrior(const geometry_msgs::PoseWithCovarianceStamped& pose) +void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose) { const auto& stamp = pose.header.stamp; @@ -332,8 +368,9 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::PoseWithCovarianceStampe // Send the transaction to the optimizer. sendTransaction(transaction); - RCLCPP_INFO_STREAM(node_->get_logger(), - "Received a set_pose request (stamp: " << stamp << ", x: " << position->x() << ", y: " + RCLCPP_INFO_STREAM(logger_, + "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() + << ", x: " << position->x() << ", y: " << position->y() << ", yaw: " << orientation->yaw() << ")"); } diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp index b69c8c371..b9e45b414 100644 --- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt new file mode 100644 index 000000000..f8e4347f5 --- /dev/null +++ b/fuse_models/test/CMakeLists.txt @@ -0,0 +1,16 @@ +# CORE GTESTS ====================================================================================== +set(TEST_TARGETS + test_unicycle_2d + test_unicycle_2d_predict + test_unicycle_2d_state_cost_function + test_graph_ignition + test_unicycle_2d_ignition +) + +foreach(test_name ${TEST_TARGETS}) + ament_add_gtest("${test_name}" "${test_name}.cpp") + target_link_libraries("${test_name}" ${PROJECT_NAME}) +endforeach() + +ament_add_gmock(test_sensor_proc "test_sensor_proc.cpp") +target_link_libraries(test_sensor_proc ${PROJECT_NAME}) diff --git a/fuse_models/test/example_variable_stamped.h b/fuse_models/test/example_variable_stamped.h index 8ac7a89b3..a1578c532 100644 --- a/fuse_models/test/example_variable_stamped.h +++ b/fuse_models/test/example_variable_stamped.h @@ -81,7 +81,7 @@ class ExampleVariableStamped : public fuse_core::Variable, public fuse_variables { stream << type() << ":\n" << " uuid: " << uuid() << '\n' - << " stamp: " << stamp() << '\n' + << " stamp: " << stamp().nanoseconds() << '\n' << " device_id: " << deviceId() << '\n' << " data: " << data_ << '\n'; } diff --git a/fuse_models/test/graph_ignition.test b/fuse_models/test/graph_ignition.test deleted file mode 100644 index f29a5d6d4..000000000 --- a/fuse_models/test/graph_ignition.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/fuse_models/test/test_graph_ignition.cpp b/fuse_models/test/test_graph_ignition.cpp index 489925991..002d39640 100644 --- a/fuse_models/test/test_graph_ignition.cpp +++ b/fuse_models/test/test_graph_ignition.cpp @@ -37,9 +37,12 @@ #include #include #include -#include +#include #include -#include +#include + +#include +#include #include "example_constraint.h" #include "example_variable.h" @@ -182,11 +185,47 @@ bool operator!=(const fuse_core::Constraint& rhs, const fuse_core::Constraint& l } // namespace fuse_core -TEST(Unicycle2DIgnition, SetGraphService) +class GraphIgnitionTestFixture : public ::testing::Test { - // Set some configuration - ros::param::set("/graph_ignition_test/ignition_sensor/set_graph_service", "/set_graph"); - ros::param::set("/graph_ignition_test/ignition_sensor/reset_service", ""); +public: + GraphIgnitionTestFixture() + { + } + + void SetUp() override + { + rclcpp::init(0, nullptr); + executor_ = std::make_shared(); + spinner_ = std::thread( + [&]() { + executor_->spin(); + }); + } + + void TearDown() override + { + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } + executor_.reset(); + rclcpp::shutdown(); + } + + std::thread spinner_; //!< Internal thread for spinning the executor + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; +}; + +TEST_F(GraphIgnitionTestFixture, SetGraphService) +{ + // Test that the expected PoseStamped message is published + rclcpp::NodeOptions options; + options.arguments({ + "--ros-args", + "-p", "ignition_sensor.set_graph_service:=set_graph", + "-p", "ignition_sensor.reset_service:=''"}); + auto node = rclcpp::Node::make_shared("graph_ignition_test", options); + executor_->add_node(node); // Initialize the callback promise. Promises are single-use. callback_promise = std::promise(); @@ -194,7 +233,7 @@ TEST(Unicycle2DIgnition, SetGraphService) // Create an ignition sensor and register the callback fuse_models::GraphIgnition ignition_sensor; - ignition_sensor.initialize("ignition_sensor", &transactionCallback); + ignition_sensor.initialize(node, "ignition_sensor", &transactionCallback); ignition_sensor.start(); // Create graph @@ -213,23 +252,25 @@ TEST(Unicycle2DIgnition, SetGraphService) graph.addVariable(variable3); auto constraint1 = ExampleConstraint::make_shared( - "test", - std::initializer_list{ variable1->uuid(), variable2->uuid() }); // NOLINT(whitespace/braces) + "test", + std::initializer_list{ variable1->uuid(), variable2->uuid() }); // NOLINT(whitespace/braces) constraint1->data = 1.5; graph.addConstraint(constraint1); auto constraint2 = ExampleConstraint::make_shared( - "test", - std::initializer_list{ variable2->uuid(), variable3->uuid() }); // NOLINT(whitespace/braces) + "test", + std::initializer_list{ variable2->uuid(), variable3->uuid() }); // NOLINT(whitespace/braces) constraint2->data = -3.7; graph.addConstraint(constraint2); // Call the SetGraph service - fuse_models::SetGraph srv; - fuse_core::serializeGraph(graph, srv.request.graph); - const bool success = ros::service::call("/set_graph", srv); - ASSERT_TRUE(success); - EXPECT_TRUE(srv.response.success); + auto srv = std::make_shared(); + fuse_core::serializeGraph(graph, srv->graph); + auto client = node->create_client("graph_ignition_test/set_graph"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto result = client->async_send_request(srv); + ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); + EXPECT_TRUE(result.get()->success); // The ignition sensor should publish a transaction in response to the service call. Wait for the callback to fire. auto status = callback_future.wait_for(std::chrono::seconds(5)); @@ -288,14 +329,19 @@ TEST(Unicycle2DIgnition, SetGraphService) // the transaction stamp, that should also be equal to the requested graph message stamp ASSERT_EQ(1u, boost::size(transaction->involvedStamps())); EXPECT_EQ(transaction->stamp(), transaction->involvedStamps().front()); - EXPECT_EQ(srv.request.graph.header.stamp, transaction->stamp()); + EXPECT_EQ(srv->graph.header.stamp, transaction->stamp()); } -TEST(Unicycle2DIgnition, SetGraphServiceWithStampedVariables) +TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) { // Set some configuration - ros::param::set("/graph_ignition_test/ignition_sensor/set_graph_service", "/set_graph"); - ros::param::set("/graph_ignition_test/ignition_sensor/reset_service", ""); + rclcpp::NodeOptions options; + options.arguments({ + "--ros-args", + "-p", "ignition_sensor.set_graph_service:=set_graph", + "-p", "ignition_sensor.reset_service:=''"}); + auto node = rclcpp::Node::make_shared("graph_ignition_test", options); + executor_->add_node(node); // Initialize the callback promise. Promises are single-use. callback_promise = std::promise(); @@ -303,7 +349,7 @@ TEST(Unicycle2DIgnition, SetGraphServiceWithStampedVariables) // Create an ignition sensor and register the callback fuse_models::GraphIgnition ignition_sensor; - ignition_sensor.initialize("ignition_sensor", &transactionCallback); + ignition_sensor.initialize(node, "ignition_sensor", &transactionCallback); ignition_sensor.start(); // Create graph @@ -322,23 +368,25 @@ TEST(Unicycle2DIgnition, SetGraphServiceWithStampedVariables) graph.addVariable(variable3); auto constraint1 = ExampleConstraint::make_shared( - "test", - std::initializer_list{ variable1->uuid(), variable2->uuid() }); // NOLINT(whitespace/braces) + "test", + std::initializer_list{ variable1->uuid(), variable2->uuid() }); // NOLINT(whitespace/braces) constraint1->data = 1.5; graph.addConstraint(constraint1); auto constraint2 = ExampleConstraint::make_shared( - "test", - std::initializer_list{ variable2->uuid(), variable3->uuid() }); // NOLINT(whitespace/braces) + "test", + std::initializer_list{ variable2->uuid(), variable3->uuid() }); // NOLINT(whitespace/braces) constraint2->data = -3.7; graph.addConstraint(constraint2); // Call the SetGraph service - fuse_models::SetGraph srv; - fuse_core::serializeGraph(graph, srv.request.graph); - const bool success = ros::service::call("/set_graph", srv); - ASSERT_TRUE(success); - EXPECT_TRUE(srv.response.success); + auto srv = std::make_shared(); + fuse_core::serializeGraph(graph, srv->graph); + auto client = node->create_client("graph_ignition_test/set_graph"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto result = client->async_send_request(srv); + ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); + EXPECT_TRUE(result.get()->success); // The ignition sensor should publish a transaction in response to the service call. Wait for the callback to fire. auto status = callback_future.wait_for(std::chrono::seconds(5)); @@ -396,17 +444,5 @@ TEST(Unicycle2DIgnition, SetGraphServiceWithStampedVariables) // Since the variables in the graph have a stamp, the transaction should have one involved stamp per variable, and the // transaction stamp should be equal to the requested graph message stamp ASSERT_EQ(boost::size(graph.getVariables()), boost::size(transaction->involvedStamps())); - EXPECT_EQ(srv.request.graph.header.stamp, transaction->stamp()); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "graph_ignition_test"); - auto spinner = ros::AsyncSpinner(1); - spinner.start(); - int ret = RUN_ALL_TESTS(); - spinner.stop(); - ros::shutdown(); - return ret; + EXPECT_EQ(srv->graph.header.stamp, transaction->stamp()); } diff --git a/fuse_models/test/test_sensor_proc.cpp b/fuse_models/test/test_sensor_proc.cpp index 80f915aa5..6b18fac95 100644 --- a/fuse_models/test/test_sensor_proc.cpp +++ b/fuse_models/test/test_sensor_proc.cpp @@ -238,9 +238,3 @@ TEST(TestSuite, populatePartialMeasurementEmptyPositionEmptyOrientation) EXPECT_EQ(0, pose_mean_partial.size()); EXPECT_EQ(0, pose_covariance_partial.size()); } - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_models/test/test_unicycle_2d.cpp b/fuse_models/test/test_unicycle_2d.cpp index 11295c096..8cb1a6c41 100644 --- a/fuse_models/test/test_unicycle_2d.cpp +++ b/fuse_models/test/test_unicycle_2d.cpp @@ -290,9 +290,3 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); } } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_models/test/test_unicycle_2d_ignition.cpp b/fuse_models/test/test_unicycle_2d_ignition.cpp index d3e0f5639..4470b183e 100644 --- a/fuse_models/test/test_unicycle_2d_ignition.cpp +++ b/fuse_models/test/test_unicycle_2d_ignition.cpp @@ -36,9 +36,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include @@ -87,13 +87,49 @@ const Derived* getConstraint(const fuse_core::Transaction& transaction) } -TEST(Unicycle2DIgnition, InitialTransaction) +class Unicycle2DIgnitionTestFixture : public ::testing::Test +{ +public: + Unicycle2DIgnitionTestFixture() + { + } + + void SetUp() override + { + rclcpp::init(0, nullptr); + executor_ = std::make_shared(); + spinner_ = std::thread( + [&]() { + executor_->spin(); + }); + } + + void TearDown() override + { + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } + executor_.reset(); + rclcpp::shutdown(); + } + + std::thread spinner_; //!< Internal thread for spinning the executor + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; +}; + +TEST_F(Unicycle2DIgnitionTestFixture, InitialTransaction) { // Set some configuration - auto initial_state = std::vector{0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8}; - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/initial_state", initial_state); - auto initial_sigma = std::vector{1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/initial_sigma", initial_sigma); + rclcpp::NodeOptions options; + options.arguments({ + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]"}); + auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); + executor_->add_node(node); // Initialize the callback promise. Promises are single-use. callback_promise = std::promise(); @@ -101,7 +137,7 @@ TEST(Unicycle2DIgnition, InitialTransaction) // Create an ignition sensor and register the callback fuse_models::Unicycle2DIgnition ignition_sensor; - ignition_sensor.initialize("ignition_sensor", &transactionCallback); + ignition_sensor.initialize(node, "ignition_sensor", &transactionCallback); ignition_sensor.start(); // The ignition sensor should publish a transaction immediately. Wait for the callback to fire. @@ -162,14 +198,19 @@ TEST(Unicycle2DIgnition, InitialTransaction) } } -TEST(Unicycle2DIgnition, SkipInitialTransaction) +TEST_F(Unicycle2DIgnitionTestFixture, SkipInitialTransaction) { // Set some configuration - auto initial_state = std::vector{0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8}; - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/initial_state", initial_state); - auto initial_sigma = std::vector{1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/initial_sigma", initial_sigma); - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/publish_on_startup", false); + rclcpp::NodeOptions options; + options.arguments({ + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", + "-p", "ignition_sensor.publish_on_startup:=false"}); + auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); + executor_->add_node(node); // Initialize the callback promise. Promises are single-use. callback_promise = std::promise(); @@ -177,7 +218,7 @@ TEST(Unicycle2DIgnition, SkipInitialTransaction) // Create an ignition sensor and register the callback fuse_models::Unicycle2DIgnition ignition_sensor; - ignition_sensor.initialize("ignition_sensor", &transactionCallback); + ignition_sensor.initialize(node, "ignition_sensor", &transactionCallback); ignition_sensor.start(); // The ignition sensor should publish a transaction immediately. Wait for the callback to fire. @@ -185,16 +226,21 @@ TEST(Unicycle2DIgnition, SkipInitialTransaction) ASSERT_FALSE(status == std::future_status::ready); } -TEST(Unicycle2DIgnition, SetPoseService) +TEST_F(Unicycle2DIgnitionTestFixture, SetPoseService) { // Set some configuration - auto initial_state = std::vector{0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8}; - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/initial_state", initial_state); - auto initial_sigma = std::vector{1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/initial_sigma", initial_sigma); - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/set_pose_service", "/set_pose"); - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/reset_service", ""); - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/publish_on_startup", false); + rclcpp::NodeOptions options; + options.arguments({ + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", + "-p", "ignition_sensor.set_pose_service:=set_pose", + "-p", "ignition_sensor.reset_service:=''", + "-p", "ignition_sensor.publish_on_startup:=false"}); + auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); + executor_->add_node(node); // Initialize the callback promise. Promises are single-use. callback_promise = std::promise(); @@ -202,22 +248,24 @@ TEST(Unicycle2DIgnition, SetPoseService) // Create an ignition sensor and register the callback fuse_models::Unicycle2DIgnition ignition_sensor; - ignition_sensor.initialize("ignition_sensor", &transactionCallback); + ignition_sensor.initialize(node, "ignition_sensor", &transactionCallback); ignition_sensor.start(); // Call the SetPose service - fuse_models::SetPose srv; - srv.request.pose.header.stamp = rclcpp::Time(12, 345678910); - srv.request.pose.pose.pose.position.x = 1.0; - srv.request.pose.pose.pose.position.y = 2.0; - srv.request.pose.pose.pose.orientation.z = 0.0499792; // yaw = 0.1rad - srv.request.pose.pose.pose.orientation.w = 0.9987503; - srv.request.pose.pose.covariance[0] = 1.0; - srv.request.pose.pose.covariance[7] = 2.0; - srv.request.pose.pose.covariance[35] = 3.0; - bool success = ros::service::call("/set_pose", srv); - ASSERT_TRUE(success); - EXPECT_TRUE(srv.response.success); + auto srv = std::make_shared(); + srv->pose.header.stamp = rclcpp::Time(12, 345678910); + srv->pose.pose.pose.position.x = 1.0; + srv->pose.pose.pose.position.y = 2.0; + srv->pose.pose.pose.orientation.z = 0.0499792; // yaw = 0.1rad + srv->pose.pose.pose.orientation.w = 0.9987503; + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 2.0; + srv->pose.pose.covariance[35] = 3.0; + auto client = node->create_client("unicycle_2d_ignition_test/set_pose"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto result = client->async_send_request(srv); + ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); + EXPECT_TRUE(result.get()->success); // The ignition sensor should publish a transaction in response to the service call. Wait for the callback to fire. auto status = callback_future.wait_for(std::chrono::seconds(5)); @@ -277,16 +325,21 @@ TEST(Unicycle2DIgnition, SetPoseService) } } -TEST(Unicycle2DIgnition, SetPoseDeprecatedService) +TEST_F(Unicycle2DIgnitionTestFixture, SetPoseDeprecatedService) { // Set some configuration - auto initial_state = std::vector{0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8}; - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/initial_state", initial_state); - auto initial_sigma = std::vector{1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/initial_sigma", initial_sigma); - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/set_pose_deprecated_service", "/set_pose_deprecated"); - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/reset_service", ""); - ros::param::set("/unicycle_2d_ignition_test/ignition_sensor/publish_on_startup", false); + rclcpp::NodeOptions options; + options.arguments({ + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", + "-p", "ignition_sensor.set_pose_deprecated_service:=set_pose_deprecated", + "-p", "ignition_sensor.reset_service:=''", + "-p", "ignition_sensor.publish_on_startup:=false"}); + auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); + executor_->add_node(node); // Initialize the callback promise. Promises are single-use. callback_promise = std::promise(); @@ -294,21 +347,23 @@ TEST(Unicycle2DIgnition, SetPoseDeprecatedService) // Create an ignition sensor and register the callback fuse_models::Unicycle2DIgnition ignition_sensor; - ignition_sensor.initialize("ignition_sensor", &transactionCallback); + ignition_sensor.initialize(node, "ignition_sensor", &transactionCallback); ignition_sensor.start(); // Call the SetPose service - fuse_models::SetPoseDeprecated srv; - srv.request.pose.header.stamp = rclcpp::Time(12, 345678910); - srv.request.pose.pose.pose.position.x = 1.0; - srv.request.pose.pose.pose.position.y = 2.0; - srv.request.pose.pose.pose.orientation.z = 0.0499792; // yaw = 0.1rad - srv.request.pose.pose.pose.orientation.w = 0.9987503; - srv.request.pose.pose.covariance[0] = 1.0; - srv.request.pose.pose.covariance[7] = 2.0; - srv.request.pose.pose.covariance[35] = 3.0; - bool success = ros::service::call("/set_pose_deprecated", srv); - ASSERT_TRUE(success); + auto srv = std::make_shared(); + srv->pose.header.stamp = rclcpp::Time(12, 345678910); + srv->pose.pose.pose.position.x = 1.0; + srv->pose.pose.pose.position.y = 2.0; + srv->pose.pose.pose.orientation.z = 0.0499792; // yaw = 0.1rad + srv->pose.pose.pose.orientation.w = 0.9987503; + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 2.0; + srv->pose.pose.covariance[35] = 3.0; + auto client = node->create_client("unicycle_2d_ignition_test/set_pose_deprecated"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto result = client->async_send_request(srv); + ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); // The ignition sensor should publish a transaction in response to the service call. Wait for the callback to fire. auto status = callback_future.wait_for(std::chrono::seconds(5)); @@ -367,15 +422,3 @@ TEST(Unicycle2DIgnition, SetPoseDeprecatedService) EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); } } - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "unicycle_2d_ignition_test"); - auto spinner = ros::AsyncSpinner(1); - spinner.start(); - int ret = RUN_ALL_TESTS(); - spinner.stop(); - ros::shutdown(); - return ret; -} diff --git a/fuse_models/test/test_unicycle_2d_predict.cpp b/fuse_models/test/test_unicycle_2d_predict.cpp index a8e8f7f34..f5c32c395 100644 --- a/fuse_models/test/test_unicycle_2d_predict.cpp +++ b/fuse_models/test/test_unicycle_2d_predict.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include @@ -450,9 +450,3 @@ TEST(Predict, predictJacobians) << "Autodiff Jacobian =\n" << J_autodiff.format(HeavyFmt) << "\nAnalytic Jacobian =\n" << J_analytic.format(HeavyFmt); } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp index 79c46883e..9f86a3840 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include @@ -99,7 +100,13 @@ TEST(CostFunction, evaluateCostFunction) // Check jacobians are correct using a gradient checker ceres::NumericDiffOptions numeric_diff_options; - ceres::GradientChecker gradient_checker(&cost_function, NULL, numeric_diff_options); + +#if CERES_VERSION_AT_LEAST(2, 1, 0) + std::vector parameterizations; + ceres::GradientChecker gradient_checker(&cost_function, ¶meterizations, numeric_diff_options); +#else + ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); +#endif // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error is 5.26356e-10 ceres::GradientChecker::ProbeResults probe_results; @@ -133,9 +140,3 @@ TEST(CostFunction, evaluateCostFunction) << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); } } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_models/test/unicycle_2d_ignition.test b/fuse_models/test/unicycle_2d_ignition.test deleted file mode 100644 index d22513bfb..000000000 --- a/fuse_models/test/unicycle_2d_ignition.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/fuse_msgs/CMakeLists.txt b/fuse_msgs/CMakeLists.txt index d189b9a63..392e9f73b 100644 --- a/fuse_msgs/CMakeLists.txt +++ b/fuse_msgs/CMakeLists.txt @@ -11,15 +11,22 @@ endif() find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) set(msg_files "msg/SerializedGraph.msg" "msg/SerializedTransaction.msg" ) -rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} +set(srv_files + "srv/SetGraph.srv" + "srv/SetPose.srv" + "srv/SetPoseDeprecated.srv" +) + +rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} DEPENDENCIES - std_msgs + std_msgs geometry_msgs ADD_LINTER_TESTS ) diff --git a/fuse_msgs/package.xml b/fuse_msgs/package.xml index 3121a2d4e..090f84828 100644 --- a/fuse_msgs/package.xml +++ b/fuse_msgs/package.xml @@ -16,6 +16,7 @@ rosidl_default_generators std_msgs + geometry_msgs rosidl_default_runtime diff --git a/fuse_models/srv/SetGraph.srv b/fuse_msgs/srv/SetGraph.srv similarity index 100% rename from fuse_models/srv/SetGraph.srv rename to fuse_msgs/srv/SetGraph.srv diff --git a/fuse_models/srv/SetPose.srv b/fuse_msgs/srv/SetPose.srv similarity index 100% rename from fuse_models/srv/SetPose.srv rename to fuse_msgs/srv/SetPose.srv diff --git a/fuse_models/srv/SetPoseDeprecated.srv b/fuse_msgs/srv/SetPoseDeprecated.srv similarity index 100% rename from fuse_models/srv/SetPoseDeprecated.srv rename to fuse_msgs/srv/SetPoseDeprecated.srv diff --git a/fuse_tutorials/fuse_plugins.xml b/fuse_tutorials/fuse_plugins.xml index 5e39d6646..bff55316d 100644 --- a/fuse_tutorials/fuse_plugins.xml +++ b/fuse_tutorials/fuse_plugins.xml @@ -1,4 +1,4 @@ - + Implements a range-only measurement constraint between the robot and a landmark. The main purpose is to diff --git a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.h b/fuse_tutorials/include/fuse_tutorials/range_sensor_model.h index b355f6095..aa2945845 100644 --- a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.h +++ b/fuse_tutorials/include/fuse_tutorials/range_sensor_model.h @@ -169,9 +169,9 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel }; std::unordered_map beacon_db_; //!< The estimated position of each beacon - ros::Subscriber beacon_subscriber_; //!< ROS subscriber for the database of prior beacon positions + ros::Subscriber beacon_sub_; //!< ROS subscription for the database of prior beacon positions bool initialized_ { false }; //!< Flag indicating the initial beacon positions have been processed - ros::Subscriber subscriber_; //!< ROS subscriber for the range sensor measurements + ros::Subscriber sub_; //!< ROS subscription for the range sensor measurements }; } // namespace fuse_tutorials diff --git a/fuse_tutorials/src/beacon_publisher.cpp b/fuse_tutorials/src/beacon_publisher.cpp index 998642e8d..40fb08ba1 100644 --- a/fuse_tutorials/src/beacon_publisher.cpp +++ b/fuse_tutorials/src/beacon_publisher.cpp @@ -108,7 +108,7 @@ void BeaconPublisher::notifyCallback( } // Publish the pointcloud - beacon_publisher_.publish(msg); + beacon_publisher_->publish(msg); } } // namespace fuse_tutorials diff --git a/fuse_tutorials/src/range_sensor_model.cpp b/fuse_tutorials/src/range_sensor_model.cpp index 001b00daa..9cbe4510a 100644 --- a/fuse_tutorials/src/range_sensor_model.cpp +++ b/fuse_tutorials/src/range_sensor_model.cpp @@ -61,7 +61,7 @@ void RangeSensorModel::priorBeaconsCallback(const sensor_msgs::PointCloud2::Cons { beacon_db_[*id_it] = Beacon { *x_it, *y_it, *sigma_it }; } - RCLCPP_INFO_STREAM(node_->get_logger(), "Updated Beacon Database."); + RCLCPP_INFO_STREAM(logger_, "Updated Beacon Database."); } void RangeSensorModel::onInit() @@ -69,7 +69,7 @@ void RangeSensorModel::onInit() // Read settings from the parameter server, or any other one-time operations. This sensor model doesn't have any // user configuration to read. But we do need a copy of the beacon database. We will subscribe to that now, as it // is assumed to be constant -- no need to clear it if the optimizer is reset. - beacon_subscriber_ = node_handle_.subscribe("prior_beacons", 10, &RangeSensorModel::priorBeaconsCallback, this); + beacon_sub_ = node_handle_.subscribe("prior_beacons", 10, &RangeSensorModel::priorBeaconsCallback, this); } void RangeSensorModel::onStart() @@ -82,7 +82,7 @@ void RangeSensorModel::onStart() // Subscribe to the ranges topic. Any received messages will be processed within the message callback function, // and the created constraints will be sent to the optimizer. By subscribing to the topic in onStart() and // unsubscribing in onStop(), we will only send transactions to the optimizer while it is running. - subscriber_ = node_handle_.subscribe("ranges", 10, &RangeSensorModel::rangesCallback, this); + sub_ = node_handle_.subscribe("ranges", 10, &RangeSensorModel::rangesCallback, this); } void RangeSensorModel::onStop() @@ -90,7 +90,7 @@ void RangeSensorModel::onStop() // Unsubscribe from the ranges topic. Since the sensor constraints are created and sent from the subscriber callback, // shutting down the subscriber effectively stops the creation of new constraints from this sensor model. This // ensures we only send transactions to the optimizer while it is running. - subscriber_.shutdown(); + sub_.reset(); } void RangeSensorModel::rangesCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) diff --git a/fuse_tutorials/src/range_sensor_simulator.cpp b/fuse_tutorials/src/range_sensor_simulator.cpp index c640a09ee..ce70e79d2 100644 --- a/fuse_tutorials/src/range_sensor_simulator.cpp +++ b/fuse_tutorials/src/range_sensor_simulator.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include #include #include #include @@ -197,7 +197,7 @@ void initializeStateEstimation( const Robot& state, const rclcpp::Clock& clock) { // Send the initial localization signal to the state estimator - auto srv = fuse_models::SetPose(); + auto srv = fuse_msgs::srv::SetPose(); srv.request.pose.header.frame_id = MAP_FRAME; srv.request.pose.pose.pose.position.x = state.x; srv.request.pose.pose.pose.position.y = state.y; @@ -355,11 +355,11 @@ int main(int argc, char **argv) // Create the true set of range beacons auto beacons = createBeacons(); - true_beacons_publisher.publish(beaconsToPointcloud(beacons), *node->get_clock()); + true_beacons_publisher->publish(beaconsToPointcloud(beacons), *node->get_clock()); // Publish a set of noisy beacon locations to act as the known priors auto noisy_beacons = createNoisyBeacons(beacons); - prior_beacons_publisher.publish(beaconsToPointcloud(noisy_beacons, *node->get_clock()); + prior_beacons_publisher->publish(beaconsToPointcloud(noisy_beacons, *node->get_clock()); // Initialize the robot state auto state = Robot(); @@ -381,11 +381,11 @@ int main(int argc, char **argv) // Simulate the robot motion auto new_state = simulateRobotMotion(state, node->now()); // Publish the new ground truth - ground_truth_publisher.publish(robotToOdometry(new_state)); + ground_truth_publisher->publish(robotToOdometry(new_state)); // Generate and publish simulated measurements from the new robot state - imu_publisher.publish(simulateImu(new_state)); - wheel_odom_publisher.publish(simulateWheelOdometry(new_state)); - range_publisher.publish(simulateRangeSensor(new_state, beacons)); + imu_publisher->publish(simulateImu(new_state)); + wheel_odom_publisher->publish(simulateWheelOdometry(new_state)); + range_publisher->publish(simulateRangeSensor(new_state, beacons)); // Wait for the next time step state = new_state; rate.sleep();