From a749d4202da0ce2479b464d8f9aa3617af1ddd97 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 28 Dec 2022 17:34:36 -0800 Subject: [PATCH 1/4] Port messages Signed-off-by: methylDragon --- fuse_models/fuse_plugins.xml | 2 +- .../include/fuse_models/acceleration_2d.h | 10 +- .../include/fuse_models/common/sensor_proc.h | 38 +++---- .../include/fuse_models/graph_ignition.h | 6 +- fuse_models/include/fuse_models/imu_2d.h | 18 ++-- fuse_models/include/fuse_models/odometry_2d.h | 18 ++-- .../fuse_models/odometry_2d_publisher.h | 18 ++-- .../parameters/acceleration_2d_params.h | 2 +- .../fuse_models/parameters/imu_2d_params.h | 2 +- .../parameters/odometry_2d_params.h | 2 +- .../fuse_models/parameters/pose_2d_params.h | 2 +- .../fuse_models/parameters/twist_2d_params.h | 2 +- fuse_models/include/fuse_models/pose_2d.h | 17 +-- fuse_models/include/fuse_models/transaction.h | 8 +- fuse_models/include/fuse_models/twist_2d.h | 10 +- .../fuse_models/unicycle_2d_ignition.h | 8 +- fuse_models/src/acceleration_2d.cpp | 10 +- fuse_models/src/graph_ignition.cpp | 12 +-- fuse_models/src/imu_2d.cpp | 100 +++++++++--------- fuse_models/src/odometry_2d.cpp | 34 +++--- fuse_models/src/odometry_2d_publisher.cpp | 30 +++--- fuse_models/src/pose_2d.cpp | 16 +-- fuse_models/src/transaction.cpp | 2 +- fuse_models/src/twist_2d.cpp | 10 +- fuse_models/src/unicycle_2d_ignition.cpp | 20 ++-- 25 files changed, 200 insertions(+), 197 deletions(-) diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index 6a6da558a..493eb9c26 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -14,7 +14,7 @@ - Publisher plugin that publishes a nav_msgs::Odometry message and broadcasts a tf transform for optimized 2D + Publisher plugin that publishes a nav_msgs::msg::Odometry message and broadcasts a tf transform for optimized 2D state data (combination of Position2DStamped, Orientation2DStamped, VelocityLinear2DStamped, and VelocityAngular2DStamped). diff --git a/fuse_models/include/fuse_models/acceleration_2d.h b/fuse_models/include/fuse_models/acceleration_2d.h index 50ab6aff7..29a09f694 100644 --- a/fuse_models/include/fuse_models/acceleration_2d.h +++ b/fuse_models/include/fuse_models/acceleration_2d.h @@ -40,7 +40,7 @@ #include #include -#include +#include #include #include @@ -52,7 +52,7 @@ namespace fuse_models * @brief An adapter-type sensor that produces 2D linear acceleration constraints from information published by another * node * - * This sensor subscribes to a geometry_msgs::AccelWithCovarianceStamped topic and converts each received message + * This sensor subscribes to a geometry_msgs::msg::AccelWithCovarianceStamped topic and converts each received message * into a 2D linear acceleration variable and constraint. * * Parameters: @@ -63,7 +63,7 @@ namespace fuse_models * - topic (string) The topic to which to subscribe for the twist messages * * Subscribes: - * - \p topic (geometry_msgs::AccelWithCovarianceStamped) Acceleration information at a given timestamp + * - \p topic (geometry_msgs::msg::AccelWithCovarianceStamped) Acceleration information at a given timestamp */ class Acceleration2D : public fuse_core::AsyncSensorModel { @@ -85,7 +85,7 @@ class Acceleration2D : public fuse_core::AsyncSensorModel * @brief Callback for acceleration messages * @param[in] msg - The acceleration message to process */ - void process(const geometry_msgs::AccelWithCovarianceStamped::ConstPtr& msg); + void process(const geometry_msgs::msg::AccelWithCovarianceStamped& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -117,7 +117,7 @@ class Acceleration2D : public fuse_core::AsyncSensorModel ros::Subscriber subscriber_; - using AccelerationThrottledCallback = fuse_core::ThrottledMessageCallback; + using AccelerationThrottledCallback = fuse_core::ThrottledMessageCallback; AccelerationThrottledCallback throttled_callback_; }; diff --git a/fuse_models/include/fuse_models/common/sensor_proc.h b/fuse_models/include/fuse_models/common/sensor_proc.h index 465d305e9..fe0fa0736 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.h +++ b/fuse_models/include/fuse_models/common/sensor_proc.h @@ -48,15 +48,15 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include #include -#include +#include #include #include #include @@ -81,7 +81,7 @@ namespace tf2 */ template <> inline -void doTransform(const geometry_msgs::TwistWithCovarianceStamped& t_in, geometry_msgs::TwistWithCovarianceStamped& t_out, const geometry_msgs::TransformStamped& transform) // NOLINT +void doTransform(const geometry_msgs::msg::TwistWithCovarianceStamped& t_in, geometry_msgs::msg::TwistWithCovarianceStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) // NOLINT { tf2::Vector3 vl; fromMsg(t_in.twist.twist.linear, vl); @@ -106,7 +106,7 @@ void doTransform(const geometry_msgs::TwistWithCovarianceStamped& t_in, geometry */ template <> inline -void doTransform(const geometry_msgs::AccelWithCovarianceStamped& t_in, geometry_msgs::AccelWithCovarianceStamped& t_out, const geometry_msgs::TransformStamped& transform) // NOLINT +void doTransform(const geometry_msgs::msg::AccelWithCovarianceStamped& t_in, geometry_msgs::msg::AccelWithCovarianceStamped& t_out, const geometry_msgs::msg::TransformStamped& transform) // NOLINT { tf2::Vector3 al; fromMsg(t_in.accel.accel.linear, al); @@ -231,7 +231,7 @@ bool transformMessage( { try { - auto trans = geometry_msgs::TransformStamped(); + auto trans = geometry_msgs::msg::TransformStamped(); if (tf_timeout.isZero()) { trans = tf_buffer.lookupTransform(output.header.frame_id, input.header.frame_id, input.header.stamp); @@ -273,7 +273,7 @@ bool transformMessage( inline bool processAbsolutePoseWithCovariance( const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose, const fuse_core::Loss::SharedPtr& loss, const std::string& target_frame, const std::vector& position_indices, @@ -288,7 +288,7 @@ inline bool processAbsolutePoseWithCovariance( return false; } - geometry_msgs::PoseWithCovarianceStamped transformed_message; + geometry_msgs::msg::PoseWithCovarianceStamped transformed_message; if (target_frame.empty()) { transformed_message = pose; @@ -407,8 +407,8 @@ inline bool processAbsolutePoseWithCovariance( inline bool processDifferentialPoseWithCovariance( const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::PoseWithCovarianceStamped& pose1, - const geometry_msgs::PoseWithCovarianceStamped& pose2, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose1, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose2, const bool independent, const fuse_core::Matrix3d& minimum_pose_relative_covariance, const fuse_core::Loss::SharedPtr& loss, @@ -755,9 +755,9 @@ inline bool processDifferentialPoseWithCovariance( inline bool processDifferentialPoseWithTwistCovariance( const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::PoseWithCovarianceStamped& pose1, - const geometry_msgs::PoseWithCovarianceStamped& pose2, - const geometry_msgs::TwistWithCovarianceStamped& twist, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose1, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose2, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const fuse_core::Matrix3d& minimum_pose_relative_covariance, const fuse_core::Matrix3d& twist_covariance_offset, const fuse_core::Loss::SharedPtr& loss, @@ -936,7 +936,7 @@ inline bool processDifferentialPoseWithTwistCovariance( inline bool processTwistWithCovariance( const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::TwistWithCovarianceStamped& twist, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const fuse_core::Loss::SharedPtr& linear_velocity_loss, const fuse_core::Loss::SharedPtr& angular_velocity_loss, const std::string& target_frame, @@ -953,7 +953,7 @@ inline bool processTwistWithCovariance( return false; } - geometry_msgs::TwistWithCovarianceStamped transformed_message; + geometry_msgs::msg::TwistWithCovarianceStamped transformed_message; if (target_frame.empty()) { transformed_message = twist; @@ -1105,7 +1105,7 @@ inline bool processTwistWithCovariance( inline bool processAccelWithCovariance( const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::AccelWithCovarianceStamped& acceleration, + const geometry_msgs::msg::AccelWithCovarianceStamped& acceleration, const fuse_core::Loss::SharedPtr& loss, const std::string& target_frame, const std::vector& indices, @@ -1120,7 +1120,7 @@ inline bool processAccelWithCovariance( return false; } - geometry_msgs::AccelWithCovarianceStamped transformed_message; + geometry_msgs::msg::AccelWithCovarianceStamped transformed_message; if (target_frame.empty()) { transformed_message = acceleration; diff --git a/fuse_models/include/fuse_models/graph_ignition.h b/fuse_models/include/fuse_models/graph_ignition.h index ee6137bd4..13847fd6f 100644 --- a/fuse_models/include/fuse_models/graph_ignition.h +++ b/fuse_models/include/fuse_models/graph_ignition.h @@ -42,7 +42,7 @@ #include #include -#include +#include #include #include @@ -110,7 +110,7 @@ class GraphIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new transaction equivalent to the supplied graph */ - void subscriberCallback(const fuse_msgs::SerializedGraph::ConstPtr& msg); + void subscriberCallback(const fuse_msgs::msg::SerializedGraph& msg); /** * @brief Triggers the publication of a new transaction equivalent to the supplied graph @@ -130,7 +130,7 @@ class GraphIgnition : public fuse_core::AsyncSensorModel * * @param[in] msg - The graph message */ - void process(const fuse_msgs::SerializedGraph& msg); + void process(const fuse_msgs::msg::SerializedGraph& msg); /** * @brief Create and send a transaction equivalent to the supplied graph diff --git a/fuse_models/include/fuse_models/imu_2d.h b/fuse_models/include/fuse_models/imu_2d.h index ad8425301..3eaf17007 100644 --- a/fuse_models/include/fuse_models/imu_2d.h +++ b/fuse_models/include/fuse_models/imu_2d.h @@ -40,9 +40,9 @@ #include #include -#include +#include #include -#include +#include #include #include @@ -56,7 +56,7 @@ namespace fuse_models * @brief An adapter-type sensor that produces orientation (relative or absolute), angular velocity, and linear * acceleration constraints from IMU sensor data published by another node * - * This sensor subscribes to a sensor_msgs::Imu topic and: + * This sensor subscribes to a sensor_msgs::msg::Imu topic and: * 1. Creates relative or absolute orientation and constraints. If the \p differential parameter is set to false (the * default), the orientation measurement will be treated as an absolute constraint. If it is set to true, * consecutive measurements will be used to generate relative orientation constraints. @@ -81,7 +81,7 @@ namespace fuse_models * - acceleration_target_frame (string) Acceleration data will be transformed into this frame before it is fused. * * Subscribes: - * - \p topic (sensor_msgs::Imu) IMU data at a given timestep + * - \p topic (sensor_msgs::msg::Imu) IMU data at a given timestep */ class Imu2D : public fuse_core::AsyncSensorModel { @@ -103,7 +103,7 @@ class Imu2D : public fuse_core::AsyncSensorModel * @brief Callback for pose messages * @param[in] msg - The IMU message to process */ - void process(const sensor_msgs::Imu::ConstPtr& msg); + void process(const sensor_msgs::msg::Imu& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -135,13 +135,13 @@ class Imu2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential(const geometry_msgs::PoseWithCovarianceStamped& pose, - const geometry_msgs::TwistWithCovarianceStamped& twist, const bool validate, + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, fuse_core::Transaction& transaction); ParameterType params_; - std::unique_ptr previous_pose_; + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_; tf2_ros::Buffer tf_buffer_; @@ -149,7 +149,7 @@ class Imu2D : public fuse_core::AsyncSensorModel ros::Subscriber subscriber_; - using ImuThrottledCallback = fuse_core::ThrottledMessageCallback; + using ImuThrottledCallback = fuse_core::ThrottledMessageCallback; ImuThrottledCallback throttled_callback_; }; diff --git a/fuse_models/include/fuse_models/odometry_2d.h b/fuse_models/include/fuse_models/odometry_2d.h index fb05f39ab..7fc9cc251 100644 --- a/fuse_models/include/fuse_models/odometry_2d.h +++ b/fuse_models/include/fuse_models/odometry_2d.h @@ -40,8 +40,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -55,7 +55,7 @@ namespace fuse_models * @brief An adapter-type sensor that produces pose (relative or absolute) and velocity constraints from sensor data * published by another node * - * This sensor subscribes to a nav_msgs::Odometry topic and: + * This sensor subscribes to a nav_msgs::msg::Odometry topic and: * 1. Creates relative or absolute pose variables and constraints. If the \p differential parameter is set to false * (the default), the measurement will be treated as an absolute constraint. If it is set to true, consecutive * measurements will be used to generate relative pose constraints. @@ -77,7 +77,7 @@ namespace fuse_models * frame should be a body-relative frame, typically 'base_link'. * * Subscribes: - * - \p topic (nav_msgs::Odometry) Odometry information at a given timestep + * - \p topic (nav_msgs::msg::Odometry) Odometry information at a given timestep */ class Odometry2D : public fuse_core::AsyncSensorModel { @@ -99,7 +99,7 @@ class Odometry2D : public fuse_core::AsyncSensorModel * @brief Callback for pose messages * @param[in] msg - The pose message to process */ - void process(const nav_msgs::Odometry::ConstPtr& msg); + void process(const nav_msgs::msg::Odometry& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -131,13 +131,13 @@ class Odometry2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential(const geometry_msgs::PoseWithCovarianceStamped& pose, - const geometry_msgs::TwistWithCovarianceStamped& twist, const bool validate, + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, fuse_core::Transaction& transaction); ParameterType params_; - std::unique_ptr previous_pose_; + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_; tf2_ros::Buffer tf_buffer_; @@ -145,7 +145,7 @@ class Odometry2D : public fuse_core::AsyncSensorModel ros::Subscriber subscriber_; - using OdometryThrottledCallback = fuse_core::ThrottledMessageCallback; + using OdometryThrottledCallback = fuse_core::ThrottledMessageCallback; OdometryThrottledCallback throttled_callback_; }; diff --git a/fuse_models/include/fuse_models/odometry_2d_publisher.h b/fuse_models/include/fuse_models/odometry_2d_publisher.h index 7e34dfb86..71a642816 100644 --- a/fuse_models/include/fuse_models/odometry_2d_publisher.h +++ b/fuse_models/include/fuse_models/odometry_2d_publisher.h @@ -44,8 +44,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -59,7 +59,7 @@ namespace fuse_models { /** - * @class Odometry2DPublisher plugin that publishes a nav_msgs::Odometry message and broadcasts a tf transform for optimized 2D + * @class Odometry2DPublisher plugin that publishes a nav_msgs::msg::Odometry message and broadcasts a tf transform for optimized 2D * state data (combination of Position2DStamped, Orientation2DStamped, VelocityLinear2DStamped, and * VelocityAngular2DStamped, AccelerationLinear2DStamped). * @@ -86,11 +86,11 @@ namespace fuse_models * - topic (string, default: "~odometry/filtered") The ROS topic to which we will publish the filtered state data * * Publishes: - * - odometry/filtered (nav_msgs::Odometry) The most recent optimized state, gives as an odometry message + * - odometry/filtered (nav_msgs::msg::Odometry) The most recent optimized state, gives as an odometry message * - tf (via a tf2_ros::TransformBroadcaster) The most recent optimized state, as a tf transform * * Subscribes: - * - tf, tf_static (tf2_msgs::TFMessage) Subscribes to tf data to obtain the requisite odom->base_link transform, + * - tf, tf_static (tf2_msgs::msg::TFMessage) Subscribes to tf data to obtain the requisite odom->base_link transform, * but only if the world_frame_id is set to the value of the map_frame_id. */ class Odometry2DPublisher : public fuse_core::AsyncPublisher // TODO(methylDragon): Refactor this in the same way it was done in fuse_publishers @@ -160,8 +160,8 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher // TODO(methylDrago 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); /** * @brief Timer callback method for the filtered state publication and tf broadcasting @@ -188,9 +188,9 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher // TODO(methylDrago bool latest_covariance_valid_{ false }; //!< Whether the latest covariance computed is valid or not - nav_msgs::Odometry odom_output_; + nav_msgs::msg::Odometry odom_output_; - geometry_msgs::AccelWithCovarianceStamped acceleration_output_; + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output_; Synchronizer synchronizer_; //!< Object that tracks the latest common timestamp of multiple variables diff --git a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h index 201570747..4e7956b2c 100644 --- a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h +++ b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h @@ -86,7 +86,7 @@ struct Acceleration2DParams : public ParameterBase //!< socket or not. TCP_NODELAY forces a socket to send the data in its buffer, //!< whatever the packet size. This reduces delay at the cost of network congestion, //!< specially if the payload of a packet is smaller than the TCP header data. This is - //!< true for small ROS messages like geometry_msgs::AccelWithCovarianceStamped + //!< true for small ROS messages like geometry_msgs::msg::AccelWithCovarianceStamped rclcpp::Duration tf_timeout { 0 }; //!< The maximum time to wait for a transform to become available rclcpp::Duration throttle_period { 0 }; //!< The throttle period duration in seconds bool throttle_use_wall_time { false }; //!< Whether to throttle using ros::WallTime or not diff --git a/fuse_models/include/fuse_models/parameters/imu_2d_params.h b/fuse_models/include/fuse_models/parameters/imu_2d_params.h index 76da55233..e09d808d7 100644 --- a/fuse_models/include/fuse_models/parameters/imu_2d_params.h +++ b/fuse_models/include/fuse_models/parameters/imu_2d_params.h @@ -118,7 +118,7 @@ struct Imu2DParams : public ParameterBase //!< socket or not. TCP_NODELAY forces a socket to send the data in its buffer, //!< whatever the packet size. This reduces delay at the cost of network congestion, //!< specially if the payload of a packet is smaller than the TCP header data. This is - //!< true for small ROS messages like geometry_msgs::AccelWithCovarianceStamped + //!< true for small ROS messages like geometry_msgs::msg::AccelWithCovarianceStamped rclcpp::Duration tf_timeout { 0 }; //!< The maximum time to wait for a transform to become available rclcpp::Duration throttle_period { 0 }; //!< The throttle period duration in seconds bool throttle_use_wall_time { false }; //!< Whether to throttle using ros::WallTime or not diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_params.h b/fuse_models/include/fuse_models/parameters/odometry_2d_params.h index 23d650b88..ffbdbb9ee 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_params.h +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_params.h @@ -116,7 +116,7 @@ struct Odometry2DParams : public ParameterBase //!< socket or not. TCP_NODELAY forces a socket to send the data in its buffer, //!< whatever the packet size. This reduces delay at the cost of network congestion, //!< specially if the payload of a packet is smaller than the TCP header data. This is - //!< true for small ROS messages like geometry_msgs::AccelWithCovarianceStamped + //!< true for small ROS messages like geometry_msgs::msg::AccelWithCovarianceStamped rclcpp::Duration tf_timeout { 0 }; //!< The maximum time to wait for a transform to become available rclcpp::Duration throttle_period { 0 }; //!< The throttle period duration in seconds bool throttle_use_wall_time { false }; //!< Whether to throttle using ros::WallTime or not diff --git a/fuse_models/include/fuse_models/parameters/pose_2d_params.h b/fuse_models/include/fuse_models/parameters/pose_2d_params.h index 1635ea577..43ff23d01 100644 --- a/fuse_models/include/fuse_models/parameters/pose_2d_params.h +++ b/fuse_models/include/fuse_models/parameters/pose_2d_params.h @@ -103,7 +103,7 @@ struct Pose2DParams : public ParameterBase //!< socket or not. TCP_NODELAY forces a socket to send the data in its buffer, //!< whatever the packet size. This reduces delay at the cost of network congestion, //!< specially if the payload of a packet is smaller than the TCP header data. This is - //!< true for small ROS messages like geometry_msgs::AccelWithCovarianceStamped + //!< true for small ROS messages like geometry_msgs::msg::AccelWithCovarianceStamped rclcpp::Duration tf_timeout { 0 }; //!< The maximum time to wait for a transform to become available rclcpp::Duration throttle_period { 0 }; //!< The throttle period duration in seconds bool throttle_use_wall_time { false }; //!< Whether to throttle using ros::WallTime or not diff --git a/fuse_models/include/fuse_models/parameters/twist_2d_params.h b/fuse_models/include/fuse_models/parameters/twist_2d_params.h index 267e2d91f..6cb0ee3ab 100644 --- a/fuse_models/include/fuse_models/parameters/twist_2d_params.h +++ b/fuse_models/include/fuse_models/parameters/twist_2d_params.h @@ -89,7 +89,7 @@ struct Twist2DParams : public ParameterBase //!< socket or not. TCP_NODELAY forces a socket to send the data in its buffer, //!< whatever the packet size. This reduces delay at the cost of network congestion, //!< specially if the payload of a packet is smaller than the TCP header data. This is - //!< true for small ROS messages like geometry_msgs::AccelWithCovarianceStamped + //!< true for small ROS messages like geometry_msgs::msg::AccelWithCovarianceStamped rclcpp::Duration tf_timeout { 0 }; //!< The maximum time to wait for a transform to become available rclcpp::Duration throttle_period { 0 }; //!< The throttle period duration in seconds bool throttle_use_wall_time { false }; //!< Whether to throttle using ros::WallTime or not diff --git a/fuse_models/include/fuse_models/pose_2d.h b/fuse_models/include/fuse_models/pose_2d.h index 0a3870b04..505a77121 100644 --- a/fuse_models/include/fuse_models/pose_2d.h +++ b/fuse_models/include/fuse_models/pose_2d.h @@ -34,13 +34,15 @@ #ifndef FUSE_MODELS_POSE_2D_H #define FUSE_MODELS_POSE_2D_H +#include + #include #include #include #include -#include +#include #include #include @@ -52,7 +54,7 @@ namespace fuse_models * @brief An adapter-type sensor that produces absolute or relative pose constraints from information published by * another node. * - * This sensor subscribes to a geometry_msgs::PoseWithCovarianceStamped topic and converts each received message + * This sensor subscribes to a geometry_msgs::msg::PoseWithCovarianceStamped topic and converts each received message * into an absolute or relative pose constraint. If the \p differential parameter is set to false (the default), the * measurement will be treated as an absolute constraint. If it is set to true, consecutive measurements will be used * to generate relative pose constraints. @@ -66,7 +68,7 @@ namespace fuse_models * constraints using consecutive measurements. * * Subscribes: - * - \p topic (geometry_msgs::PoseWithCovarianceStamped) Absolute pose information at a given timestamp + * - \p topic (geometry_msgs::msg::PoseWithCovarianceStamped) Absolute pose information at a given timestamp */ class Pose2D : public fuse_core::AsyncSensorModel { @@ -88,7 +90,7 @@ class Pose2D : public fuse_core::AsyncSensorModel * @brief Callback for pose messages * @param[in] msg - The pose message to process */ - void process(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg); + void process(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -119,12 +121,12 @@ class Pose2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential(const geometry_msgs::PoseWithCovarianceStamped& pose, const bool validate, + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, const bool validate, fuse_core::Transaction& transaction); ParameterType params_; - geometry_msgs::PoseWithCovarianceStamped::ConstPtr previous_pose_msg_; + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_msg_; tf2_ros::Buffer tf_buffer_; @@ -132,7 +134,8 @@ class Pose2D : public fuse_core::AsyncSensorModel ros::Subscriber subscriber_; - using PoseThrottledCallback = fuse_core::ThrottledMessageCallback; + using PoseThrottledCallback = + fuse_core::ThrottledMessageCallback; PoseThrottledCallback throttled_callback_; }; diff --git a/fuse_models/include/fuse_models/transaction.h b/fuse_models/include/fuse_models/transaction.h index 10bef4bfe..eb4b5e03c 100644 --- a/fuse_models/include/fuse_models/transaction.h +++ b/fuse_models/include/fuse_models/transaction.h @@ -40,7 +40,7 @@ #include #include -#include +#include #include namespace fuse_models @@ -50,7 +50,7 @@ namespace fuse_models * @brief An adapter-type sensor that produces transactions with the same added and removed constraints from an input * transaction. This is useful for debugging purposes because it allows to play back the recorded transactions. * - * This sensor subscribes to a fuse_msgs::SerializedTransaction topic and deserializes each received message into a + * This sensor subscribes to a fuse_msgs::msg::SerializedTransaction topic and deserializes each received message into a * transaction. * * Parameters: @@ -58,7 +58,7 @@ namespace fuse_models * - ~topic (string) The topic to which to subscribe for the transaction messages * * Subscribes: - * - topic (fuse_msgs::SerializedTransaction) Transaction + * - topic (fuse_msgs::msg::SerializedTransaction) Transaction */ class Transaction : public fuse_core::AsyncSensorModel { @@ -96,7 +96,7 @@ class Transaction : public fuse_core::AsyncSensorModel * @brief Callback for transaction messages * @param[in] msg - The transaction message to process */ - void process(const fuse_msgs::SerializedTransaction::ConstPtr& msg); + void process(const fuse_msgs::msg::SerializedTransaction& msg); ParameterType params_; //!< Object containing all of the configuration parameters diff --git a/fuse_models/include/fuse_models/twist_2d.h b/fuse_models/include/fuse_models/twist_2d.h index 33edb56e2..fc489f39b 100644 --- a/fuse_models/include/fuse_models/twist_2d.h +++ b/fuse_models/include/fuse_models/twist_2d.h @@ -40,7 +40,7 @@ #include #include -#include +#include #include #include @@ -51,7 +51,7 @@ namespace fuse_models /** * @brief An adapter-type sensor that produces absolute velocity constraints from information published by another node * - * This sensor subscribes to a geometry_msgs::TwistWithCovarianceStamped topic and converts each received message + * This sensor subscribes to a geometry_msgs::msg::TwistWithCovarianceStamped topic and converts each received message * into two absolute velocity constraints (one for linear velocity, and one for angular). * * Parameters: @@ -61,7 +61,7 @@ namespace fuse_models * - topic (string) The topic to which to subscribe for the twist messages * * Subscribes: - * - \p topic (geometry_msgs::TwistWithCovarianceStamped) Absolute velocity information at a given timestamp + * - \p topic (geometry_msgs::msg::TwistWithCovarianceStamped) Absolute velocity information at a given timestamp */ class Twist2D : public fuse_core::AsyncSensorModel { @@ -83,7 +83,7 @@ class Twist2D : public fuse_core::AsyncSensorModel * @brief Callback for twist messages * @param[in] msg - The twist message to process */ - void process(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg); + void process(const geometry_msgs::msg::TwistWithCovarianceStamped& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -111,7 +111,7 @@ class Twist2D : public fuse_core::AsyncSensorModel ros::Subscriber subscriber_; - using TwistThrottledCallback = fuse_core::ThrottledMessageCallback; + using TwistThrottledCallback = fuse_core::ThrottledMessageCallback; TwistThrottledCallback throttled_callback_; }; diff --git a/fuse_models/include/fuse_models/unicycle_2d_ignition.h b/fuse_models/include/fuse_models/unicycle_2d_ignition.h index deafc16f9..19a3d39ed 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_ignition.h +++ b/fuse_models/include/fuse_models/unicycle_2d_ignition.h @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include @@ -116,7 +116,7 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - void subscriberCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg); + void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); /** * @brief Triggers the publication of a new prior transaction at the supplied pose @@ -144,7 +144,7 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, yaw) */ - void process(const geometry_msgs::PoseWithCovarianceStamped& pose); + void process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose); /** * @brief Create and send a prior transaction based on the supplied pose @@ -154,7 +154,7 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, yaw) */ - void sendPrior(const geometry_msgs::PoseWithCovarianceStamped& pose); + void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose); std::atomic_bool started_; //!< Flag indicating the sensor has been started diff --git a/fuse_models/src/acceleration_2d.cpp b/fuse_models/src/acceleration_2d.cpp index 18954ae44..3d773bc5b 100644 --- a/fuse_models/src/acceleration_2d.cpp +++ b/fuse_models/src/acceleration_2d.cpp @@ -37,7 +37,7 @@ #include #include -#include +#include #include #include @@ -81,7 +81,7 @@ void Acceleration2D::onStart() { if (!params_.indices.empty()) { - subscriber_ = node_handle_.subscribe( + subscriber_ = node_handle_.subscribe( ros::names::resolve(params_.topic), params_.queue_size, &AccelerationThrottledCallback::callback, &throttled_callback_, ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); } @@ -92,16 +92,16 @@ void Acceleration2D::onStop() subscriber_.shutdown(); } -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, diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index 9e8947189..5f8b646cf 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -34,7 +34,7 @@ #include -#include +#include #include @@ -60,7 +60,7 @@ void GraphIgnition::onInit() // Connect to the reset service if (!params_.reset_service.empty()) { - reset_client_ = node_handle_.serviceClient(ros::names::resolve(params_.reset_service)); + reset_client_ = node_handle_.serviceClient(ros::names::resolve(params_.reset_service)); } // Advertise @@ -80,11 +80,11 @@ 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) { @@ -108,7 +108,7 @@ bool GraphIgnition::setGraphServiceCallback(fuse_models::SetGraph::Request& req, 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_) @@ -140,7 +140,7 @@ void GraphIgnition::process(const fuse_msgs::SerializedGraph& msg) "Waiting for '" << reset_client_.getService() << "' service to become avaiable."); } - auto srv = std_srvs::Empty(); + auto srv = std_srvs::srv::Empty(); if (!reset_client_.call(srv)) { // The reset() service failed. Propagate that failure to the caller of this service. diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp index 200ac9f27..28882be40 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 @@ -92,7 +92,7 @@ void Imu2D::onStart() !params_.angular_velocity_indices.empty()) { previous_pose_.reset(); - subscriber_ = node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, + subscriber_ = node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, &ImuThrottledCallback::callback, &throttled_callback_, ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); } @@ -103,38 +103,38 @@ void Imu2D::onStop() subscriber_.shutdown(); } -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]; + 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]; + 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; @@ -174,27 +174,27 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg) 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; @@ -218,11 +218,11 @@ 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; @@ -242,7 +242,7 @@ 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; diff --git a/fuse_models/src/odometry_2d.cpp b/fuse_models/src/odometry_2d.cpp index f04c50450..19d7eb009 100644 --- a/fuse_models/src/odometry_2d.cpp +++ b/fuse_models/src/odometry_2d.cpp @@ -37,9 +37,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -93,7 +93,7 @@ void Odometry2D::onStart() !params_.angular_velocity_indices.empty()) { previous_pose_.reset(); - subscriber_ = node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, + subscriber_ = node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, &OdometryThrottledCallback::callback, &throttled_callback_, ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); } @@ -104,21 +104,21 @@ void Odometry2D::onStop() subscriber_.shutdown(); } -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; @@ -161,11 +161,11 @@ 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; @@ -185,7 +185,7 @@ 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; diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp index bb87ad169..5204ac017 100644 --- a/fuse_models/src/odometry_2d_publisher.cpp +++ b/fuse_models/src/odometry_2d_publisher.cpp @@ -39,11 +39,11 @@ #include #include -#include -#include +#include +#include #include #include -#include +#include #include #include @@ -82,8 +82,8 @@ void Odometry2DPublisher::onInit() tf_listener_ = std::make_unique(*tf_buffer_, node_handle_); } - odom_pub_ = node_handle_.advertise(ros::names::resolve(params_.topic), params_.queue_size); - acceleration_pub_ = node_handle_.advertise( + 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_); @@ -116,8 +116,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, @@ -234,8 +234,8 @@ 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( @@ -260,8 +260,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 { @@ -324,8 +324,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_); @@ -484,7 +484,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 +503,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; diff --git a/fuse_models/src/pose_2d.cpp b/fuse_models/src/pose_2d.cpp index fc5dda05e..0b27c52f0 100644 --- a/fuse_models/src/pose_2d.cpp +++ b/fuse_models/src/pose_2d.cpp @@ -37,7 +37,7 @@ #include #include -#include +#include #include #include @@ -86,7 +86,7 @@ void Pose2D::onStart() if (!params_.position_indices.empty() || !params_.orientation_indices.empty()) { - subscriber_ = node_handle_.subscribe( + subscriber_ = node_handle_.subscribe( ros::names::resolve(params_.topic), params_.queue_size, &PoseThrottledCallback::callback, &throttled_callback_, ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); } @@ -97,24 +97,24 @@ void Pose2D::onStop() subscriber_.shutdown(); } -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, @@ -129,10 +129,10 @@ 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)) diff --git a/fuse_models/src/transaction.cpp b/fuse_models/src/transaction.cpp index fa4b80d16..c12499e96 100644 --- a/fuse_models/src/transaction.cpp +++ b/fuse_models/src/transaction.cpp @@ -64,7 +64,7 @@ void Transaction::onStop() subscriber_.shutdown(); } -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()); diff --git a/fuse_models/src/twist_2d.cpp b/fuse_models/src/twist_2d.cpp index 571f97ff1..0eb2549ef 100644 --- a/fuse_models/src/twist_2d.cpp +++ b/fuse_models/src/twist_2d.cpp @@ -37,7 +37,7 @@ #include #include -#include +#include #include #include @@ -83,7 +83,7 @@ void Twist2D::onStart() if (!params_.linear_indices.empty() || !params_.angular_indices.empty()) { - subscriber_ = node_handle_.subscribe( + subscriber_ = node_handle_.subscribe( ros::names::resolve(params_.topic), params_.queue_size, &TwistThrottledCallback::callback, &throttled_callback_, ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); } @@ -94,16 +94,16 @@ void Twist2D::onStop() subscriber_.shutdown(); } -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, diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 6f54b4be1..4b3ccc0b1 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -49,12 +49,12 @@ #include #include -#include +#include #include -#include +#include #include #include -#include +#include #include @@ -86,7 +86,7 @@ void Unicycle2DIgnition::onInit() // Connect to the reset service if (!params_.reset_service.empty()) { - reset_client_ = node_handle_.serviceClient(ros::names::resolve(params_.reset_service)); + reset_client_ = node_handle_.serviceClient(ros::names::resolve(params_.reset_service)); } // Advertise @@ -114,7 +114,7 @@ void Unicycle2DIgnition::start() // Send an initial state transaction immediately, if requested if (params_.publish_on_startup && !initial_transaction_sent_) { - auto pose = geometry_msgs::PoseWithCovarianceStamped(); + auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); pose.header.stamp = this->get_node_clock_interface()->now(); pose.pose.pose.position.x = params_.initial_state[0]; pose.pose.pose.position.y = params_.initial_state[1]; @@ -132,11 +132,11 @@ 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) { @@ -176,7 +176,7 @@ bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( } } -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_) @@ -233,7 +233,7 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& "Waiting for '" << reset_client_.getService() << "' service to become avaiable."); } - auto srv = std_srvs::Empty(); + auto srv = std_srvs::srv::Empty(); if (!reset_client_.call(srv)) { // The reset() service failed. Propagate that failure to the caller of this service. @@ -246,7 +246,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; From 6e50e5b379740626b16635ce25a3a77ae30f3405 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 28 Dec 2022 17:36:56 -0800 Subject: [PATCH 2/4] Port fuse_models 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 | 2 +- .../include/fuse_models/acceleration_2d.h | 34 +- .../include/fuse_models/common/sensor_proc.h | 61 ++-- .../include/fuse_models/graph_ignition.h | 42 ++- fuse_models/include/fuse_models/imu_2d.h | 30 +- fuse_models/include/fuse_models/odometry_2d.h | 31 +- .../fuse_models/odometry_2d_publisher.h | 43 ++- .../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 | 31 +- .../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 | 31 +- fuse_models/include/fuse_models/transaction.h | 20 +- fuse_models/include/fuse_models/twist_2d.h | 31 +- fuse_models/include/fuse_models/unicycle_2d.h | 23 +- .../fuse_models/unicycle_2d_ignition.h | 56 ++- .../include/fuse_models/unicycle_2d_predict.h | 2 +- fuse_models/package.xml | 78 ++-- fuse_models/src/acceleration_2d.cpp | 59 ++- fuse_models/src/graph_ignition.cpp | 91 +++-- fuse_models/src/imu_2d.cpp | 75 ++-- fuse_models/src/odometry_2d.cpp | 74 +++- fuse_models/src/odometry_2d_publisher.cpp | 107 ++++-- fuse_models/src/pose_2d.cpp | 65 +++- fuse_models/src/transaction.cpp | 31 +- fuse_models/src/twist_2d.cpp | 59 ++- fuse_models/src/unicycle_2d.cpp | 55 ++- fuse_models/src/unicycle_2d_ignition.cpp | 122 +++++-- ...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 | 120 +++--- fuse_models/test/test_sensor_proc.cpp | 6 - fuse_models/test/test_unicycle_2d.cpp | 6 - .../test/test_unicycle_2d_ignition.cpp | 179 +++++---- 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, 1588 insertions(+), 1034 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 3d773bc5b..8e44a761f 100644 --- a/fuse_models/src/acceleration_2d.cpp +++ b/fuse_models/src/acceleration_2d.cpp @@ -38,8 +38,8 @@ #include #include -#include -#include +#include +#include // Register this sensor model with ROS as a plugin. @@ -51,45 +51,78 @@ 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::msg::AccelWithCovarianceStamped& msg) @@ -105,7 +138,7 @@ void Acceleration2D::process(const geometry_msgs::msg::AccelWithCovarianceStampe 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 5f8b646cf..b77fcbb43 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(), + interfaces_.get_node_services_interface()->resolve_service_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(), + interfaces_.get_node_services_interface()->resolve_service_name(params_.set_graph_service), + std::bind( + &GraphIgnition::setGraphServiceCallback, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS(), + cb_group_ + ); } void GraphIgnition::start() @@ -88,22 +124,24 @@ void GraphIgnition::subscriberCallback(const fuse_msgs::msg::SerializedGraph& ms } 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; } @@ -134,17 +172,21 @@ void GraphIgnition::process(const fuse_msgs::msg::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::srv::Empty(); - if (!reset_client_.call(srv)) + auto srv = std::make_shared(); + auto result_future = reset_client_->async_send_request(srv); + if (rclcpp::spin_until_future_complete( + interfaces_.get_node_base_interface(), result_future, std::chrono::seconds(10)) + != rclcpp::FutureReturnCode::SUCCESS) { // 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."); + throw std::runtime_error("Failed to call the '" + std::string(reset_client_->get_service_name()) + "' service."); } } @@ -187,8 +229,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 28882be40..7094eef05 100644 --- a/fuse_models/src/imu_2d.cpp +++ b/fuse_models/src/imu_2d.cpp @@ -40,8 +40,8 @@ #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,15 +113,27 @@ 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::msg::Imu& msg) @@ -152,7 +185,7 @@ void Imu2D::process(const sensor_msgs::msg::Imu& msg) params_.orientation_target_frame, {}, params_.orientation_indices, - tf_buffer_, + *tf_buffer_, validate, *transaction, params_.tf_timeout); @@ -168,7 +201,7 @@ void Imu2D::process(const sensor_msgs::msg::Imu& msg) params_.twist_target_frame, {}, params_.angular_velocity_indices, - tf_buffer_, + *tf_buffer_, validate, *transaction, params_.tf_timeout); @@ -209,7 +242,7 @@ void Imu2D::process(const sensor_msgs::msg::Imu& msg) params_.linear_acceleration_loss, params_.acceleration_target_frame, params_.linear_acceleration_indices, - tf_buffer_, + *tf_buffer_, validate, *transaction, params_.tf_timeout); @@ -226,10 +259,10 @@ void Imu2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStam 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; } @@ -246,10 +279,10 @@ void Imu2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStam 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 19d7eb009..55efa607b 100644 --- a/fuse_models/src/odometry_2d.cpp +++ b/fuse_models/src/odometry_2d.cpp @@ -40,8 +40,8 @@ #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,15 +114,28 @@ 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::msg::Odometry& msg) @@ -136,7 +170,7 @@ void Odometry2D::process(const nav_msgs::msg::Odometry& 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::msg::Odometry& msg) params_.twist_target_frame, params_.linear_velocity_indices, params_.angular_velocity_indices, - tf_buffer_, + *tf_buffer_, validate, *transaction, params_.tf_timeout); @@ -169,11 +203,11 @@ void Odometry2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianc 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; } @@ -189,10 +223,10 @@ void Odometry2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianc 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 5204ac017..48e034992 100644 --- a/fuse_models/src/odometry_2d_publisher.cpp +++ b/fuse_models/src/odometry_2d_publisher.cpp @@ -41,13 +41,14 @@ #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; @@ -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); @@ -238,9 +271,12 @@ void Odometry2DPublisher::onStart() 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( @@ -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; } @@ -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) { @@ -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 0b27c52f0..d2b82b6cd 100644 --- a/fuse_models/src/pose_2d.cpp +++ b/fuse_models/src/pose_2d.cpp @@ -38,8 +38,8 @@ #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,15 +107,27 @@ 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::msg::PoseWithCovarianceStamped& msg) @@ -119,7 +152,7 @@ void Pose2D::process(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) params_.target_frame, params_.position_indices, params_.orientation_indices, - tf_buffer_, + *tf_buffer_, validate, *transaction, params_.tf_timeout); @@ -135,10 +168,10 @@ void Pose2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceSta 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 c12499e96..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::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 0eb2549ef..6e4beb46f 100644 --- a/fuse_models/src/twist_2d.cpp +++ b/fuse_models/src/twist_2d.cpp @@ -38,8 +38,8 @@ #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,15 +104,27 @@ 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::msg::TwistWithCovarianceStamped& msg) @@ -109,7 +142,7 @@ void Twist2D::process(const geometry_msgs::msg::TwistWithCovarianceStamped& msg) 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 4b3ccc0b1..e01a727fb 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 @@ -50,8 +50,8 @@ #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(), + interfaces_.get_node_services_interface()->resolve_service_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(), + interfaces_.get_node_services_interface()->resolve_service_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(), + interfaces_.get_node_services_interface()->resolve_service_name(params_.set_pose_deprecated_service), + std::bind( + &Unicycle2DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS(), + cb_group_ + ); } void Unicycle2DIgnition::start() @@ -115,7 +150,7 @@ void Unicycle2DIgnition::start() if (params_.publish_on_startup && !initial_transaction_sent_) { auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); - pose.header.stamp = this->get_node_clock_interface()->now(); + 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])); @@ -140,38 +175,40 @@ void Unicycle2DIgnition::subscriberCallback(const geometry_msgs::msg::PoseWithCo } 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; } } @@ -227,17 +264,21 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta 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::srv::Empty(); - if (!reset_client_.call(srv)) + auto srv = std::make_shared(); + auto result_future = reset_client_->async_send_request(srv); + if (rclcpp::spin_until_future_complete( + interfaces_.get_node_base_interface(), result_future, std::chrono::seconds(10)) + != rclcpp::FutureReturnCode::SUCCESS) { // 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."); + throw std::runtime_error("Failed to call the '" + std::string(reset_client_->get_service_name()) + "' service."); } } @@ -332,8 +373,9 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS // 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..708893cb5 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,24 @@ 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("/set_graph"); + 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 +328,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 +348,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 +367,24 @@ 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("/set_graph"); + 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 +442,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..0ac90e65e 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,23 @@ 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("/set_pose"); + 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 +324,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 +346,22 @@ 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("/set_pose_deprecated"); + 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 +420,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(); From ff677c908b521751331356c42641159df25cbd45 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 10 Jan 2023 03:19:16 -0800 Subject: [PATCH 3/4] Fix alloc error and some bugs Signed-off-by: methylDragon --- .../parameters/odometry_2d_publisher_params.h | 4 ++-- .../fuse_models/parameters/parameter_base.h | 4 ++-- fuse_models/src/graph_ignition.cpp | 12 +++--------- fuse_models/src/unicycle_2d_ignition.cpp | 14 ++++---------- 4 files changed, 11 insertions(+), 23 deletions(-) diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h index 60736b7fd..8bfc757dd 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h @@ -100,8 +100,8 @@ struct Odometry2DPublisherParams : public ParameterBase map_frame_id = fuse_core::getParam(interfaces, ns + "map_frame_id", map_frame_id); odom_frame_id = fuse_core::getParam(interfaces, ns + "odom_frame_id", odom_frame_id); base_link_frame_id = fuse_core::getParam(interfaces, ns + "base_link_frame_id", base_link_frame_id); - base_link_frame_id = fuse_core::getParam(interfaces, ns + "base_link_output_frame_id", base_link_frame_id); - odom_frame_id = fuse_core::getParam(interfaces, ns + "world_frame_id", odom_frame_id); + base_link_output_frame_id = fuse_core::getParam(interfaces, ns + "base_link_output_frame_id", base_link_output_frame_id); + world_frame_id = fuse_core::getParam(interfaces, ns + "world_frame_id", world_frame_id); const bool frames_valid = map_frame_id != odom_frame_id && diff --git a/fuse_models/include/fuse_models/parameters/parameter_base.h b/fuse_models/include/fuse_models/parameters/parameter_base.h index fd0dc4714..5e3442e21 100644 --- a/fuse_models/include/fuse_models/parameters/parameter_base.h +++ b/fuse_models/include/fuse_models/parameters/parameter_base.h @@ -88,8 +88,8 @@ inline std::vector loadSensorConfig( fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name) { - std::vector dimensions = - fuse_core::getParam(interfaces, name, dimensions); + std::vector dimensions; + dimensions = fuse_core::getParam(interfaces, name, dimensions); if (!dimensions.empty()) { return common::getDimensionIndices(dimensions); diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index b77fcbb43..5a5bc7915 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -78,7 +78,7 @@ void GraphIgnition::onInit() interfaces_.get_node_base_interface(), interfaces_.get_node_graph_interface(), interfaces_.get_node_services_interface(), - interfaces_.get_node_services_interface()->resolve_service_name(params_.reset_service), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.reset_service), rclcpp::ServicesQoS(), cb_group_ ); @@ -98,7 +98,7 @@ void GraphIgnition::onInit() set_graph_service_ = rclcpp::create_service( interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), - interfaces_.get_node_services_interface()->resolve_service_name(params_.set_graph_service), + 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(), @@ -180,14 +180,8 @@ void GraphIgnition::process(const fuse_msgs::msg::SerializedGraph& msg) } 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); - if (rclcpp::spin_until_future_complete( - interfaces_.get_node_base_interface(), result_future, std::chrono::seconds(10)) - != rclcpp::FutureReturnCode::SUCCESS) - { - // The reset() service failed. Propagate that failure to the caller of this service. - throw std::runtime_error("Failed to call the '" + std::string(reset_client_->get_service_name()) + "' service."); - } } // Now that the optimizer has been reset, actually send the initial state constraints to the optimizer diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index e01a727fb..f41131f67 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -103,7 +103,7 @@ void Unicycle2DIgnition::onInit() interfaces_.get_node_base_interface(), interfaces_.get_node_graph_interface(), interfaces_.get_node_services_interface(), - interfaces_.get_node_services_interface()->resolve_service_name(params_.reset_service), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.reset_service), rclcpp::ServicesQoS(), cb_group_ ); @@ -123,7 +123,7 @@ void Unicycle2DIgnition::onInit() set_pose_service_ = rclcpp::create_service( interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), - interfaces_.get_node_services_interface()->resolve_service_name(params_.set_pose_service), + 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(), @@ -132,7 +132,7 @@ void Unicycle2DIgnition::onInit() set_pose_deprecated_service_ = rclcpp::create_service( interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), - interfaces_.get_node_services_interface()->resolve_service_name(params_.set_pose_deprecated_service), + 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(), @@ -272,14 +272,8 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta } 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); - if (rclcpp::spin_until_future_complete( - interfaces_.get_node_base_interface(), result_future, std::chrono::seconds(10)) - != rclcpp::FutureReturnCode::SUCCESS) - { - // The reset() service failed. Propagate that failure to the caller of this service. - throw std::runtime_error("Failed to call the '" + std::string(reset_client_->get_service_name()) + "' service."); - } } // Now that the pose has been validated and the optimizer has been reset, actually send the initial state constraints From c0a8f4679d118b960fb694253dda7437dbfe939b Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 10 Jan 2023 13:32:12 -0800 Subject: [PATCH 4/4] Wait on result Signed-off-by: methylDragon --- fuse_models/src/graph_ignition.cpp | 1 + fuse_models/src/unicycle_2d_ignition.cpp | 1 + fuse_models/test/test_graph_ignition.cpp | 10 ++++++---- fuse_models/test/test_unicycle_2d_ignition.cpp | 10 ++++++---- 4 files changed, 14 insertions(+), 8 deletions(-) diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index 5a5bc7915..1c26d55a9 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -182,6 +182,7 @@ void GraphIgnition::process(const fuse_msgs::msg::SerializedGraph& msg) 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 diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index f41131f67..d72bca8e0 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -274,6 +274,7 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta 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 diff --git a/fuse_models/test/test_graph_ignition.cpp b/fuse_models/test/test_graph_ignition.cpp index 708893cb5..002d39640 100644 --- a/fuse_models/test/test_graph_ignition.cpp +++ b/fuse_models/test/test_graph_ignition.cpp @@ -222,7 +222,7 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) rclcpp::NodeOptions options; options.arguments({ "--ros-args", - "-p", "ignition_sensor.set_graph_service:=/set_graph", + "-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); @@ -266,7 +266,8 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) // Call the SetGraph service auto srv = std::make_shared(); fuse_core::serializeGraph(graph, srv->graph); - auto client = node->create_client("/set_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); @@ -337,7 +338,7 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) rclcpp::NodeOptions options; options.arguments({ "--ros-args", - "-p", "ignition_sensor.set_graph_service:=/set_graph", + "-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); @@ -381,7 +382,8 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) // Call the SetGraph service auto srv = std::make_shared(); fuse_core::serializeGraph(graph, srv->graph); - auto client = node->create_client("/set_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); diff --git a/fuse_models/test/test_unicycle_2d_ignition.cpp b/fuse_models/test/test_unicycle_2d_ignition.cpp index 0ac90e65e..4470b183e 100644 --- a/fuse_models/test/test_unicycle_2d_ignition.cpp +++ b/fuse_models/test/test_unicycle_2d_ignition.cpp @@ -236,7 +236,7 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseService) "[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.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); @@ -261,7 +261,8 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseService) 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("/set_pose"); + 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); @@ -334,7 +335,7 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseDeprecatedService) "[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.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); @@ -359,7 +360,8 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseDeprecatedService) 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("/set_pose_deprecated"); + 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)));