From 8055ad48770a82b90307d6b0db6eaee2822f68b1 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 29 Oct 2024 22:24:02 +0000 Subject: [PATCH 01/39] fix clang tidy errors in imu_3d --- fuse_models/include/fuse_models/imu_3d.hpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/fuse_models/include/fuse_models/imu_3d.hpp b/fuse_models/include/fuse_models/imu_3d.hpp index 46c8979b9..cd6e5b392 100644 --- a/fuse_models/include/fuse_models/imu_3d.hpp +++ b/fuse_models/include/fuse_models/imu_3d.hpp @@ -112,6 +112,11 @@ class Imu3D : public fuse_core::AsyncSensorModel */ virtual ~Imu3D() = default; + Imu3D(Imu3D const&) = delete; + Imu3D(Imu3D&&) = delete; + Imu3D& operator=(Imu3D const&) = delete; + Imu3D& operator=(Imu3D&&) = delete; + /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ @@ -157,7 +162,7 @@ class Imu3D : public fuse_core::AsyncSensorModel * @param[out] transaction - The generated variables and constraints are added to this transaction */ void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, bool validate, fuse_core::Transaction& transaction); fuse_core::node_interfaces::NodeInterfaces Date: Tue, 29 Oct 2024 22:53:44 +0000 Subject: [PATCH 02/39] clang tidy fixes for various core files --- .../include/fuse_core/async_publisher.hpp | 9 +- .../include/fuse_core/async_sensor_model.hpp | 8 +- fuse_core/include/fuse_core/sensor_model.hpp | 7 +- fuse_core/src/async_publisher.cpp | 6 +- fuse_core/src/async_sensor_model.cpp | 6 +- .../fuse_models/common/sensor_proc.hpp | 119 +++++++++--------- 6 files changed, 87 insertions(+), 68 deletions(-) diff --git a/fuse_core/include/fuse_core/async_publisher.hpp b/fuse_core/include/fuse_core/async_publisher.hpp index af016b583..ff5c0bc05 100644 --- a/fuse_core/include/fuse_core/async_publisher.hpp +++ b/fuse_core/include/fuse_core/async_publisher.hpp @@ -34,10 +34,8 @@ #ifndef FUSE_CORE__ASYNC_PUBLISHER_HPP_ #define FUSE_CORE__ASYNC_PUBLISHER_HPP_ -#include #include #include -#include #include #include @@ -73,6 +71,10 @@ class AsyncPublisher : public Publisher * @brief Destructor */ virtual ~AsyncPublisher(); + AsyncPublisher(AsyncPublisher const&) = delete; + AsyncPublisher(AsyncPublisher&&) = delete; + AsyncPublisher& operator=(AsyncPublisher const&) = delete; + AsyncPublisher& operator=(AsyncPublisher&&) = delete; /** * @brief Initialize the AsyncPublisher object @@ -203,6 +205,7 @@ class AsyncPublisher : public Publisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ + // NOLINTNEXTLINE virtual void notifyCallback(Transaction::ConstSharedPtr /*transaction*/, Graph::ConstSharedPtr /*graph*/) { } @@ -230,7 +233,7 @@ class AsyncPublisher : public Publisher private: //! Stop the internal executor thread (in order to use this class again it must be re-initialized) - void internal_stop(); + void internalStop(); }; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/async_sensor_model.hpp b/fuse_core/include/fuse_core/async_sensor_model.hpp index c550fe72e..c83b7c6d4 100644 --- a/fuse_core/include/fuse_core/async_sensor_model.hpp +++ b/fuse_core/include/fuse_core/async_sensor_model.hpp @@ -34,7 +34,6 @@ #ifndef FUSE_CORE__ASYNC_SENSOR_MODEL_HPP_ #define FUSE_CORE__ASYNC_SENSOR_MODEL_HPP_ -#include #include #include @@ -97,6 +96,10 @@ class AsyncSensorModel : public SensorModel * @brief Destructor */ virtual ~AsyncSensorModel(); + AsyncSensorModel(AsyncSensorModel const&) = delete; + AsyncSensorModel(AsyncSensorModel&&) = delete; + AsyncSensorModel& operator=(AsyncSensorModel const&) = delete; + AsyncSensorModel& operator=(AsyncSensorModel&&) = delete; /** * @brief Function to be executed whenever the optimizer has completed a Graph update @@ -236,6 +239,7 @@ class AsyncSensorModel : public SensorModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ + // NOLINTNEXTLINE virtual void onGraphUpdate(Graph::ConstSharedPtr /*graph*/) { } @@ -281,7 +285,7 @@ class AsyncSensorModel : public SensorModel private: //! Stop the internal executor thread (in order to use this class again it must be re-initialized) - void internal_stop(); + void internalStop(); }; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/sensor_model.hpp b/fuse_core/include/fuse_core/sensor_model.hpp index 935f0ea24..61b7bfbd5 100644 --- a/fuse_core/include/fuse_core/sensor_model.hpp +++ b/fuse_core/include/fuse_core/sensor_model.hpp @@ -76,6 +76,10 @@ class SensorModel * @brief Destructor */ virtual ~SensorModel() = default; + SensorModel(SensorModel const&) = default; + SensorModel(SensorModel&&) = default; + SensorModel& operator=(SensorModel const&) = default; + SensorModel& operator=(SensorModel&&) = default; /** * @brief Function to be executed whenever the optimizer has completed a Graph update @@ -91,6 +95,7 @@ class SensorModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ + // NOLINTNEXTLINE virtual void graphCallback(Graph::ConstSharedPtr /*graph*/) { } @@ -113,7 +118,7 @@ class SensorModel /** * @brief Get the unique name of this sensor */ - virtual const std::string& name() const = 0; + [[nodiscard]] virtual const std::string& name() const = 0; /** * @brief Function to be executed whenever the optimizer is ready to receive transactions diff --git a/fuse_core/src/async_publisher.cpp b/fuse_core/src/async_publisher.cpp index cf174247a..df16c96f9 100644 --- a/fuse_core/src/async_publisher.cpp +++ b/fuse_core/src/async_publisher.cpp @@ -43,7 +43,7 @@ AsyncPublisher::AsyncPublisher(size_t thread_count) : name_("uninitialized"), ex AsyncPublisher::~AsyncPublisher() { - internal_stop(); + internalStop(); } void AsyncPublisher::initialize(node_interfaces::NodeInterfaces interfaces, @@ -131,12 +131,12 @@ void AsyncPublisher::stop() // Can't run in executor's thread because the executor won't service more // callbacks after the context is shutdown. // Join executor's threads right away. - internal_stop(); + internalStop(); onStop(); } } -void AsyncPublisher::internal_stop() +void AsyncPublisher::internalStop() { if (spinner_.joinable()) { diff --git a/fuse_core/src/async_sensor_model.cpp b/fuse_core/src/async_sensor_model.cpp index 9f9903737..bf71909ae 100644 --- a/fuse_core/src/async_sensor_model.cpp +++ b/fuse_core/src/async_sensor_model.cpp @@ -51,7 +51,7 @@ AsyncSensorModel::AsyncSensorModel(size_t thread_count) : name_("uninitialized") AsyncSensorModel::~AsyncSensorModel() { - internal_stop(); + internalStop(); } void AsyncSensorModel::initialize(node_interfaces::NodeInterfaces interfaces, @@ -144,12 +144,12 @@ void AsyncSensorModel::stop() // Can't run in executor's thread because the executor won't service more // callbacks after the context is shutdown. // Join executor's threads right away. - internal_stop(); + internalStop(); onStop(); } } -void AsyncSensorModel::internal_stop() +void AsyncSensorModel::internalStop() { if (spinner_.joinable()) { diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 2e0c6d71d..373b39b20 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -87,7 +87,7 @@ #include -static auto sensor_proc_clock = rclcpp::Clock(); +static auto const sensorProcClock = rclcpp::Clock(); namespace tf2 { @@ -99,23 +99,23 @@ namespace tf2 * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline void doTransform(const geometry_msgs::msg::TwistWithCovarianceStamped& t_in, - geometry_msgs::msg::TwistWithCovarianceStamped& t_out, +inline void doTransform(const geometry_msgs::msg::TwistWithCovarianceStamped& data_in, + geometry_msgs::msg::TwistWithCovarianceStamped& data_out, const geometry_msgs::msg::TransformStamped& transform) // NOLINT { tf2::Vector3 vl; - fromMsg(t_in.twist.twist.linear, vl); + fromMsg(data_in.twist.twist.linear, vl); tf2::Vector3 va; - fromMsg(t_in.twist.twist.angular, va); + fromMsg(data_in.twist.twist.angular, va); tf2::Transform t; fromMsg(transform.transform, t); - t_out.twist.twist.linear = tf2::toMsg(t.getBasis() * vl); - t_out.twist.twist.angular = tf2::toMsg(t.getBasis() * va); - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; + data_out.twist.twist.linear = tf2::toMsg(t.getBasis() * vl); + data_out.twist.twist.angular = tf2::toMsg(t.getBasis() * va); + data_out.header.stamp = transform.header.stamp; + data_out.header.frame_id = transform.header.frame_id; - t_out.twist.covariance = transformCovariance(t_in.twist.covariance, t); + data_out.twist.covariance = transformCovariance(data_in.twist.covariance, t); } /** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs AccelWithCovarianceStamped type. @@ -125,23 +125,23 @@ inline void doTransform(const geometry_msgs::msg::TwistWithCovarianceStamped& t_ * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline void doTransform(const geometry_msgs::msg::AccelWithCovarianceStamped& t_in, - geometry_msgs::msg::AccelWithCovarianceStamped& t_out, - const geometry_msgs::msg::TransformStamped& transform) // NOLINT +inline void doTransform(const geometry_msgs::msg::AccelWithCovarianceStamped& data_in, + geometry_msgs::msg::AccelWithCovarianceStamped& data_out, + const geometry_msgs::msg::TransformStamped& transform) { tf2::Vector3 al; - fromMsg(t_in.accel.accel.linear, al); + fromMsg(data_in.accel.accel.linear, al); tf2::Vector3 aa; - fromMsg(t_in.accel.accel.angular, aa); + fromMsg(data_in.accel.accel.angular, aa); tf2::Transform t; fromMsg(transform.transform, t); - t_out.accel.accel.linear = tf2::toMsg(t.getBasis() * al); - t_out.accel.accel.angular = tf2::toMsg(t.getBasis() * aa); - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; + data_out.accel.accel.linear = tf2::toMsg(t.getBasis() * al); + data_out.accel.accel.angular = tf2::toMsg(t.getBasis() * aa); + data_out.header.stamp = transform.header.stamp; + data_out.header.frame_id = transform.header.frame_id; - t_out.accel.covariance = transformCovariance(t_in.accel.covariance, t); + data_out.accel.covariance = transformCovariance(data_in.accel.covariance, t); } } // namespace tf2 @@ -164,7 +164,7 @@ inline std::vector mergeIndices(const std::vector& lhs_indices, { auto merged_indices = boost::copy_range>(boost::range::join(lhs_indices, rhs_indices)); - const auto rhs_it = merged_indices.begin() + lhs_indices.size(); + const auto rhs_it = merged_indices.begin() + static_cast(lhs_indices.size()); std::transform(rhs_it, merged_indices.end(), rhs_it, std::bind(std::plus(), std::placeholders::_1, rhs_offset)); @@ -186,13 +186,13 @@ inline void populatePartialMeasurement(const fuse_core::VectorXd& mean_full, con const std::vector& indices, fuse_core::VectorXd& mean_partial, fuse_core::MatrixXd& covariance_partial) { - for (size_t r = 0; r < indices.size(); ++r) + for (int64_t r = 0; r < static_cast(indices.size()); ++r) { - mean_partial(r) = mean_full(indices[r]); + mean_partial(r) = mean_full(static_cast(indices[r])); - for (size_t c = 0; c < indices.size(); ++c) + for (int64_t c = 0; c < static_cast(indices.size()); ++c) { - covariance_partial(r, c) = covariance_full(indices[r], indices[c]); + covariance_partial(r, c) = covariance_full(static_cast(indices[r]), static_cast(indices[c])); } } } @@ -209,11 +209,11 @@ inline void populatePartialMeasurement(const fuse_core::VectorXd& mean_full, con inline void populatePartialMeasurement(const fuse_core::MatrixXd& covariance_full, const std::vector& indices, fuse_core::MatrixXd& covariance_partial) { - for (size_t r = 0; r < indices.size(); ++r) + for (int64_t r = 0; r < static_cast(indices.size()); ++r) { - for (size_t c = 0; c < indices.size(); ++c) + for (int64_t c = 0; c < static_cast(indices.size()); ++c) { - covariance_partial(r, c) = covariance_full(indices[r], indices[c]); + covariance_partial(r, c) = covariance_full(static_cast(indices[r]), static_cast(indices[c])); } } } @@ -298,7 +298,7 @@ bool transformMessage(const tf2_ros::Buffer& tf_buffer, const T& input, T& outpu } catch (const tf2::TransformException& ex) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 5.0 * 1000, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 5.0 * 1000, "Could not transform message from " << input.header.frame_id << " to " << output.header.frame_id << ". Error was " << ex.what()); @@ -353,7 +353,7 @@ inline bool processAbsolutePoseWithCovariance(const std::string& source, const f if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Failed to transform pose message with stamp " << rclcpp::Time(pose.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -400,7 +400,7 @@ inline bool processAbsolutePoseWithCovariance(const std::string& source, const f } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial absolute pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -470,7 +470,7 @@ inline bool processAbsolutePose3DWithCovariance(const std::string& source, const if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Failed to transform pose message with stamp " << rclcpp::Time(pose.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -507,7 +507,7 @@ inline bool processAbsolutePose3DWithCovariance(const std::string& source, const } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial absolute pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -556,7 +556,7 @@ inline bool processAbsolutePose3DWithCovariance(const std::string& source, const } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial absolute pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -874,7 +874,7 @@ inline bool processDifferentialPoseWithCovariance(const std::string& source, con } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -973,7 +973,9 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c // by José Luis Blanco Claraco (https://arxiv.org/abs/2103.15980) // Convert from ROS msg to covariance geometry types - covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; + covariance_geometry::PoseQuaternionCovarianceRPY p1; + covariance_geometry::PoseQuaternionCovarianceRPY p2; + covariance_geometry::PoseQuaternionCovarianceRPY p12; covariance_geometry::fromROS(pose1.pose, p1); covariance_geometry::fromROS(pose2.pose, p2); @@ -1011,14 +1013,18 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c // C12 = J_p12^-1 * (C2 - J_p1 * C1 * J_p1^T) * J_p12^-T // First we need to convert covariances from RPY (6x6) to quaternion (7x7) - covariance_geometry::PoseQuaternionCovariance p1_q, p2_q, p12_q; + covariance_geometry::PoseQuaternionCovariance p1_q; + covariance_geometry::PoseQuaternionCovariance p2_q; + covariance_geometry::PoseQuaternionCovariance p12_q; covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance(p1, p1_q); covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance(p2, p2_q); // Then we need to compute the delta pose covariance_geometry::ComposePose3DQuaternion(covariance_geometry::InversePose(p1_q.first), p2_q.first, p12_q.first); // Now we have to compute pose composition jacobians so we can rotate covariances - Eigen::Matrix7d j_p1, j_p12, j_p12_inv; + Eigen::Matrix7d j_p1; + Eigen::Matrix7d j_p12; + Eigen::Matrix7d j_p12_inv; Eigen::Matrix4d j_qn; covariance_geometry::jacobianQuaternionNormalization(p12_q.first.second, j_qn); @@ -1057,7 +1063,7 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -1112,7 +1118,7 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -1255,7 +1261,7 @@ inline bool processDifferentialPoseWithTwistCovariance(const std::string& source if (dt < 1e-6) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Very small time difference " << dt << "s from '" << source << "' source."); return false; } @@ -1285,7 +1291,7 @@ inline bool processDifferentialPoseWithTwistCovariance(const std::string& source } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement using the twist covariance from '" << source << "' source: " << ex.what()); return false; @@ -1359,7 +1365,8 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour } // Convert the poses into tf2 transforms - tf2::Transform pose1_tf2, pose2_tf2; + tf2::Transform pose1_tf2; + tf2::Transform pose2_tf2; tf2::fromMsg(pose1.pose.pose, pose1_tf2); tf2::fromMsg(pose2.pose.pose, pose2_tf2); @@ -1394,7 +1401,7 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour if (dt < 1e-6) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Very small time difference " << dt << "s from '" << source << "' source."); return false; } @@ -1421,7 +1428,7 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement using the twist covariance from '" << source << "' source: " << ex.what()); return false; @@ -1470,7 +1477,7 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement using the twist covariance from '" << source << "' source: " << ex.what()); return false; @@ -1547,7 +1554,7 @@ inline bool processTwistWithCovariance(const std::string& source, const fuse_cor if (!transformMessage(tf_buffer, twist, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Failed to transform twist message with stamp " << rclcpp::Time(twist.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -1590,7 +1597,7 @@ inline bool processTwistWithCovariance(const std::string& source, const fuse_cor } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear velocity measurement from '" << source << "' source: " << ex.what()); add_constraint = false; @@ -1632,7 +1639,7 @@ inline bool processTwistWithCovariance(const std::string& source, const fuse_cor } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0, "Invalid partial angular velocity measurement from '" << source << "' source: " << ex.what()); add_constraint = false; @@ -1714,7 +1721,7 @@ inline bool processTwist3DWithCovariance(const std::string& source, const fuse_c if (!transformMessage(tf_buffer, twist, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Failed to transform twist message with stamp " << rclcpp::Time(twist.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -1758,7 +1765,7 @@ inline bool processTwist3DWithCovariance(const std::string& source, const fuse_c } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear velocity measurement from '" << source << "' source: " << ex.what()); add_constraint = false; @@ -1810,7 +1817,7 @@ inline bool processTwist3DWithCovariance(const std::string& source, const fuse_c } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0, "Invalid partial angular velocity measurement from '" << source << "' source: " << ex.what()); add_constraint = false; @@ -1884,7 +1891,7 @@ inline bool processAccelWithCovariance(const std::string& source, const fuse_cor if (!transformMessage(tf_buffer, acceleration, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0, "Failed to transform acceleration message with stamp " << rclcpp::Time(acceleration.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -1920,7 +1927,7 @@ inline bool processAccelWithCovariance(const std::string& source, const fuse_cor } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear acceleration measurement from '" << source << "' source: " << ex.what()); return false; @@ -1987,7 +1994,7 @@ inline bool processAccel3DWithCovariance(const std::string& source, const fuse_c if (!transformMessage(tf_buffer, acceleration, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0, "Failed to transform acceleration message with stamp " << rclcpp::Time(acceleration.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -2017,7 +2024,7 @@ inline bool processAccel3DWithCovariance(const std::string& source, const fuse_c } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear acceleration measurement from '" << source << "' source: " << ex.what()); return false; From e3b91413f99c7f4998931aa3d0fd8ae0a5acfccc Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 29 Oct 2024 23:42:55 +0000 Subject: [PATCH 03/39] more clang tidy fixes, allow c-style arrays --- .clang-tidy | 6 +- .../include/fuse_core/timestamp_manager.hpp | 5 + fuse_core/src/timestamp_manager.cpp | 2 +- fuse_core/src/transaction.cpp | 12 +- .../fuse_optimizers/fixed_lag_smoother.hpp | 13 +- fuse_optimizers/src/fixed_lag_smoother.cpp | 114 +++++++++--------- 6 files changed, 79 insertions(+), 73 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 19ae154ea..142007eb7 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -11,21 +11,23 @@ Checks: -*, readability-*, -bugprone-easily-swappable-parameters, -bugprone-exception-escape, + -cppcoreguidelines-avoid-c-arrays, -cppcoreguidelines-avoid-magic-numbers, -cppcoreguidelines-non-private-member-variables-in-classes, -cppcoreguidelines-pro-bounds-constant-array-index, -cppcoreguidelines-pro-bounds-pointer-arithmetic, + -cppcoreguidelines-pro-type-vararg, -google-readability-casting, -google-default-arguments, -misc-include-cleaner, -misc-non-private-member-variables-in-classes, - -modernize-use-trailing-return-type, -modernize-avoid-bind, + -modernize-avoid-c-arrays, + -modernize-use-trailing-return-type, -readability-identifier-length, -readability-function-cognitive-complexity, -readability-magic-numbers, -readability-uppercase-literal-suffix, - -cppcoreguidelines-pro-type-vararg, HeaderFilterRegex: '' CheckOptions: - key: llvm-namespace-comment.ShortNamespaceLines diff --git a/fuse_core/include/fuse_core/timestamp_manager.hpp b/fuse_core/include/fuse_core/timestamp_manager.hpp index 758395270..d084b8700 100644 --- a/fuse_core/include/fuse_core/timestamp_manager.hpp +++ b/fuse_core/include/fuse_core/timestamp_manager.hpp @@ -112,6 +112,11 @@ class TimestampManager explicit TimestampManager(MotionModelFunction generator, const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); + TimestampManager(TimestampManager const&) = default; + TimestampManager(TimestampManager&&) = default; + TimestampManager& operator=(TimestampManager const&) = default; + TimestampManager& operator=(TimestampManager&&) = default; + /** * @brief Constructor that accepts the motion model generator as a member function pointer and * object pointer diff --git a/fuse_core/src/timestamp_manager.cpp b/fuse_core/src/timestamp_manager.cpp index 80031b584..e512169ce 100644 --- a/fuse_core/src/timestamp_manager.cpp +++ b/fuse_core/src/timestamp_manager.cpp @@ -44,7 +44,7 @@ namespace fuse_core { TimestampManager::TimestampManager(MotionModelFunction generator, const rclcpp::Duration& buffer_length) - : generator_(generator), buffer_length_(buffer_length) + : generator_(std::move(generator)), buffer_length_(buffer_length) { } diff --git a/fuse_core/src/transaction.cpp b/fuse_core/src/transaction.cpp index dc1c088a6..cebb04dc4 100644 --- a/fuse_core/src/transaction.cpp +++ b/fuse_core/src/transaction.cpp @@ -47,10 +47,8 @@ const rclcpp::Time& Transaction::minStamp() const { return stamp_; } - else - { - return std::min(*involved_stamps_.begin(), stamp_); - } + + return std::min(*involved_stamps_.begin(), stamp_); } const rclcpp::Time& Transaction::maxStamp() const @@ -59,10 +57,8 @@ const rclcpp::Time& Transaction::maxStamp() const { return stamp_; } - else - { - return std::max(*involved_stamps_.rbegin(), stamp_); - } + + return std::max(*involved_stamps_.rbegin(), stamp_); } void Transaction::addInvolvedStamp(const rclcpp::Time& stamp) diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp index 53a07f651..2d229b678 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp @@ -37,7 +37,6 @@ #include #include -#include #include #include #include @@ -124,13 +123,17 @@ class FixedLagSmoother : public Optimizer * @param[in] interfaces The node interfaces for the node driving the optimizer * @param[in] graph The graph used with the optimizer */ - FixedLagSmoother(fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph = nullptr); + explicit FixedLagSmoother(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr); /** * @brief Destructor */ virtual ~FixedLagSmoother(); + FixedLagSmoother(FixedLagSmoother const&) = delete; + FixedLagSmoother(FixedLagSmoother&&) = delete; + FixedLagSmoother& operator=(FixedLagSmoother const&) = delete; + FixedLagSmoother& operator=(FixedLagSmoother&&) = delete; protected: /** @@ -301,8 +304,8 @@ class FixedLagSmoother : public Optimizer /** * @brief Service callback that resets the optimizer to its original state */ - bool resetServiceCallback(const std::shared_ptr, - std::shared_ptr); + bool resetServiceCallback(std::shared_ptr const&, + std::shared_ptr const&); /** * @brief Thread-safe read-only access to the timestamp of the first transaction diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 3fcd33be1..6da9a773c 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -240,7 +240,7 @@ void FixedLagSmoother::optimizationLoop() RCLCPP_FATAL_STREAM(logger_, "Optimization failed after updating the graph with the transaction with timestamp " << new_transaction_stamp.nanoseconds() << ". Leaving optimization loop and requesting node shutdown..."); - RCLCPP_INFO(logger_, summary_.FullReport().c_str()); + RCLCPP_INFO(logger_, "%s", summary_.FullReport().c_str()); rclcpp::shutdown(); break; } @@ -436,9 +436,11 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const r } } -bool FixedLagSmoother::resetServiceCallback(const std::shared_ptr, - std::shared_ptr) +// NOLINTBEGIN(performance-unnecessary-value-param) +bool FixedLagSmoother::resetServiceCallback(std::shared_ptr const& /*unused*/, + std::shared_ptr const& /*unused*/) { + // NOLINTEND(performance-unnecessary-value-param) // Tell all the plugins to stop stopPlugins(); // Reset the optimizer state @@ -486,65 +488,63 @@ void FixedLagSmoother::transactionCallback(const std::string& sensor_name, fuse_ << ", difference: " << (start_time - max_time).nanoseconds() << "ns"); return; } + // We need to add the new transaction to the pending_transactions_ queue + std::lock_guard pending_transactions_lock(pending_transactions_mutex_); + + // Add the new transaction to the pending set + // The pending set is arranged "smallest stamp last" to making popping off the back more + // efficient + auto comparator = [](const rclcpp::Time& value, const TransactionQueueElement& element) { + return value >= element.stamp(); + }; + auto position = + std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); + position = pending_transactions_.insert(position, { sensor_name, std::move(transaction) }); // NOLINT + + // If we haven't "started" yet.. + if (!started_) { - // We need to add the new transaction to the pending_transactions_ queue - std::lock_guard pending_transactions_lock(pending_transactions_mutex_); - - // Add the new transaction to the pending set - // The pending set is arranged "smallest stamp last" to making popping off the back more - // efficient - auto comparator = [](const rclcpp::Time& value, const TransactionQueueElement& element) { - return value >= element.stamp(); - }; - auto position = - std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); - position = pending_transactions_.insert(position, { sensor_name, std::move(transaction) }); // NOLINT - - // If we haven't "started" yet.. - if (!started_) + // ...check if we should + if (sensor_models_.at(sensor_name).ignition) + { + started_ = true; + ignited_ = true; + start_time = position->minStamp(); + setStartTime(start_time); + + // And purge out old transactions + // - Either before or exactly at the start time + // - Or with a minimum time before the minimum time of this ignition sensor transaction + // + // TODO(efernandez) Do '&min_time = std::as_const(start_ime)' when C++17 is supported and we + // can use std::as_const: + // https://en.cppreference.com/w/cpp/utility/as_const + pending_transactions_.erase( + std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), + [&sensor_name, max_time, + &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) + return transaction.sensor_name != sensor_name && + (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); + }), // NOLINT(whitespace/braces) + pending_transactions_.end()); + } + else { - // ...check if we should - if (sensor_models_.at(sensor_name).ignition) + // And purge out old transactions to limit the pending size while waiting for an ignition + // sensor + auto purge_time = rclcpp::Time(0, 0, RCL_ROS_TIME); // NOTE(CH3): Uninitialized + auto last_pending_time = pending_transactions_.front().stamp(); + + // rclcpp::Time doesn't allow negatives + if (rclcpp::Time(params_.transaction_timeout.nanoseconds(), last_pending_time.get_clock_type()) < + last_pending_time) { - started_ = true; - ignited_ = true; - start_time = position->minStamp(); - setStartTime(start_time); - - // And purge out old transactions - // - Either before or exactly at the start time - // - Or with a minimum time before the minimum time of this ignition sensor transaction - // - // TODO(efernandez) Do '&min_time = std::as_const(start_ime)' when C++17 is supported and we - // can use std::as_const: - // https://en.cppreference.com/w/cpp/utility/as_const - pending_transactions_.erase( - std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), - [&sensor_name, max_time, - &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) - return transaction.sensor_name != sensor_name && - (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), // NOLINT(whitespace/braces) - pending_transactions_.end()); + purge_time = last_pending_time - params_.transaction_timeout; } - else - { - // And purge out old transactions to limit the pending size while waiting for an ignition - // sensor - auto purge_time = rclcpp::Time(0, 0, RCL_ROS_TIME); // NOTE(CH3): Uninitialized - auto last_pending_time = pending_transactions_.front().stamp(); - - // rclcpp::Time doesn't allow negatives - if (rclcpp::Time(params_.transaction_timeout.nanoseconds(), last_pending_time.get_clock_type()) < - last_pending_time) - { - purge_time = last_pending_time - params_.transaction_timeout; - } - while (!pending_transactions_.empty() && pending_transactions_.back().maxStamp() < purge_time) - { - pending_transactions_.pop_back(); - } + while (!pending_transactions_.empty() && pending_transactions_.back().maxStamp() < purge_time) + { + pending_transactions_.pop_back(); } } } From 08ccd72c9dbb1882eaa17ef392d13c2a36c8f7e8 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 29 Oct 2024 23:54:22 +0000 Subject: [PATCH 04/39] remove unused headers --- .../include/fuse_optimizers/fixed_lag_smoother_params.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp index ee967cc54..5033b2582 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp @@ -37,9 +37,7 @@ #include -#include #include -#include #include #include From 50461abf29c37bb2bcf2b8b4f662ee7182fbec2d Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 31 Oct 2024 21:57:22 +0000 Subject: [PATCH 05/39] add skeleton of april tag pose detector, clang tidy fix to imu --- fuse_models/CMakeLists.txt | 1 + .../include/fuse_models/april_tag_pose.hpp | 181 ++++++++++++++++++ .../parameters/april_tag_pose_params.hpp | 73 +++++++ .../fuse_models/parameters/imu_3d_params.hpp | 8 +- fuse_models/src/april_tag_pose.cpp | 1 + 5 files changed, 260 insertions(+), 4 deletions(-) create mode 100644 fuse_models/include/fuse_models/april_tag_pose.hpp create mode 100644 fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp create mode 100644 fuse_models/src/april_tag_pose.cpp diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index abc639c8b..3e5a55dcf 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -43,6 +43,7 @@ find_package(Eigen3 REQUIRED) # Declare a C++ library add_library( ${PROJECT_NAME} + src/april_tag_pose.cpp src/acceleration_2d.cpp src/graph_ignition.cpp src/imu_2d.cpp diff --git a/fuse_models/include/fuse_models/april_tag_pose.hpp b/fuse_models/include/fuse_models/april_tag_pose.hpp new file mode 100644 index 000000000..54a3afc65 --- /dev/null +++ b/fuse_models/include/fuse_models/april_tag_pose.hpp @@ -0,0 +1,181 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__IMU_3D_HPP_ +#define FUSE_MODELS__IMU_3D_HPP_ + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "tf2_msgs/msg/tf_message.hpp" + +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 MessageType 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. + * 2. Creates 3D velocity variables and constraints. + * + * This sensor really just separates out the orientation, angular velocity, and linear acceleration + * components of the message, and processes them just like the Pose3D, Twist3D, and Acceleration3D + * classes. + * + * Parameters: + * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - queue_size (int, default: 10) The subscriber queue size for the pose messages + * - topic (string) The topic to which to subscribe for the pose messages + * - differential (bool, default: false) Whether we should fuse orientation measurements + * absolutely, or to create relative orientation constraints + * using consecutive measurements. + * - remove_gravitational_acceleration (bool, default: false) Whether we should remove acceleration + * due to gravity from the acceleration + * values produced by the IMU before + * fusing + * - gravitational_acceleration (double, default: 9.80665) Acceleration due to gravity, in + * meters/sec^2. This value is only used if + * \p remove_gravitational_acceleration is + * true + * - orientation_target_frame (string) Orientation data will be transformed into this frame before + * it is fused. + * - twist_target_frame (string) Twist/velocity data will be transformed into this frame before it + * is fused. + * - acceleration_target_frame (string) Acceleration data will be transformed into this frame + * before it is fused. + * + * Subscribes: + * - \p topic (MessageType) IMU data at a given timestep + */ +class AprilTagPose : public fuse_core::AsyncSensorModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS(AprilTagPose) + using ParameterType = parameters::AprilTagPoseParams; + using MessageType = tf2_msgs::msg::TFMessage; + + /** + * @brief Default constructor + */ + AprilTagPose(); + + /** + * @brief Destructor + */ + virtual ~AprilTagPose() = default; + + AprilTagPose(AprilTagPose const&) = delete; + AprilTagPose(AprilTagPose&&) = delete; + AprilTagPose& operator=(AprilTagPose const&) = delete; + AprilTagPose& operator=(AprilTagPose&&) = delete; + + /** + * @brief Shadowing extension to the AsyncSensorModel::initialize call + */ + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + + /** + * @brief Callback for pose messages + * @param[in] msg - The IMU message to process + */ + void process(const MessageType& msg); + +protected: + fuse_core::UUID device_id_; //!< The UUID of this device + + /** + * @brief Perform any required initialization for the sensor model + * + * This could include things like reading from the parameter server or subscribing to topics. The + * class's node handles will be properly initialized before SensorModel::onInit() is called. + * Spinning of the callback queue will not begin until after the call to SensorModel::onInit() + * completes. + */ + void onInit() override; + + /** + * @brief Subscribe to the input topic to start sending transactions to the optimizer + */ + void onStart() override; + + /** + * @brief Unsubscribe from the input topic to stop sending transactions to the optimizer + */ + void onStop() override; + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The sensor model's logger + + ParameterType params_; + + tf2_msgs::msg::TFMessage previous_pose_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr sub_; + + using AprilTagThrottledCallback = fuse_core::ThrottledMessageCallback; + AprilTagThrottledCallback throttled_callback_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__IMU_3D_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp b/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp new file mode 100644 index 000000000..8c2c199c9 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp @@ -0,0 +1,73 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__PARAMETERS__APRIL_TAG_POSE_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__APRIL_TAG_POSE_PARAMS_HPP_ + +#include + +#include + +#include +#include +#include +#include +#include + +namespace fuse_models::parameters +{ + +/** + * @brief Defines the set of parameters required by the Imu3D class + */ +struct AprilTagPoseParams : public ParameterBase +{ +public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) override + { + } +}; + +} // namespace fuse_models::parameters + +#endif // FUSE_MODELS__PARAMETERS__APRIL_TAG_POSE_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp index c0df539a7..b64372911 100644 --- a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -134,10 +134,10 @@ struct Imu3DParams : public ParameterBase rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not double gravitational_acceleration{ 9.80665 }; - std::string acceleration_target_frame{}; - std::string orientation_target_frame{}; - std::string topic{}; - std::string twist_target_frame{}; + std::string acceleration_target_frame; + std::string orientation_target_frame; + std::string topic; + std::string twist_target_frame; std::vector angular_velocity_indices; std::vector linear_acceleration_indices; std::vector orientation_indices; diff --git a/fuse_models/src/april_tag_pose.cpp b/fuse_models/src/april_tag_pose.cpp new file mode 100644 index 000000000..22a0df698 --- /dev/null +++ b/fuse_models/src/april_tag_pose.cpp @@ -0,0 +1 @@ +#include From 92b4d0148a4b5514fb95ab5aceef12497f7b1d18 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 31 Oct 2024 22:01:27 +0000 Subject: [PATCH 06/39] switch copyright --- fuse_models/include/fuse_models/april_tag_pose.hpp | 2 +- .../include/fuse_models/parameters/april_tag_pose_params.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fuse_models/include/fuse_models/april_tag_pose.hpp b/fuse_models/include/fuse_models/april_tag_pose.hpp index 54a3afc65..af0d3ba2d 100644 --- a/fuse_models/include/fuse_models/april_tag_pose.hpp +++ b/fuse_models/include/fuse_models/april_tag_pose.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2023, Giacomo Franchini + * Copyright (c) 2024, PickNik Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp b/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp index 8c2c199c9..f19fad302 100644 --- a/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp +++ b/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2023, Giacomo Franchini + * Copyright (c) 2024, PickNik Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without From f4fb404c323e48f9c39cb39ae8b51db7ade74e23 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 1 Nov 2024 20:29:46 +0000 Subject: [PATCH 07/39] clang tidy fixes --- .clang-tidy | 1 + fuse_core/include/fuse_core/variable.hpp | 30 +++++------ fuse_core/test/example_variable.hpp | 22 ++++---- fuse_core/test/test_variable.cpp | 14 +++--- .../acceleration_angular_2d_stamped.hpp | 4 +- .../acceleration_angular_3d_stamped.hpp | 4 +- .../acceleration_linear_2d_stamped.hpp | 4 +- .../acceleration_linear_3d_stamped.hpp | 4 +- .../fuse_variables/fixed_size_variable.hpp | 10 ++-- .../fuse_variables/orientation_2d_stamped.hpp | 6 +-- .../fuse_variables/orientation_3d_stamped.hpp | 27 +++++----- .../include/fuse_variables/pinhole_camera.hpp | 4 +- .../fuse_variables/pinhole_camera_fixed.hpp | 2 +- .../point_2d_fixed_landmark.hpp | 4 +- .../fuse_variables/point_2d_landmark.hpp | 4 +- .../point_3d_fixed_landmark.hpp | 4 +- .../fuse_variables/point_3d_landmark.hpp | 4 +- .../fuse_variables/position_2d_stamped.hpp | 4 +- .../fuse_variables/position_3d_stamped.hpp | 4 +- .../include/fuse_variables/stamped.hpp | 6 ++- .../velocity_angular_2d_stamped.hpp | 4 +- .../velocity_angular_3d_stamped.hpp | 4 +- .../velocity_linear_2d_stamped.hpp | 4 +- .../velocity_linear_3d_stamped.hpp | 4 +- fuse_variables/src/orientation_2d_stamped.cpp | 9 ++-- fuse_variables/src/orientation_3d_stamped.cpp | 8 +-- .../test_acceleration_angular_2d_stamped.cpp | 2 +- .../test_acceleration_angular_3d_stamped.cpp | 2 +- .../test_acceleration_linear_2d_stamped.cpp | 2 +- .../test_acceleration_linear_3d_stamped.cpp | 2 +- .../test/test_fixed_size_variable.cpp | 8 ++- .../test/test_orientation_2d_stamped.cpp | 50 ++++++++----------- .../test/test_orientation_3d_stamped.cpp | 32 ++++-------- .../test/test_point_2d_fixed_landmark.cpp | 2 +- .../test/test_point_2d_landmark.cpp | 2 +- .../test/test_point_3d_fixed_landmark.cpp | 2 +- .../test/test_point_3d_landmark.cpp | 2 +- .../test/test_position_2d_stamped.cpp | 2 +- .../test/test_position_3d_stamped.cpp | 2 +- .../test/test_velocity_angular_2d_stamped.cpp | 2 +- .../test/test_velocity_angular_3d_stamped.cpp | 2 +- .../test/test_velocity_linear_2d_stamped.cpp | 2 +- .../test/test_velocity_linear_3d_stamped.cpp | 2 +- 43 files changed, 153 insertions(+), 160 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 142007eb7..d3a332ed9 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -14,6 +14,7 @@ Checks: -*, -cppcoreguidelines-avoid-c-arrays, -cppcoreguidelines-avoid-magic-numbers, -cppcoreguidelines-non-private-member-variables-in-classes, + -cppcoreguidelines-pro-bounds-array-to-pointer-decay, -cppcoreguidelines-pro-bounds-constant-array-index, -cppcoreguidelines-pro-bounds-pointer-arithmetic, -cppcoreguidelines-pro-type-vararg, diff --git a/fuse_core/include/fuse_core/variable.hpp b/fuse_core/include/fuse_core/variable.hpp index 550021977..0593ef5cc 100644 --- a/fuse_core/include/fuse_core/variable.hpp +++ b/fuse_core/include/fuse_core/variable.hpp @@ -224,11 +224,15 @@ class Variable * @brief Destructor */ virtual ~Variable() = default; + Variable(Variable const&) = default; + Variable(Variable&&) = default; + Variable& operator=(Variable const&) = default; + Variable& operator=(Variable&&) = default; /** * @brief Returns a UUID for this variable. */ - const UUID& uuid() const + [[nodiscard]] const UUID& uuid() const { return uuid_; } @@ -247,7 +251,7 @@ class Variable * To make this easy to implement in all derived classes, the FUSE_VARIABLE_TYPE_DEFINITION() * and FUSE_VARIABLE_DEFINITIONS() macro functions have been provided. */ - virtual std::string type() const = 0; + [[nodiscard]] virtual std::string type() const = 0; /** * @brief Returns the number of elements of this variable. @@ -257,7 +261,7 @@ class Variable * exception is a 3D rotation represented as a quaternion. It only has 3 degrees of freedom, * but it is represented as four elements, (w, x, y, z), so it's size will be 4. */ - virtual size_t size() const = 0; + [[nodiscard]] virtual size_t size() const = 0; /** * @brief Returns the number of elements of the local parameterization space. @@ -266,7 +270,7 @@ class Variable * override the \p localSize() method. By default, the \p size() method is used for \p * localSize() as well. */ - virtual size_t localSize() const + [[nodiscard]] virtual size_t localSize() const { return size(); } @@ -279,7 +283,7 @@ class Variable * Variable::size() elements will be accessed externally. This interface is provided for * integration with Ceres, which uses raw pointers. */ - virtual const double* data() const = 0; + [[nodiscard]] virtual const double* data() const = 0; /** * @brief Read-write access to the variable data @@ -314,7 +318,7 @@ class Variable * * @return A unique pointer to a new instance of the most-derived Variable */ - virtual Variable::UniquePtr clone() const = 0; + [[nodiscard]] virtual Variable::UniquePtr clone() const = 0; /** * @brief Create a new Ceres local parameterization object to apply to updates of this variable @@ -331,7 +335,7 @@ class Variable * * @return A base pointer to an instance of a derived LocalParameterization */ - virtual fuse_core::LocalParameterization* localParameterization() const + [[nodiscard]] virtual std::unique_ptr localParameterization() const { return nullptr; } @@ -354,21 +358,19 @@ class Variable * * @return A base pointer to an instance of a derived Manifold */ - virtual fuse_core::Manifold* manifold() const + [[nodiscard]] virtual std::unique_ptr manifold() const { // To support legacy Variable classes that still implements the localParameterization() method, // construct a ManifoldAdapter object from the LocalParameterization pointer as the default // action. If the Variable has been updated to use the new Manifold classes, then the Variable // should override this method and return a pointer to the appropriate derived Manifold object. auto local_parameterization = localParameterization(); - if (!local_parameterization) + if (local_parameterization == nullptr) { return nullptr; } - else - { - return new fuse_core::ManifoldAdapter(local_parameterization); - } + + return std::unique_ptr(new fuse_core::ManifoldAdapter(local_parameterization.get())); } #endif @@ -455,7 +457,7 @@ class Variable virtual void deserialize(fuse_core::TextInputArchive& /* archive */) = 0; private: - fuse_core::UUID uuid_; //!< The unique ID number for this variable + fuse_core::UUID uuid_{}; //!< The unique ID number for this variable // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_core/test/example_variable.hpp b/fuse_core/test/example_variable.hpp index a0e6b80c0..0cb4c9454 100644 --- a/fuse_core/test/example_variable.hpp +++ b/fuse_core/test/example_variable.hpp @@ -79,7 +79,7 @@ class ExampleVariable : public fuse_core::Variable * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -127,8 +127,8 @@ class LegacyParameterization : public fuse_core::LocalParameterization bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { double q_delta[4]; - ceres::AngleAxisToQuaternion(delta, q_delta); - ceres::QuaternionProduct(x, q_delta, x_plus_delta); + ceres::AngleAxisToQuaternion(delta, static_cast(q_delta)); + ceres::QuaternionProduct(x, static_cast(q_delta), x_plus_delta); return true; } @@ -164,8 +164,8 @@ class LegacyParameterization : public fuse_core::LocalParameterization x_inverse[3] = -x[3]; double q_delta[4]; - ceres::QuaternionProduct(x_inverse, y, q_delta); - ceres::QuaternionToAngleAxis(q_delta, y_minus_x); + ceres::QuaternionProduct(static_cast(x_inverse), y, static_cast(q_delta)); + ceres::QuaternionToAngleAxis(static_cast(q_delta), y_minus_x); return true; } @@ -219,17 +219,17 @@ class LegacyVariable : public fuse_core::Variable { } - size_t size() const override + [[nodiscard]] size_t size() const override { return 4; } - const double* data() const override + [[nodiscard]] const double* data() const override { - return data_; + return static_cast(data_); } double* data() override { - return data_; + return static_cast(data_); } void print(std::ostream& /*stream = std::cout*/) const override { @@ -251,9 +251,9 @@ class LegacyVariable : public fuse_core::Variable * * @return A pointer to a local parameterization object that indicates how to "add" increments to the quaternion */ - fuse_core::LocalParameterization* localParameterization() const override + [[nodiscard]] std::unique_ptr localParameterization() const override { - return new LegacyParameterization(); + return std::unique_ptr(new LegacyParameterization()); } private: diff --git a/fuse_core/test/test_variable.cpp b/fuse_core/test/test_variable.cpp index 4198913a6..7c558bd13 100644 --- a/fuse_core/test/test_variable.cpp +++ b/fuse_core/test/test_variable.cpp @@ -82,7 +82,7 @@ TEST(LegacyVariable, Serialization) #if CERES_SUPPORTS_MANIFOLDS struct QuaternionCostFunction { - explicit QuaternionCostFunction(double* observation) + explicit QuaternionCostFunction(const double* observation) { observation_[0] = observation[0]; observation_[1] = observation[1]; @@ -109,7 +109,7 @@ struct QuaternionCostFunction return true; } - double observation_[4]; + double observation_[4]{}; }; TEST(LegacyVariable, ManifoldAdapter) @@ -123,12 +123,12 @@ TEST(LegacyVariable, ManifoldAdapter) // Create a simple a constraint with an identity quaternion double target_quat[4] = { 1.0, 0.0, 0.0, 0.0 }; - ceres::CostFunction* cost_function = - new ceres::AutoDiffCostFunction(new QuaternionCostFunction(target_quat)); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction( + new QuaternionCostFunction(static_cast(target_quat))); // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.manifold()); + problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold().get()); std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -179,10 +179,10 @@ TEST(LegacyVariable, Deserialization) // Test the manifold interface, and that the Legacy LocalParameterization is wrapped // in a ManifoldAdapter - fuse_core::Manifold* actual_manifold = nullptr; + std::unique_ptr actual_manifold = nullptr; ASSERT_NO_THROW(actual_manifold = actual.manifold()); ASSERT_NE(actual_manifold, nullptr); - auto actual_manifold_adapter = dynamic_cast(actual_manifold); + auto* actual_manifold_adapter = dynamic_cast(actual_manifold.get()); ASSERT_NE(actual_manifold_adapter, nullptr); } #endif diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp index efaeeb0f9..59d5a1b20 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp @@ -118,7 +118,7 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -138,7 +138,7 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp index b91b1e1b5..6bd41e183 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp @@ -152,7 +152,7 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -172,7 +172,7 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp index cbdf503cd..693a35d23 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp @@ -135,7 +135,7 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -155,7 +155,7 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp index 11c71edaa..9f8c0f900 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp @@ -152,7 +152,7 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -172,7 +172,7 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/fixed_size_variable.hpp b/fuse_variables/include/fuse_variables/fixed_size_variable.hpp index 1d4e86f66..7c452e961 100644 --- a/fuse_variables/include/fuse_variables/fixed_size_variable.hpp +++ b/fuse_variables/include/fuse_variables/fixed_size_variable.hpp @@ -68,12 +68,16 @@ class FixedSizeVariable : public fuse_core::Variable /** * @brief A static version of the variable size */ - constexpr static size_t SIZE = N; + constexpr static size_t varSize = N; /** * @brief Default constructor */ FixedSizeVariable() = default; + FixedSizeVariable(FixedSizeVariable const&) = default; + FixedSizeVariable(FixedSizeVariable&&) = default; + FixedSizeVariable& operator=(FixedSizeVariable const&) = default; + FixedSizeVariable& operator=(FixedSizeVariable&&) = default; /** * @brief Constructor @@ -152,10 +156,6 @@ class FixedSizeVariable : public fuse_core::Variable archive& data_; } }; - -// Define the constant that was declared above -template -constexpr size_t FixedSizeVariable::SIZE; } // namespace fuse_variables #endif // FUSE_VARIABLES__FIXED_SIZE_VARIABLE_HPP_ diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp index 6393926f0..14371edfc 100644 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp @@ -266,7 +266,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * * @return A base pointer to an instance of a derived LocalParameterization */ - fuse_core::LocalParameterization* localParameterization() const override; + [[nodiscard]] std::unique_ptr localParameterization() const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -277,7 +277,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * * @return A base pointer to an instance of a derived manifold */ - fuse_core::Manifold* manifold() const override; + [[nodiscard]] std::unique_ptr manifold() const override; #endif private: @@ -294,7 +294,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp index 1781597c1..2f108854c 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp @@ -36,6 +36,7 @@ #include +#include #include #include @@ -98,8 +99,8 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { double q_delta[4]; - ceres::AngleAxisToQuaternion(delta, q_delta); - ceres::QuaternionProduct(x, q_delta, x_plus_delta); + ceres::AngleAxisToQuaternion(delta, static_cast(q_delta)); + ceres::QuaternionProduct(x, static_cast(q_delta), x_plus_delta); return true; } @@ -129,10 +130,10 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati bool Minus(const double* x, const double* y, double* y_minus_x) const override { double x_inverse[4]; - QuaternionInverse(x, x_inverse); + QuaternionInverse(x, static_cast(x_inverse)); double q_delta[4]; - ceres::QuaternionProduct(x_inverse, y, q_delta); - ceres::QuaternionToAngleAxis(q_delta, y_minus_x); + ceres::QuaternionProduct(static_cast(x_inverse), y, static_cast(q_delta)); + ceres::QuaternionToAngleAxis(static_cast(q_delta), y_minus_x); return true; } @@ -203,8 +204,8 @@ class Orientation3DManifold : public fuse_core::Manifold bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { double q_delta[4]; - ceres::AngleAxisToQuaternion(delta, q_delta); - ceres::QuaternionProduct(x, q_delta, x_plus_delta); + ceres::AngleAxisToQuaternion(delta, static_cast(q_delta)); + ceres::QuaternionProduct(x, static_cast(q_delta), x_plus_delta); return true; } @@ -234,10 +235,10 @@ class Orientation3DManifold : public fuse_core::Manifold bool Minus(const double* y, const double* x, double* y_minus_x) const override { double x_inverse[4]; - QuaternionInverse(x, x_inverse); + QuaternionInverse(x, static_cast(x_inverse)); double q_delta[4]; - ceres::QuaternionProduct(x_inverse, y, q_delta); - ceres::QuaternionToAngleAxis(q_delta, y_minus_x); + ceres::QuaternionProduct(static_cast(x_inverse), y, static_cast(q_delta)); + ceres::QuaternionToAngleAxis(static_cast(q_delta), static_cast(y_minus_x)); return true; } @@ -445,7 +446,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * @return A pointer to a local parameterization object that indicates how to "add" increments to * the quaternion */ - fuse_core::LocalParameterization* localParameterization() const override; + [[nodiscard]] std::unique_ptr localParameterization() const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -453,7 +454,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * * @return A pointer to a manifold object that indicates how to "add" increments to the quaternion */ - fuse_core::Manifold* manifold() const override; + [[nodiscard]] std::unique_ptr manifold() const override; #endif private: @@ -470,7 +471,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/pinhole_camera.hpp b/fuse_variables/include/fuse_variables/pinhole_camera.hpp index c74f32e3c..f2209f49d 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera.hpp +++ b/fuse_variables/include/fuse_variables/pinhole_camera.hpp @@ -181,7 +181,7 @@ class PinholeCamera : public FixedSizeVariable<4> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -212,7 +212,7 @@ class PinholeCamera : public FixedSizeVariable<4> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp index bebfed26d..a4b0c3491 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp +++ b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp @@ -91,7 +91,7 @@ class PinholeCameraFixed : public PinholeCamera * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp index 623af71ac..c2b72f8a2 100644 --- a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp @@ -139,7 +139,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -160,7 +160,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp index 292d4b372..2b2924b69 100644 --- a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp @@ -134,7 +134,7 @@ class Point2DLandmark : public FixedSizeVariable<2> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -155,7 +155,7 @@ class Point2DLandmark : public FixedSizeVariable<2> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp index 699eb95fa..75e90bb71 100644 --- a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp @@ -156,7 +156,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -177,7 +177,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp index 4cd048e0e..dd0dad5b4 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp @@ -154,7 +154,7 @@ class Point3DLandmark : public FixedSizeVariable<3> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -175,7 +175,7 @@ class Point3DLandmark : public FixedSizeVariable<3> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp index af349ce34..f9dc76f48 100644 --- a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp @@ -135,7 +135,7 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -155,7 +155,7 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp index 0e7bfd09a..e67341a79 100644 --- a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp @@ -151,7 +151,7 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -171,7 +171,7 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/stamped.hpp b/fuse_variables/include/fuse_variables/stamped.hpp index 4f2da197d..98f186b72 100644 --- a/fuse_variables/include/fuse_variables/stamped.hpp +++ b/fuse_variables/include/fuse_variables/stamped.hpp @@ -76,6 +76,10 @@ class Stamped * @brief Destructor */ virtual ~Stamped() = default; + Stamped(Stamped const&) = default; + Stamped(Stamped&&) = default; + Stamped& operator=(Stamped const&) = default; + Stamped& operator=(Stamped&&) = default; /** * @brief Read-only access to the associated timestamp. @@ -94,7 +98,7 @@ class Stamped } private: - fuse_core::UUID device_id_; //!< The UUID associated with this specific device or hardware + fuse_core::UUID device_id_{}; //!< The UUID associated with this specific device or hardware rclcpp::Time stamp_{ 0, 0, RCL_ROS_TIME }; //!< The timestamp associated with this variable //!< instance diff --git a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp index 1050bb7c1..2e95e8f9b 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp @@ -117,7 +117,7 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -137,7 +137,7 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp index 000207d9f..77ee7045c 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp @@ -151,7 +151,7 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -171,7 +171,7 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp index 9565bf33f..3e260132e 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp @@ -135,7 +135,7 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -155,7 +155,7 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp index 9c0c8b35b..7a3b51b74 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp @@ -152,7 +152,7 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] std::unique_ptr manifold() const override { return nullptr; } @@ -172,7 +172,7 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp index 7b2f2d7a0..79af1f49d 100644 --- a/fuse_variables/src/orientation_2d_stamped.cpp +++ b/fuse_variables/src/orientation_2d_stamped.cpp @@ -31,6 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include @@ -62,15 +63,15 @@ void Orientation2DStamped::print(std::ostream& stream) const << " - yaw: " << yaw() << "\n"; } -fuse_core::LocalParameterization* Orientation2DStamped::localParameterization() const +std::unique_ptr Orientation2DStamped::localParameterization() const { - return new Orientation2DLocalParameterization(); + return std::unique_ptr(new Orientation2DLocalParameterization()); } #if CERES_SUPPORTS_MANIFOLDS -fuse_core::Manifold* Orientation2DStamped::manifold() const +[[nodiscard]] std::unique_ptr Orientation2DStamped::manifold() const { - return new Orientation2DManifold(); + return std::unique_ptr(new Orientation2DManifold()); } #endif diff --git a/fuse_variables/src/orientation_3d_stamped.cpp b/fuse_variables/src/orientation_3d_stamped.cpp index b72f73a19..175c6b268 100644 --- a/fuse_variables/src/orientation_3d_stamped.cpp +++ b/fuse_variables/src/orientation_3d_stamped.cpp @@ -65,15 +65,15 @@ void Orientation3DStamped::print(std::ostream& stream) const << " - z: " << z() << "\n"; } -fuse_core::LocalParameterization* Orientation3DStamped::localParameterization() const +std::unique_ptr Orientation3DStamped::localParameterization() const { - return new Orientation3DLocalParameterization(); + return std::unique_ptr(new Orientation3DLocalParameterization()); } #if CERES_SUPPORTS_MANIFOLDS -fuse_core::Manifold* Orientation3DStamped::manifold() const +[[nodiscard]] std::unique_ptr Orientation3DStamped::manifold() const { - return new Orientation3DManifold(); + return std::unique_ptr(new Orientation3DManifold()); } #endif diff --git a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp index f595a98f8..0f6042ea3 100644 --- a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp @@ -123,7 +123,7 @@ TEST(AccelerationAngular2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(acceleration.data(), acceleration.size()); + problem.AddParameterBlock(acceleration.data(), static_cast(acceleration.size())); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp index 3f6a40061..c1fe6232b 100644 --- a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp @@ -127,7 +127,7 @@ TEST(AccelerationAngular3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(acceleration.data(), acceleration.size()); + problem.AddParameterBlock(acceleration.data(), static_cast(acceleration.size())); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp index 849a273fd..26370eebf 100644 --- a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp @@ -125,7 +125,7 @@ TEST(AccelerationLinear2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(acceleration.data(), acceleration.size()); + problem.AddParameterBlock(acceleration.data(), static_cast(acceleration.size())); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp index a67b3e8b5..2b8d0c6ce 100644 --- a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp @@ -127,7 +127,7 @@ TEST(AccelerationLinear3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(acceleration.data(), acceleration.size()); + problem.AddParameterBlock(acceleration.data(), static_cast(acceleration.size())); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_fixed_size_variable.cpp b/fuse_variables/test/test_fixed_size_variable.cpp index 239f702d3..dd358a91e 100644 --- a/fuse_variables/test/test_fixed_size_variable.cpp +++ b/fuse_variables/test/test_fixed_size_variable.cpp @@ -48,6 +48,10 @@ class TestVariable : public fuse_variables::FixedSizeVariable<2> { } virtual ~TestVariable() = default; + TestVariable(TestVariable const&) = default; + TestVariable(TestVariable&&) = default; + TestVariable& operator=(TestVariable const&) = default; + TestVariable& operator=(TestVariable&&) = default; void print(std::ostream& /*stream = std::cout*/) const override { @@ -75,8 +79,8 @@ TEST(FixedSizeVariable, Size) { // Verify the expected size is returned TestVariable variable; - EXPECT_EQ(2u, variable.size()); // base class interface - EXPECT_EQ(2u, TestVariable::SIZE); // static member variable + EXPECT_EQ(2u, variable.size()); // base class interface + EXPECT_EQ(2u, TestVariable::varSize); // static member variable } TEST(FixedSizeVariable, Data) diff --git a/fuse_variables/test/test_orientation_2d_stamped.cpp b/fuse_variables/test/test_orientation_2d_stamped.cpp index a20ff82f0..c9f6f1ec7 100644 --- a/fuse_variables/test/test_orientation_2d_stamped.cpp +++ b/fuse_variables/test/test_orientation_2d_stamped.cpp @@ -134,7 +134,8 @@ TEST(Orientation2DStamped, Plus) double x[1] = { 1.0 }; double delta[1] = { 0.5 }; double actual[1] = { 0.0 }; - bool success = parameterization->Plus(x, delta, actual); + bool success = + parameterization->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(1.5, actual[0], 1.0e-5); @@ -145,13 +146,12 @@ TEST(Orientation2DStamped, Plus) double x[1] = { 2.0 }; double delta[1] = { 3.0 }; double actual[1] = { 0.0 }; - bool success = parameterization->Plus(x, delta, actual); + bool success = + parameterization->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(5 - 2 * M_PI, actual[0], 1.0e-5); } - - delete parameterization; } TEST(Orientation2DStamped, PlusJacobian) @@ -164,16 +164,14 @@ TEST(Orientation2DStamped, PlusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = parameterization->ComputeJacobian(x, actual); + bool success = parameterization->ComputeJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; - reference.ComputeJacobian(x, expected); + reference.ComputeJacobian(static_cast(x), static_cast(expected)); EXPECT_TRUE(success); EXPECT_NEAR(expected[0], actual[0], 1.0e-5); } - - delete parameterization; } TEST(Orientation2DStamped, Minus) @@ -185,7 +183,8 @@ TEST(Orientation2DStamped, Minus) double x1[1] = { 1.0 }; double x2[1] = { 1.5 }; double actual[1] = { 0.0 }; - bool success = parameterization->Minus(x1, x2, actual); + bool success = + parameterization->Minus(static_cast(x1), static_cast(x2), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(0.5, actual[0], 1.0e-5); @@ -196,7 +195,8 @@ TEST(Orientation2DStamped, Minus) double x1[1] = { 2.0 }; double x2[1] = { 5 - 2 * M_PI }; double actual[1] = { 0.0 }; - bool success = parameterization->Minus(x1, x2, actual); + bool success = + parameterization->Minus(static_cast(x1), static_cast(x2), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(3.0, actual[0], 1.0e-5); @@ -213,16 +213,14 @@ TEST(Orientation2DStamped, MinusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = parameterization->ComputeMinusJacobian(x, actual); + bool success = parameterization->ComputeMinusJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; - reference.ComputeMinusJacobian(x, expected); + reference.ComputeMinusJacobian(static_cast(x), static_cast(expected)); EXPECT_TRUE(success); EXPECT_NEAR(expected[0], actual[0], 1.0e-5); } - - delete parameterization; } struct CostFunctor @@ -253,7 +251,7 @@ TEST(Orientation2DStamped, Optimization) #if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.localParameterization()); #else - problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.manifold()); + problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold().get()); #endif std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); @@ -300,6 +298,7 @@ TEST(Orientation2DStamped, Serialization) struct Orientation2DFunctor { template + // NOLINTNEXTLINE bool Plus(const T* x, const T* delta, T* x_plus_delta) const { x_plus_delta[0] = fuse_core::wrapAngle2D(x[0] + delta[0]); @@ -307,6 +306,7 @@ struct Orientation2DFunctor } template + // NOLINTNEXTLINE bool Minus(const T* y, const T* x, T* y_minus_x) const { y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); @@ -325,7 +325,7 @@ TEST(Orientation2DStamped, ManifoldPlus) double x[1] = { 1.0 }; double delta[1] = { 0.5 }; double actual[1] = { 0.0 }; - bool success = manifold->Plus(x, delta, actual); + bool success = manifold->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(1.5, actual[0], 1.0e-5); @@ -336,13 +336,11 @@ TEST(Orientation2DStamped, ManifoldPlus) double x[1] = { 2.0 }; double delta[1] = { 3.0 }; double actual[1] = { 0.0 }; - bool success = manifold->Plus(x, delta, actual); + bool success = manifold->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(5 - 2 * M_PI, actual[0], 1.0e-5); } - - delete manifold; } TEST(Orientation2DStamped, ManifoldPlusJacobian) @@ -355,16 +353,14 @@ TEST(Orientation2DStamped, ManifoldPlusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = manifold->PlusJacobian(x, actual); + bool success = manifold->PlusJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; - reference.PlusJacobian(x, expected); + reference.PlusJacobian(static_cast(x), static_cast(expected)); EXPECT_TRUE(success); EXPECT_NEAR(expected[0], actual[0], 1.0e-5); } - - delete manifold; } TEST(Orientation2DStamped, ManifoldMinus) @@ -376,7 +372,7 @@ TEST(Orientation2DStamped, ManifoldMinus) double x1[1] = { 1.0 }; double x2[1] = { 1.5 }; double actual[1] = { 0.0 }; - bool success = manifold->Minus(x2, x1, actual); + bool success = manifold->Minus(static_cast(x2), static_cast(x1), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(0.5, actual[0], 1.0e-5); @@ -387,7 +383,7 @@ TEST(Orientation2DStamped, ManifoldMinus) double x1[1] = { 2.0 }; double x2[1] = { 5 - 2 * M_PI }; double actual[1] = { 0.0 }; - bool success = manifold->Minus(x2, x1, actual); + bool success = manifold->Minus(static_cast(x2), static_cast(x1), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(3.0, actual[0], 1.0e-5); @@ -404,7 +400,7 @@ TEST(Orientation2DStamped, ManifoldMinusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = manifold->MinusJacobian(x, actual); + bool success = manifold->MinusJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; reference.MinusJacobian(x, expected); @@ -412,7 +408,5 @@ TEST(Orientation2DStamped, ManifoldMinusJacobian) EXPECT_TRUE(success); EXPECT_NEAR(expected[0], actual[0], 1.0e-5); } - - delete manifold; } #endif diff --git a/fuse_variables/test/test_orientation_3d_stamped.cpp b/fuse_variables/test/test_orientation_3d_stamped.cpp index c50926b28..db3643387 100644 --- a/fuse_variables/test/test_orientation_3d_stamped.cpp +++ b/fuse_variables/test/test_orientation_3d_stamped.cpp @@ -162,8 +162,6 @@ TEST(Orientation3DStamped, Plus) EXPECT_NEAR(0.360184, result[1], 1.0e-5); EXPECT_NEAR(0.194124, result[2], 1.0e-5); EXPECT_NEAR(0.526043, result[3], 1.0e-5); - - delete parameterization; } TEST(Orientation3DStamped, Minus) @@ -179,8 +177,6 @@ TEST(Orientation3DStamped, Minus) EXPECT_NEAR(0.15, result[0], 1.0e-5); EXPECT_NEAR(-0.2, result[1], 1.0e-5); EXPECT_NEAR(0.433012702, result[2], 1.0e-5); - - delete parameterization; } TEST(Orientation3DStamped, PlusJacobian) @@ -220,8 +216,6 @@ TEST(Orientation3DStamped, PlusJacobian) } } } - - delete parameterization; } TEST(Orientation3DStamped, MinusJacobian) @@ -261,8 +255,6 @@ TEST(Orientation3DStamped, MinusJacobian) } } } - - delete parameterization; } TEST(Orientation3DStamped, Stamped) @@ -282,7 +274,7 @@ TEST(Orientation3DStamped, Stamped) struct QuaternionCostFunction { - explicit QuaternionCostFunction(double* observation) + explicit QuaternionCostFunction(double const* observation) { observation_[0] = observation[0]; observation_[1] = observation[1]; @@ -309,7 +301,7 @@ struct QuaternionCostFunction return true; } - double observation_[4]; + double observation_[4]{}; }; TEST(Orientation3DStamped, Optimization) @@ -331,7 +323,7 @@ TEST(Orientation3DStamped, Optimization) #if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.localParameterization()); #else - problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.manifold()); + problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold().get()); #endif std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); @@ -351,7 +343,7 @@ TEST(Orientation3DStamped, Optimization) TEST(Orientation3DStamped, Euler) { - const double RAD_TO_DEG = 180.0 / M_PI; + const double rad_to_deg = 180.0 / M_PI; // Create an Orientation3DStamped with R, P, Y values of 10, -20, 30 degrees Orientation3DStamped orientation_r(rclcpp::Time(12345678, 910111213)); @@ -360,7 +352,7 @@ TEST(Orientation3DStamped, Euler) orientation_r.y() = 0.0; orientation_r.z() = 0.0; - EXPECT_NEAR(10.0, RAD_TO_DEG * orientation_r.roll(), 1e-5); + EXPECT_NEAR(10.0, rad_to_deg * orientation_r.roll(), 1e-5); Orientation3DStamped orientation_p(rclcpp::Time(12345678, 910111213)); orientation_p.w() = 0.9848078; @@ -368,7 +360,7 @@ TEST(Orientation3DStamped, Euler) orientation_p.y() = -0.1736482; orientation_p.z() = 0.0; - EXPECT_NEAR(-20.0, RAD_TO_DEG * orientation_p.pitch(), 1e-5); + EXPECT_NEAR(-20.0, rad_to_deg * orientation_p.pitch(), 1e-5); Orientation3DStamped orientation_y(rclcpp::Time(12345678, 910111213)); orientation_y.w() = 0.9659258; @@ -376,7 +368,7 @@ TEST(Orientation3DStamped, Euler) orientation_y.y() = 0.0; orientation_y.z() = 0.258819; - EXPECT_NEAR(30.0, RAD_TO_DEG * orientation_y.yaw(), 1e-5); + EXPECT_NEAR(30.0, rad_to_deg * orientation_y.yaw(), 1e-5); } TEST(Orientation3DStamped, Serialization) @@ -417,6 +409,7 @@ TEST(Orientation3DStamped, Serialization) struct Orientation3DFunctor { template + // NOLINTNEXTLINE bool Plus(const T* x, const T* delta, T* x_plus_delta) const { T q_delta[4]; @@ -425,6 +418,7 @@ struct Orientation3DFunctor return true; } template + // NOLINTNEXTLINE bool Minus(const T* y, const T* x, T* y_minus_x) const { T x_inverse[4]; @@ -452,8 +446,6 @@ TEST(Orientation3DStamped, ManifoldPlus) EXPECT_NEAR(0.360184, result[1], 1.0e-5); EXPECT_NEAR(0.194124, result[2], 1.0e-5); EXPECT_NEAR(0.526043, result[3], 1.0e-5); - - delete manifold; } TEST(Orientation3DStamped, ManifoldPlusJacobian) @@ -493,8 +485,6 @@ TEST(Orientation3DStamped, ManifoldPlusJacobian) } } } - - delete manifold; } TEST(Orientation3DStamped, ManifoldMinus) @@ -510,8 +500,6 @@ TEST(Orientation3DStamped, ManifoldMinus) EXPECT_NEAR(0.15, result[0], 1.0e-5); EXPECT_NEAR(-0.2, result[1], 1.0e-5); EXPECT_NEAR(0.433012702, result[2], 1.0e-5); - - delete manifold; } TEST(Orientation3DStamped, ManifoldMinusJacobian) @@ -551,8 +539,6 @@ TEST(Orientation3DStamped, ManifoldMinusJacobian) } } } - - delete manifold; } TEST(Orientation3DStamped, ManifoldSerialization) diff --git a/fuse_variables/test/test_point_2d_fixed_landmark.cpp b/fuse_variables/test/test_point_2d_fixed_landmark.cpp index 45b13ffd7..8ad4b7050 100644 --- a/fuse_variables/test/test_point_2d_fixed_landmark.cpp +++ b/fuse_variables/test/test_point_2d_fixed_landmark.cpp @@ -96,7 +96,7 @@ TEST(Point2DFixedLandmark, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_point_2d_landmark.cpp b/fuse_variables/test/test_point_2d_landmark.cpp index 0615470dd..8428aab0c 100644 --- a/fuse_variables/test/test_point_2d_landmark.cpp +++ b/fuse_variables/test/test_point_2d_landmark.cpp @@ -96,7 +96,7 @@ TEST(Point2DLandmark, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_point_3d_fixed_landmark.cpp b/fuse_variables/test/test_point_3d_fixed_landmark.cpp index 5bdf54a46..e7fffa9a9 100644 --- a/fuse_variables/test/test_point_3d_fixed_landmark.cpp +++ b/fuse_variables/test/test_point_3d_fixed_landmark.cpp @@ -98,7 +98,7 @@ TEST(Point3DFixedLandmark, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_point_3d_landmark.cpp b/fuse_variables/test/test_point_3d_landmark.cpp index 055b25331..fd5afed7d 100644 --- a/fuse_variables/test/test_point_3d_landmark.cpp +++ b/fuse_variables/test/test_point_3d_landmark.cpp @@ -98,7 +98,7 @@ TEST(Point3DLandmark, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_position_2d_stamped.cpp b/fuse_variables/test/test_position_2d_stamped.cpp index ce35789db..0141c0062 100644 --- a/fuse_variables/test/test_position_2d_stamped.cpp +++ b/fuse_variables/test/test_position_2d_stamped.cpp @@ -125,7 +125,7 @@ TEST(Position2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_position_3d_stamped.cpp b/fuse_variables/test/test_position_3d_stamped.cpp index c70746b53..7d4d32a1c 100644 --- a/fuse_variables/test/test_position_3d_stamped.cpp +++ b/fuse_variables/test/test_position_3d_stamped.cpp @@ -147,7 +147,7 @@ TEST(Position3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp index 029af9778..dcdfe0cc9 100644 --- a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp @@ -123,7 +123,7 @@ TEST(VelocityAngular2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(velocity.data(), velocity.size()); + problem.AddParameterBlock(velocity.data(), static_cast(velocity.size())); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp index 5d269cb39..608ad1608 100644 --- a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp @@ -127,7 +127,7 @@ TEST(VelocityAngular3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(velocity.data(), velocity.size()); + problem.AddParameterBlock(velocity.data(), static_cast(velocity.size())); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp index ea8aef29e..57d51555d 100644 --- a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp @@ -125,7 +125,7 @@ TEST(VelocityLinear2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(velocity.data(), velocity.size()); + problem.AddParameterBlock(velocity.data(), static_cast(velocity.size())); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp index 7bcca429d..9e69250ba 100644 --- a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp @@ -127,7 +127,7 @@ TEST(VelocityLinear3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(velocity.data(), velocity.size()); + problem.AddParameterBlock(velocity.data(), static_cast(velocity.size())); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); From b19c96797eef80328e973e1313f612e42d13d91b Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 4 Nov 2024 18:45:31 +0000 Subject: [PATCH 08/39] undo unique ptr changes --- .clang-tidy | 1 + .../src/marginalize_variables.cpp | 81 +++++++++---------- .../test/test_marginalize_variables.cpp | 44 +++++----- fuse_core/include/fuse_core/graph.hpp | 2 +- fuse_core/include/fuse_core/variable.hpp | 9 +-- fuse_core/test/example_variable.hpp | 6 +- fuse_core/test/test_variable.cpp | 6 +- fuse_graphs/src/hash_graph.cpp | 48 ++++++----- .../acceleration_angular_2d_stamped.hpp | 2 +- .../acceleration_angular_3d_stamped.hpp | 2 +- .../acceleration_linear_2d_stamped.hpp | 2 +- .../acceleration_linear_3d_stamped.hpp | 2 +- .../fuse_variables/orientation_2d_stamped.hpp | 4 +- .../fuse_variables/orientation_3d_stamped.hpp | 4 +- .../include/fuse_variables/pinhole_camera.hpp | 2 +- .../fuse_variables/pinhole_camera_fixed.hpp | 2 +- .../point_2d_fixed_landmark.hpp | 2 +- .../fuse_variables/point_2d_landmark.hpp | 2 +- .../point_3d_fixed_landmark.hpp | 2 +- .../fuse_variables/point_3d_landmark.hpp | 2 +- .../fuse_variables/position_2d_stamped.hpp | 2 +- .../fuse_variables/position_3d_stamped.hpp | 2 +- .../velocity_angular_2d_stamped.hpp | 2 +- .../velocity_angular_3d_stamped.hpp | 2 +- .../velocity_linear_2d_stamped.hpp | 2 +- .../velocity_linear_3d_stamped.hpp | 2 +- fuse_variables/src/orientation_2d_stamped.cpp | 8 +- fuse_variables/src/orientation_3d_stamped.cpp | 8 +- .../test/test_orientation_2d_stamped.cpp | 10 +-- .../test/test_orientation_3d_stamped.cpp | 2 +- 30 files changed, 129 insertions(+), 136 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index d3a332ed9..b9ba1944d 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -14,6 +14,7 @@ Checks: -*, -cppcoreguidelines-avoid-c-arrays, -cppcoreguidelines-avoid-magic-numbers, -cppcoreguidelines-non-private-member-variables-in-classes, + -cppcoreguidelines-owning-memory, -cppcoreguidelines-pro-bounds-array-to-pointer-decay, -cppcoreguidelines-pro-bounds-constant-array-index, -cppcoreguidelines-pro-bounds-pointer-arithmetic, diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index f3fa47a32..b3b6e3cb5 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -35,9 +35,7 @@ #include #include -#include #include -#include #include #include #include @@ -110,19 +108,20 @@ UuidOrdering computeEliminationOrder(const std::vector& margina // Construct the CCOLAMD input structures auto recommended_size = - ccolamd_recommended(variable_constraints.size(), constraint_order.size(), variable_order.size()); - auto A = std::vector(recommended_size); + ccolamd_recommended(static_cast(variable_constraints.size()), static_cast(constraint_order.size()), + static_cast(variable_order.size())); + auto a = std::vector(recommended_size); auto p = std::vector(variable_order.size() + 1); // Use the VariableConstraints table to construct the A and p structures - auto A_iter = A.begin(); + auto a_iter = a.begin(); auto p_iter = p.begin(); *p_iter = 0; ++p_iter; for (unsigned int variable_index = 0u; variable_index < variable_order.size(); ++variable_index) { - A_iter = variable_constraints.getConstraints(variable_index, A_iter); - *p_iter = std::distance(A.begin(), A_iter); + a_iter = variable_constraints.getConstraints(variable_index, a_iter); + *p_iter = static_cast(std::distance(a.begin(), a_iter)); ++p_iter; } @@ -141,9 +140,9 @@ UuidOrdering computeEliminationOrder(const std::vector& margina int stats[CCOLAMD_STATS]; // Finally call CCOLAMD - auto success = ccolamd(constraint_order.size(), variable_order.size(), recommended_size, A.data(), p.data(), knobs, - stats, variable_groups.data()); - if (!success) + auto success = ccolamd(static_cast(constraint_order.size()), static_cast(variable_order.size()), + static_cast(recommended_size), a.data(), p.data(), knobs, stats, variable_groups.data()); + if (success == 0) { throw std::runtime_error("Failed to call CCOLAMD to generate the elimination order."); } @@ -279,7 +278,7 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G LinearTerm result; // Generate the cost function from the input constraint - auto cost_function = constraint.costFunction(); + auto* cost_function = constraint.costFunction(); size_t row_count = cost_function->num_residuals(); // Loop over the constraint's variables and do several things: @@ -308,9 +307,9 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G bool success = cost_function->Evaluate(variable_values.data(), result.b.data(), jacobians.data()); delete cost_function; success = success && result.b.array().isFinite().all(); - for (const auto& A : result.A) + for (const auto& a : result.A) { - success = success && A.array().isFinite().all(); + success = success && a.array().isFinite().all(); } if (!success) { @@ -364,20 +363,16 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G } else if (manifold) { - fuse_core::MatrixXd J(manifold->AmbientSize(), manifold->TangentSize()); - manifold->PlusJacobian(variable_values[index], J.data()); - jacobian *= J; - } - if (manifold) - { - delete manifold; + fuse_core::MatrixXd j(manifold->AmbientSize(), manifold->TangentSize()); + manifold->PlusJacobian(variable_values[index], j.data()); + jacobian *= j; } #endif } // Correct A and b for the effects of the loss function - auto loss_function = constraint.lossFunction(); - if (loss_function) + auto* loss_function = constraint.lossFunction(); + if (loss_function != nullptr) { double squared_norm = result.b.squaredNorm(); double rho[3]; @@ -390,8 +385,8 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G double alpha = 0.0; if ((squared_norm > 0.0) && (rho[2] > 0.0)) { - const double D = 1.0 + 2.0 * squared_norm * rho[2] / rho[1]; - alpha = 1.0 - std::sqrt(D); + const double d = 1.0 + 2.0 * squared_norm * rho[2] / rho[1]; + alpha = 1.0 - std::sqrt(d); } // Correct the Jacobians @@ -473,29 +468,29 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) } // Construct the Ab matrix - fuse_core::MatrixXd Ab = fuse_core::MatrixXd::Zero(row_offsets.back(), column_offsets.back() + 1u); + fuse_core::MatrixXd ab = fuse_core::MatrixXd::Zero(row_offsets.back(), column_offsets.back() + 1u); for (size_t term_index = 0ul; term_index < linear_terms.size(); ++term_index) { const auto& linear_term = linear_terms[term_index]; auto row_offset = row_offsets[term_index]; for (size_t i = 0ul; i < linear_term.variables.size(); ++i) { - const auto& A = linear_term.A[i]; + const auto& a = linear_term.A[i]; auto dense = index_to_dense[linear_term.variables[i]]; auto column_offset = column_offsets[dense]; - for (int row = 0; row < A.rows(); ++row) + for (int row = 0; row < a.rows(); ++row) { - for (int col = 0; col < A.cols(); ++col) + for (int col = 0; col < a.cols(); ++col) { - Ab(row_offset + row, column_offset + col) = A(row, col); + ab(row_offset + row, column_offset + col) = a(row, col); } } } const auto& b = linear_term.b; - int column_offset = column_offsets.back(); + int column_offset = static_cast(column_offsets.back()); for (int row = 0; row < b.rows(); ++row) { - Ab(row_offset + row, column_offset) = b(row); + ab(row_offset + row, column_offset) = b(row); } } @@ -508,13 +503,13 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) using MatrixType = fuse_core::MatrixXd; using HCoeffsType = Eigen::internal::plain_diag_type::type; using RowVectorType = Eigen::internal::plain_row_type::type; - auto rows = Ab.rows(); - auto cols = Ab.cols(); + auto rows = ab.rows(); + auto cols = ab.cols(); auto size = std::min(rows, cols); - auto hCoeffs = HCoeffsType(size); + auto h_coeffs = HCoeffsType(size); auto temp = RowVectorType(cols); - Eigen::internal::householder_qr_inplace_blocked::run(Ab, hCoeffs, 48, temp.data()); - Ab.triangularView().setZero(); // Zero out the below-diagonal elements + Eigen::internal::householder_qr_inplace_blocked::run(ab, h_coeffs, 48, temp.data()); + ab.triangularView().setZero(); // Zero out the below-diagonal elements } // Extract the marginal term from R (now stored in Ab) @@ -522,7 +517,7 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) // The remaining rows are the marginal on the remaining variables: P(y, z, ...) auto min_row = column_offsets[1]; // However, depending on the input, not all rows may be usable. - auto max_row = std::min(Ab.rows(), Ab.cols() - 1); // -1 for the included b vector + auto max_row = std::min(ab.rows(), ab.cols() - 1); // -1 for the included b vector auto marginal_rows = max_row - min_row; auto marginal_term = LinearTerm(); if (marginal_rows > 0) @@ -535,22 +530,22 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) { auto index = dense_to_index[dense]; marginal_term.variables.push_back(index); - fuse_core::MatrixXd A = fuse_core::MatrixXd::Zero(marginal_rows, index_to_cols[index]); + fuse_core::MatrixXd a = fuse_core::MatrixXd::Zero(marginal_rows, index_to_cols[index]); auto column_offset = column_offsets[dense]; - for (int row = 0; row < A.rows(); ++row) + for (int row = 0; row < a.rows(); ++row) { - for (int col = 0; col < A.cols(); ++col) + for (int col = 0; col < a.cols(); ++col) { - A(row, col) = Ab(min_row + row, column_offset + col); + a(row, col) = ab(min_row + row, column_offset + col); } } - marginal_term.A.push_back(std::move(A)); + marginal_term.A.push_back(std::move(a)); } marginal_term.b = fuse_core::VectorXd::Zero(marginal_rows); auto column_offset = column_offsets.back(); for (int row = 0; row < marginal_term.b.rows(); ++row) { - marginal_term.b(row) = Ab(min_row + row, column_offset); + marginal_term.b(row) = ab(min_row + row, column_offset); } } return marginal_term; diff --git a/fuse_constraints/test/test_marginalize_variables.cpp b/fuse_constraints/test/test_marginalize_variables.cpp index 2530a887b..bd74c0d5e 100644 --- a/fuse_constraints/test/test_marginalize_variables.cpp +++ b/fuse_constraints/test/test_marginalize_variables.cpp @@ -355,16 +355,16 @@ TEST(MarginalizeVariables, Linearize) auto actual = fuse_constraints::detail::linearize(*constraint, graph, elimination_order); // Define the expected values - fuse_core::MatrixXd expected_A0(3, 3); + fuse_core::MatrixXd expected_a0(3, 3); /* *INDENT-OFF* */ - expected_A0 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, 0.27712824947756498073, + expected_a0 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, 0.27712824947756498073, -0.65053818745824854020, -2.5352112979770691226e-07, 7.1505019336171038447e-08, 2.5546009342625186633e-07, -0.57735026918958520792; /* *INDENT-ON* */ - fuse_core::MatrixXd expected_A1(3, 3); + fuse_core::MatrixXd expected_a1(3, 3); /* *INDENT-OFF* */ - expected_A1 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, + expected_a1 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, 1.3069243487082160549e-07, 0.70710678118650927004, -2.5352115484711390536e-07, 1.6590414383954588118e-07, 2.0699913566568639567e-07, 0.57735026918958520792; /* *INDENT-ON* */ @@ -377,8 +377,8 @@ TEST(MarginalizeVariables, Linearize) EXPECT_EQ(3u, actual.variables[0]); EXPECT_EQ(1u, actual.variables[1]); - EXPECT_MATRIX_NEAR(expected_A0, actual.A[0], 1.0e-9); - EXPECT_MATRIX_NEAR(expected_A1, actual.A[1], 1.0e-9); + EXPECT_MATRIX_NEAR(expected_a0, actual.A[0], 1.0e-9); + EXPECT_MATRIX_NEAR(expected_a1, actual.A[1], 1.0e-9); EXPECT_MATRIX_NEAR(expected_b, actual.b, 1.0e-9); } @@ -387,13 +387,13 @@ TEST(MarginalizeVariables, MarginalizeNext) // Construct a couple of linear terms auto term1 = fuse_constraints::detail::LinearTerm(); term1.variables.push_back(1); - auto A1 = fuse_core::MatrixXd(3, 3); + auto a1 = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A1 << 0.99999999999999922284, 4.4999993911720714834e-08, -2.9999995598828377297e-08, -3.181980062078038074e-08, + a1 << 0.99999999999999922284, 4.4999993911720714834e-08, -2.9999995598828377297e-08, -3.181980062078038074e-08, 0.70710678118654701763, 1.0606600528428877794e-08, 1.7320505793505525105e-08, -8.6602525498080673572e-09, 0.57735026918962550901; /* *INDENT-ON* */ - term1.A.push_back(A1); + term1.A.push_back(a1); auto b1 = fuse_core::VectorXd(3); b1 << -2.9999995786018886696e-08, -4.2426400911723613558e-08, -5.1961516896187549911e-08; term1.b = b1; @@ -401,20 +401,20 @@ TEST(MarginalizeVariables, MarginalizeNext) auto term2 = fuse_constraints::detail::LinearTerm(); term2.variables.push_back(3); term2.variables.push_back(1); - auto A21 = fuse_core::MatrixXd(3, 3); + auto a21 = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A21 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, 1.3069243487082160549e-07, + a21 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, 1.3069243487082160549e-07, 0.70710678118650927004, -2.5352115484711390536e-07, 1.6590414383954588118e-07, 2.0699913566568639567e-07, 0.57735026918958520792; /* *INDENT-ON* */ - auto A22 = fuse_core::MatrixXd(3, 3); + auto a22 = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A22 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, 0.27712824947756498073, + a22 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, 0.27712824947756498073, -0.6505381874582485402, -2.5352112979770691226e-07, 7.1505019336171038447e-08, 2.5546009342625186633e-07, -0.57735026918958520792; /* *INDENT-ON* */ - term2.A.push_back(A21); - term2.A.push_back(A22); + term2.A.push_back(a21); + term2.A.push_back(a22); auto b2 = fuse_core::VectorXd(3); b2 << 7.1706607563166841896e-07, -4.0638046747479327072e-07, 2.1341989211309879704e-07; term2.b = b2; @@ -429,12 +429,12 @@ TEST(MarginalizeVariables, MarginalizeNext) // Define the expected marginal auto expected = fuse_constraints::detail::LinearTerm(); expected.variables.push_back(3); - auto A_expected = fuse_core::MatrixXd(3, 3); + auto a_expected = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A_expected << -0.686835139329528, 0.064384601986636, 0.000000153209328, 0.000000000000000, -0.509885650799691, + a_expected << -0.686835139329528, 0.064384601986636, 0.000000153209328, 0.000000000000000, -0.509885650799691, 0.000000079984512, 0.000000000000000, 0.000000000000000, 0.408248290463911; /* *INDENT-ON* */ - expected.A.push_back(A_expected); + expected.A.push_back(a_expected); auto b_expected = fuse_core::VectorXd(3); b_expected << -0.000000497197868, 0.000000315186479, 0.000000114168337; expected.b = b_expected; @@ -451,22 +451,22 @@ TEST(MarginalizeVariables, MarginalizeNext) TEST(MarginalizeVariables, MarginalizeVariables) { // Create variables - auto x1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(1.0)); + auto x1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(1, 0)); x1->w() = 0.927362; x1->x() = 0.1; x1->y() = 0.2; x1->z() = 0.3; - auto x2 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(2.0)); + auto x2 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(2, 0)); x2->w() = 0.848625; x2->x() = 0.13798; x2->y() = 0.175959; x2->z() = 0.479411; - auto x3 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3.0)); + auto x3 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3, 0)); x3->w() = 0.735597; x3->x() = 0.170384; x3->y() = 0.144808; x3->z() = 0.63945; - auto l1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3.5)); + auto l1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3, 500000000)); l1->w() = 0.803884; l1->x() = 0.304917; l1->y() = 0.268286; diff --git a/fuse_core/include/fuse_core/graph.hpp b/fuse_core/include/fuse_core/graph.hpp index 6e73ffc5d..d4ad20815 100644 --- a/fuse_core/include/fuse_core/graph.hpp +++ b/fuse_core/include/fuse_core/graph.hpp @@ -355,7 +355,7 @@ class Graph virtual void getCovariance(const std::vector>& covariance_requests, std::vector>& covariance_matrices, const ceres::Covariance::Options& options = ceres::Covariance::Options(), - const bool use_tangent_space = true) const = 0; + bool use_tangent_space = true) const = 0; /** * @brief Update the graph with the contents of a transaction diff --git a/fuse_core/include/fuse_core/variable.hpp b/fuse_core/include/fuse_core/variable.hpp index 0593ef5cc..9f05f76db 100644 --- a/fuse_core/include/fuse_core/variable.hpp +++ b/fuse_core/include/fuse_core/variable.hpp @@ -36,7 +36,6 @@ #include #include -#include #include #include @@ -335,7 +334,7 @@ class Variable * * @return A base pointer to an instance of a derived LocalParameterization */ - [[nodiscard]] virtual std::unique_ptr localParameterization() const + [[nodiscard]] virtual fuse_core::LocalParameterization* localParameterization() const { return nullptr; } @@ -358,19 +357,19 @@ class Variable * * @return A base pointer to an instance of a derived Manifold */ - [[nodiscard]] virtual std::unique_ptr manifold() const + [[nodiscard]] virtual fuse_core::Manifold* manifold() const { // To support legacy Variable classes that still implements the localParameterization() method, // construct a ManifoldAdapter object from the LocalParameterization pointer as the default // action. If the Variable has been updated to use the new Manifold classes, then the Variable // should override this method and return a pointer to the appropriate derived Manifold object. - auto local_parameterization = localParameterization(); + auto* local_parameterization = localParameterization(); if (local_parameterization == nullptr) { return nullptr; } - return std::unique_ptr(new fuse_core::ManifoldAdapter(local_parameterization.get())); + return new fuse_core::ManifoldAdapter(local_parameterization); } #endif diff --git a/fuse_core/test/example_variable.hpp b/fuse_core/test/example_variable.hpp index 0cb4c9454..4cf04aa8b 100644 --- a/fuse_core/test/example_variable.hpp +++ b/fuse_core/test/example_variable.hpp @@ -79,7 +79,7 @@ class ExampleVariable : public fuse_core::Variable * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -251,9 +251,9 @@ class LegacyVariable : public fuse_core::Variable * * @return A pointer to a local parameterization object that indicates how to "add" increments to the quaternion */ - [[nodiscard]] std::unique_ptr localParameterization() const override + [[nodiscard]] fuse_core::LocalParameterization* localParameterization() const override { - return std::unique_ptr(new LegacyParameterization()); + return new LegacyParameterization(); } private: diff --git a/fuse_core/test/test_variable.cpp b/fuse_core/test/test_variable.cpp index 7c558bd13..7af36796a 100644 --- a/fuse_core/test/test_variable.cpp +++ b/fuse_core/test/test_variable.cpp @@ -128,7 +128,7 @@ TEST(LegacyVariable, ManifoldAdapter) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold().get()); + problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold()); std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -179,10 +179,10 @@ TEST(LegacyVariable, Deserialization) // Test the manifold interface, and that the Legacy LocalParameterization is wrapped // in a ManifoldAdapter - std::unique_ptr actual_manifold = nullptr; + fuse_core::Manifold* actual_manifold = nullptr; ASSERT_NO_THROW(actual_manifold = actual.manifold()); ASSERT_NE(actual_manifold, nullptr); - auto* actual_manifold_adapter = dynamic_cast(actual_manifold.get()); + auto* actual_manifold_adapter = dynamic_cast(actual_manifold); ASSERT_NE(actual_manifold_adapter, nullptr); } #endif diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index 14e385447..3a3c91534 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -63,13 +63,13 @@ HashGraph::HashGraph(const HashGraph& other) { // Make a deep copy of the constraints std::transform(other.constraints_.begin(), other.constraints_.end(), std::inserter(constraints_, constraints_.end()), - [](const Constraints::value_type& uuid__constraint) -> Constraints::value_type { - return { uuid__constraint.first, uuid__constraint.second->clone() }; + [](const Constraints::value_type& uuid_constraint) -> Constraints::value_type { + return { uuid_constraint.first, uuid_constraint.second->clone() }; }); // NOLINT(whitespace/braces) // Make a deep copy of the variables std::transform(other.variables_.begin(), other.variables_.end(), std::inserter(variables_, variables_.end()), - [](const Variables::value_type& uuid__variable) -> Variables::value_type { - return { uuid__variable.first, uuid__variable.second->clone() }; + [](const Variables::value_type& uuid_variable) -> Variables::value_type { + return { uuid_variable.first, uuid_variable.second->clone() }; }); // NOLINT(whitespace/braces) } @@ -165,9 +165,9 @@ const fuse_core::Constraint& HashGraph::getConstraint(const fuse_core::UUID& con fuse_core::Graph::const_constraint_range HashGraph::getConstraints() const noexcept { - std::function to_constraint_ref = - [](const Constraints::value_type& uuid__constraint) -> const fuse_core::Constraint& { - return *uuid__constraint.second; + std::function to_constraint_ref = + [](const Constraints::value_type& uuid_constraint) -> const fuse_core::Constraint& { + return *uuid_constraint.second; }; return fuse_core::Graph::const_constraint_range( @@ -190,19 +190,17 @@ fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(cons boost::make_transform_iterator(constraints.cbegin(), uuid_to_constraint_ref), boost::make_transform_iterator(constraints.cend(), uuid_to_constraint_ref)); } - else if (variableExists(variable_uuid)) + if (variableExists(variable_uuid)) { // User requested a valid variable, but there are no attached constraints. Return an empty // range. - return fuse_core::Graph::const_constraint_range(); - } - else - { - // We only want to throw if the requested variable does not exist. - throw std::logic_error("Attempting to access constraints connected to variable (" + - fuse_core::uuid::to_string(variable_uuid) + - "), but that variable does not exist in this graph."); + return {}; } + + // We only want to throw if the requested variable does not exist. + throw std::logic_error("Attempting to access constraints connected to variable (" + + fuse_core::uuid::to_string(variable_uuid) + + "), but that variable does not exist in this graph."); } bool HashGraph::variableExists(const fuse_core::UUID& variable_uuid) const noexcept @@ -266,8 +264,8 @@ const fuse_core::Variable& HashGraph::getVariable(const fuse_core::UUID& variabl fuse_core::Graph::const_variable_range HashGraph::getVariables() const noexcept { - std::function to_variable_ref = - [](const Variables::value_type& uuid__variable) -> const fuse_core::Variable& { return *uuid__variable.second; }; + std::function to_variable_ref = + [](const Variables::value_type& uuid_variable) -> const fuse_core::Variable& { return *uuid_variable.second; }; return fuse_core::Graph::const_variable_range(boost::make_transform_iterator(variables_.cbegin(), to_variable_ref), boost::make_transform_iterator(variables_.cend(), to_variable_ref)); @@ -460,10 +458,10 @@ void HashGraph::print(std::ostream& stream) const void HashGraph::createProblem(ceres::Problem& problem) const { // Add all the variables to the problem - for (auto& uuid__variable : variables_) + for (auto const& uuid_variable : variables_) { - fuse_core::Variable& variable = *(uuid__variable.second); - problem.AddParameterBlock(variable.data(), variable.size(), + fuse_core::Variable& variable = *(uuid_variable.second); + problem.AddParameterBlock(variable.data(), static_cast(variable.size()), #if !CERES_SUPPORTS_MANIFOLDS variable.localParameterization()); #else @@ -475,12 +473,12 @@ void HashGraph::createProblem(ceres::Problem& problem) const auto lower_bound = variable.lowerBound(index); if (lower_bound > std::numeric_limits::lowest()) { - problem.SetParameterLowerBound(variable.data(), index, lower_bound); + problem.SetParameterLowerBound(variable.data(), static_cast(index), lower_bound); } auto upper_bound = variable.upperBound(index); if (upper_bound < std::numeric_limits::max()) { - problem.SetParameterUpperBound(variable.data(), index, upper_bound); + problem.SetParameterUpperBound(variable.data(), static_cast(index), upper_bound); } } // Handle variables that are held constant @@ -491,9 +489,9 @@ void HashGraph::createProblem(ceres::Problem& problem) const } // Add the constraints std::vector parameter_blocks; - for (auto& uuid__constraint : constraints_) + for (auto const& uuid_constraint : constraints_) { - fuse_core::Constraint& constraint = *(uuid__constraint.second); + fuse_core::Constraint& constraint = *(uuid_constraint.second); // We need the memory address of each variable value referenced by this constraint parameter_blocks.clear(); parameter_blocks.reserve(constraint.variables().size()); diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp index 59d5a1b20..c1601dac4 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp @@ -118,7 +118,7 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp index 6bd41e183..39abe9994 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp @@ -152,7 +152,7 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp index 693a35d23..86b6a7659 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp @@ -135,7 +135,7 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp index 9f8c0f900..e5147accd 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp @@ -152,7 +152,7 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp index 14371edfc..871f33ce7 100644 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp @@ -266,7 +266,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * * @return A base pointer to an instance of a derived LocalParameterization */ - [[nodiscard]] std::unique_ptr localParameterization() const override; + [[nodiscard]] fuse_core::LocalParameterization* localParameterization() const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -277,7 +277,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * * @return A base pointer to an instance of a derived manifold */ - [[nodiscard]] std::unique_ptr manifold() const override; + [[nodiscard]] fuse_core::Manifold* manifold() const override; #endif private: diff --git a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp index 2f108854c..ceae3a676 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp @@ -446,7 +446,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * @return A pointer to a local parameterization object that indicates how to "add" increments to * the quaternion */ - [[nodiscard]] std::unique_ptr localParameterization() const override; + [[nodiscard]] fuse_core::LocalParameterization* localParameterization() const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -454,7 +454,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * * @return A pointer to a manifold object that indicates how to "add" increments to the quaternion */ - [[nodiscard]] std::unique_ptr manifold() const override; + [[nodiscard]] fuse_core::Manifold* manifold() const override; #endif private: diff --git a/fuse_variables/include/fuse_variables/pinhole_camera.hpp b/fuse_variables/include/fuse_variables/pinhole_camera.hpp index f2209f49d..854460661 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera.hpp +++ b/fuse_variables/include/fuse_variables/pinhole_camera.hpp @@ -181,7 +181,7 @@ class PinholeCamera : public FixedSizeVariable<4> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp index a4b0c3491..ee5a53d7a 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp +++ b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp @@ -91,7 +91,7 @@ class PinholeCameraFixed : public PinholeCamera * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp index c2b72f8a2..c097e9e74 100644 --- a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp @@ -139,7 +139,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp index 2b2924b69..558fc8e74 100644 --- a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp @@ -134,7 +134,7 @@ class Point2DLandmark : public FixedSizeVariable<2> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp index 75e90bb71..ca2eaa86f 100644 --- a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp @@ -156,7 +156,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp index dd0dad5b4..630e51e4a 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp @@ -154,7 +154,7 @@ class Point3DLandmark : public FixedSizeVariable<3> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp index f9dc76f48..55459d899 100644 --- a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp @@ -135,7 +135,7 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp index e67341a79..faa614f9d 100644 --- a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp @@ -151,7 +151,7 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp index 2e95e8f9b..4b32ca169 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp @@ -117,7 +117,7 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp index 77ee7045c..48a79e6bd 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp @@ -151,7 +151,7 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp index 3e260132e..d70b9d05d 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp @@ -135,7 +135,7 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp index 7a3b51b74..23bf71587 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp @@ -152,7 +152,7 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - [[nodiscard]] std::unique_ptr manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp index 79af1f49d..4f01dffb7 100644 --- a/fuse_variables/src/orientation_2d_stamped.cpp +++ b/fuse_variables/src/orientation_2d_stamped.cpp @@ -63,15 +63,15 @@ void Orientation2DStamped::print(std::ostream& stream) const << " - yaw: " << yaw() << "\n"; } -std::unique_ptr Orientation2DStamped::localParameterization() const +fuse_core::LocalParameterization* Orientation2DStamped::localParameterization() const { - return std::unique_ptr(new Orientation2DLocalParameterization()); + return new Orientation2DLocalParameterization(); } #if CERES_SUPPORTS_MANIFOLDS -[[nodiscard]] std::unique_ptr Orientation2DStamped::manifold() const +[[nodiscard]] fuse_core::Manifold* Orientation2DStamped::manifold() const { - return std::unique_ptr(new Orientation2DManifold()); + return new Orientation2DManifold(); } #endif diff --git a/fuse_variables/src/orientation_3d_stamped.cpp b/fuse_variables/src/orientation_3d_stamped.cpp index 175c6b268..895ee1a4b 100644 --- a/fuse_variables/src/orientation_3d_stamped.cpp +++ b/fuse_variables/src/orientation_3d_stamped.cpp @@ -65,15 +65,15 @@ void Orientation3DStamped::print(std::ostream& stream) const << " - z: " << z() << "\n"; } -std::unique_ptr Orientation3DStamped::localParameterization() const +fuse_core::LocalParameterization* Orientation3DStamped::localParameterization() const { - return std::unique_ptr(new Orientation3DLocalParameterization()); + return new Orientation3DLocalParameterization(); } #if CERES_SUPPORTS_MANIFOLDS -[[nodiscard]] std::unique_ptr Orientation3DStamped::manifold() const +[[nodiscard]] fuse_core::Manifold* Orientation3DStamped::manifold() const { - return std::unique_ptr(new Orientation3DManifold()); + return new Orientation3DManifold(); } #endif diff --git a/fuse_variables/test/test_orientation_2d_stamped.cpp b/fuse_variables/test/test_orientation_2d_stamped.cpp index c9f6f1ec7..1389a1ff2 100644 --- a/fuse_variables/test/test_orientation_2d_stamped.cpp +++ b/fuse_variables/test/test_orientation_2d_stamped.cpp @@ -251,7 +251,7 @@ TEST(Orientation2DStamped, Optimization) #if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.localParameterization()); #else - problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold().get()); + problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold()); #endif std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); @@ -318,7 +318,7 @@ using Orientation2DManifold = ceres::AutoDiffManifold{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; @@ -365,7 +365,7 @@ TEST(Orientation2DStamped, ManifoldPlusJacobian) TEST(Orientation2DStamped, ManifoldMinus) { - auto manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); + auto* manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); // Simple test { @@ -392,7 +392,7 @@ TEST(Orientation2DStamped, ManifoldMinus) TEST(Orientation2DStamped, ManifoldMinusJacobian) { - auto manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); + auto* manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation2DManifold(); auto test_values = std::vector{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; diff --git a/fuse_variables/test/test_orientation_3d_stamped.cpp b/fuse_variables/test/test_orientation_3d_stamped.cpp index db3643387..740c04461 100644 --- a/fuse_variables/test/test_orientation_3d_stamped.cpp +++ b/fuse_variables/test/test_orientation_3d_stamped.cpp @@ -323,7 +323,7 @@ TEST(Orientation3DStamped, Optimization) #if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.localParameterization()); #else - problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold().get()); + problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold()); #endif std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); From 2239565d46286faee5910098a9aacdf25405a0f8 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 4 Nov 2024 21:32:46 +0000 Subject: [PATCH 09/39] april tag WIP --- .../include/fuse_models/april_tag_pose.hpp | 29 +--- .../parameters/april_tag_pose_params.hpp | 44 +++++- fuse_models/src/april_tag_pose.cpp | 148 ++++++++++++++++++ 3 files changed, 190 insertions(+), 31 deletions(-) diff --git a/fuse_models/include/fuse_models/april_tag_pose.hpp b/fuse_models/include/fuse_models/april_tag_pose.hpp index af0d3ba2d..1cb4efb38 100644 --- a/fuse_models/include/fuse_models/april_tag_pose.hpp +++ b/fuse_models/include/fuse_models/april_tag_pose.hpp @@ -56,15 +56,9 @@ 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 + * @brief An adapter-type sensor that produces pose constraints from AprilTag detections * - * This sensor subscribes to a MessageType 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. - * 2. Creates 3D velocity variables and constraints. + * This sensor subscribes to a MessageType topic and creates orientation and pose variables and constraints. * * This sensor really just separates out the orientation, angular velocity, and linear acceleration * components of the message, and processes them just like the Pose3D, Twist3D, and Acceleration3D @@ -76,23 +70,6 @@ namespace fuse_models * - device_name (string) Used to generate the device/robot ID if the device_id is not provided * - queue_size (int, default: 10) The subscriber queue size for the pose messages * - topic (string) The topic to which to subscribe for the pose messages - * - differential (bool, default: false) Whether we should fuse orientation measurements - * absolutely, or to create relative orientation constraints - * using consecutive measurements. - * - remove_gravitational_acceleration (bool, default: false) Whether we should remove acceleration - * due to gravity from the acceleration - * values produced by the IMU before - * fusing - * - gravitational_acceleration (double, default: 9.80665) Acceleration due to gravity, in - * meters/sec^2. This value is only used if - * \p remove_gravitational_acceleration is - * true - * - orientation_target_frame (string) Orientation data will be transformed into this frame before - * it is fused. - * - twist_target_frame (string) Twist/velocity data will be transformed into this frame before it - * is fused. - * - acceleration_target_frame (string) Acceleration data will be transformed into this frame - * before it is fused. * * Subscribes: * - \p topic (MessageType) IMU data at a given timestep @@ -165,8 +142,6 @@ class AprilTagPose : public fuse_core::AsyncSensorModel ParameterType params_; - tf2_msgs::msg::TFMessage previous_pose_; - std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; diff --git a/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp b/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp index f19fad302..e56f153da 100644 --- a/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp +++ b/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp @@ -35,20 +35,20 @@ #define FUSE_MODELS__PARAMETERS__APRIL_TAG_POSE_PARAMS_HPP_ #include +#include #include #include #include -#include #include -#include +#include namespace fuse_models::parameters { /** - * @brief Defines the set of parameters required by the Imu3D class + * @brief Defines the set of parameters required by the Odometry3D class */ struct AprilTagPoseParams : public ParameterBase { @@ -63,9 +63,45 @@ struct AprilTagPoseParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) override + const std::string& ns) { + position_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); + orientation_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + + disable_checks = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "throttle_period"), throttle_period, false); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); + + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + + pose_target_frame = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_target_frame"), pose_target_frame); + + pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + pose_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_covariance"), + std::array{ 1, 1, 1, 1, 1, 1 }); } + + bool disable_checks{ false }; + bool independent{ true }; + fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance matrix + rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become available + rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not + std::array pose_covariance; //!< The diagonal elements of the tag pose covariance + int queue_size{ 10 }; + std::string topic; + std::string pose_target_frame; + std::vector position_indices; + std::vector orientation_indices; + fuse_core::Loss::SharedPtr pose_loss; }; } // namespace fuse_models::parameters diff --git a/fuse_models/src/april_tag_pose.cpp b/fuse_models/src/april_tag_pose.cpp index 22a0df698..5c7640fd7 100644 --- a/fuse_models/src/april_tag_pose.cpp +++ b/fuse_models/src/april_tag_pose.cpp @@ -1 +1,149 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include +#include +#include #include +#include +#include +#include +#include +#include + +// Register this sensor model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::AprilTagPose, fuse_core::SensorModel) + +namespace fuse_models +{ + +AprilTagPose::AprilTagPose() + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&AprilTagPose::process, this, std::placeholders::_1)) +{ +} + +void AprilTagPose::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 AprilTagPose::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(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + throttled_callback_.setThrottlePeriod(params_.throttle_period); + + if (!params_.throttle_use_wall_time) + { + throttled_callback_.setClock(clock_); + } + + if (params_.position_indices.empty() && params_.orientation_indices.empty()) + { + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << 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 AprilTagPose::onStart() +{ + if (!params_.position_indices.empty() || !params_.orientation_indices.empty()) + { + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, params_.topic, params_.queue_size, + std::bind(&AprilTagThrottledCallback::callback, &throttled_callback_, + std::placeholders::_1), + sub_options); + } +} + +void AprilTagPose::onStop() +{ + sub_.reset(); +} + +void AprilTagPose::process(MessageType const& msg) +{ + for (auto const& transform : msg.transforms) + { + // Create a transaction object + auto transaction = fuse_core::Transaction::make_shared(); + transaction->stamp(transform.header.stamp); + + // Create the pose from the transform + auto pose = std::make_unique(); + pose->header = transform.header; + pose->pose.pose.orientation = transform.transform.rotation; + pose->pose.pose.position.x = transform.transform.translation.x; + pose->pose.pose.position.y = transform.transform.translation.y; + pose->pose.pose.position.z = transform.transform.translation.z; + + // TODO(henrygerardmoore): figure out better method to set the covariance + for (std::size_t i = 0; i < params_.pose_covariance.size(); ++i) + { + pose->pose.covariance[i * 7] = params_.pose_covariance[i]; + } + + const bool validate = !params_.disable_checks; + common::processAbsolutePose3DWithCovariance(name(), device_id_, *pose, params_.pose_loss, params_.pose_target_frame, + params_.position_indices, params_.orientation_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); + + // Send the transaction object to the plugin's parent + sendTransaction(transaction); + } +} + +} // namespace fuse_models From 9813f812b90af58799e36b51d05c0070f9e11c2d Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 4 Nov 2024 21:33:05 +0000 Subject: [PATCH 10/39] clang tidy fixes --- fuse_models/include/fuse_models/odometry_3d.hpp | 6 +++++- .../fuse_models/parameters/odometry_3d_params.hpp | 15 +++++---------- fuse_models/include/fuse_models/pose_2d.hpp | 6 +++++- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/fuse_models/include/fuse_models/odometry_3d.hpp b/fuse_models/include/fuse_models/odometry_3d.hpp index 6a8f6e552..bec0dfce7 100644 --- a/fuse_models/include/fuse_models/odometry_3d.hpp +++ b/fuse_models/include/fuse_models/odometry_3d.hpp @@ -102,6 +102,10 @@ class Odometry3D : public fuse_core::AsyncSensorModel * @brief Destructor */ virtual ~Odometry3D() = default; + Odometry3D(Odometry3D const&) = delete; + Odometry3D(Odometry3D&&) = delete; + Odometry3D& operator=(Odometry3D const&) = delete; + Odometry3D& operator=(Odometry3D&&) = delete; /** * @brief Shadowing extension to the AsyncSensorModel::initialize call @@ -148,7 +152,7 @@ class Odometry3D : public fuse_core::AsyncSensorModel * @param[out] transaction - The generated variables and constraints are added to this transaction */ void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, bool validate, fuse_core::Transaction& transaction); fuse_core::node_interfaces::NodeInterfaces #include -namespace fuse_models -{ - -namespace parameters +namespace fuse_models::parameters { /** @@ -129,9 +126,9 @@ struct Odometry3DParams : public ParameterBase //!< available rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not - std::string topic{}; - std::string pose_target_frame{}; - std::string twist_target_frame{}; + std::string topic; + std::string pose_target_frame; + std::string twist_target_frame; std::vector position_indices; std::vector orientation_indices; std::vector linear_velocity_indices; @@ -141,8 +138,6 @@ struct Odometry3DParams : public ParameterBase fuse_core::Loss::SharedPtr angular_velocity_loss; }; -} // namespace parameters - -} // namespace fuse_models +} // namespace fuse_models::parameters #endif // FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/pose_2d.hpp b/fuse_models/include/fuse_models/pose_2d.hpp index 154102025..c5bc02ae7 100644 --- a/fuse_models/include/fuse_models/pose_2d.hpp +++ b/fuse_models/include/fuse_models/pose_2d.hpp @@ -91,6 +91,10 @@ class Pose2D : public fuse_core::AsyncSensorModel * @brief Destructor */ virtual ~Pose2D() = default; + Pose2D(Pose2D const&) = delete; + Pose2D(Pose2D&&) = delete; + Pose2D& operator=(Pose2D const&) = delete; + Pose2D& operator=(Pose2D&&) = delete; /** * @brief Shadowing extension to the AsyncSensorModel::initialize call @@ -134,7 +138,7 @@ 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::msg::PoseWithCovarianceStamped& pose, const bool validate, + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, bool validate, fuse_core::Transaction& transaction); fuse_core::node_interfaces::NodeInterfaces Date: Mon, 4 Nov 2024 22:21:10 +0000 Subject: [PATCH 11/39] build fixes --- .../fuse_models/parameters/april_tag_pose_params.hpp | 4 ++-- fuse_models/src/april_tag_pose.cpp | 9 ++++----- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp b/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp index e56f153da..d7605e283 100644 --- a/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp +++ b/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp @@ -86,7 +86,7 @@ struct AprilTagPoseParams : public ParameterBase pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); pose_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_covariance"), - std::array{ 1, 1, 1, 1, 1, 1 }); + std::vector{ 1, 1, 1, 1, 1, 1 }); } bool disable_checks{ false }; @@ -95,7 +95,7 @@ struct AprilTagPoseParams : public ParameterBase rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become available rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not - std::array pose_covariance; //!< The diagonal elements of the tag pose covariance + std::vector pose_covariance; //!< The diagonal elements of the tag pose covariance int queue_size{ 10 }; std::string topic; std::string pose_target_frame; diff --git a/fuse_models/src/april_tag_pose.cpp b/fuse_models/src/april_tag_pose.cpp index 5c7640fd7..919f6e419 100644 --- a/fuse_models/src/april_tag_pose.cpp +++ b/fuse_models/src/april_tag_pose.cpp @@ -101,11 +101,10 @@ void AprilTagPose::onStart() rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; - sub_ = rclcpp::create_subscription( - interfaces_, params_.topic, params_.queue_size, - std::bind(&AprilTagThrottledCallback::callback, &throttled_callback_, - std::placeholders::_1), - sub_options); + sub_ = rclcpp::create_subscription(interfaces_, params_.topic, params_.queue_size, + std::bind(&AprilTagThrottledCallback::callback, + &throttled_callback_, std::placeholders::_1), + sub_options); } } From 639457418fc6483ca3cf6a835f6bbf2b7de1a2fa Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 4 Nov 2024 23:10:49 +0000 Subject: [PATCH 12/39] clang-tidy fixes --- .../src/marginalize_variables.cpp | 22 ++--- .../test/test_marginalize_variables.cpp | 8 +- fuse_core/src/timestamp_manager.cpp | 12 +-- fuse_core/src/transaction.cpp | 12 +-- fuse_core/test/test_variable.cpp | 4 +- fuse_graphs/src/hash_graph.cpp | 24 +++--- fuse_optimizers/src/fixed_lag_smoother.cpp | 18 ++-- .../test_acceleration_angular_2d_stamped.cpp | 28 +++---- .../test_acceleration_angular_3d_stamped.cpp | 29 ++++--- .../test_acceleration_linear_2d_stamped.cpp | 28 +++---- .../test_acceleration_linear_3d_stamped.cpp | 28 +++---- .../test/test_fixed_size_variable.cpp | 4 +- .../test/test_orientation_2d_stamped.cpp | 64 ++++++++------- .../test/test_orientation_3d_stamped.cpp | 82 +++++++++---------- .../test/test_point_2d_fixed_landmark.cpp | 16 ++-- .../test/test_point_2d_landmark.cpp | 16 ++-- .../test/test_point_3d_fixed_landmark.cpp | 16 ++-- .../test/test_point_3d_landmark.cpp | 16 ++-- .../test/test_position_2d_stamped.cpp | 28 +++---- .../test/test_position_3d_stamped.cpp | 38 ++++----- .../test/test_velocity_angular_2d_stamped.cpp | 28 +++---- .../test/test_velocity_angular_3d_stamped.cpp | 28 +++---- .../test/test_velocity_linear_2d_stamped.cpp | 28 +++---- .../test/test_velocity_linear_3d_stamped.cpp | 28 +++---- 24 files changed, 289 insertions(+), 316 deletions(-) diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index b3b6e3cb5..92cce1db8 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -97,7 +97,7 @@ UuidOrdering computeEliminationOrder(const std::vector& margina // New constraint and variable indices are automatically generated for (const auto& constraint : constraints) { - unsigned int constraint_index = constraint_order[constraint.uuid()]; + unsigned int const constraint_index = constraint_order[constraint.uuid()]; for (const auto& constraint_variable_uuid : constraint.variables()) { variable_constraints.insert(constraint_index, variable_order[constraint_variable_uuid]); @@ -279,7 +279,7 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G // Generate the cost function from the input constraint auto* cost_function = constraint.costFunction(); - size_t row_count = cost_function->num_residuals(); + size_t const row_count = cost_function->num_residuals(); // Loop over the constraint's variables and do several things: // * Generate a vector of variable value pointers. This is needed for the Ceres API. @@ -298,7 +298,7 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G const auto& variable = graph.getVariable(variable_uuid); variable_values.push_back(variable.data()); result.variables.push_back(elimination_order.at(variable_uuid)); - result.A.push_back(fuse_core::MatrixXd(row_count, variable.size())); + result.A.emplace_back(row_count, variable.size()); jacobians.push_back(result.A.back().data()); } result.b = fuse_core::VectorXd(row_count); @@ -351,17 +351,17 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G delete local_parameterization; } #else - auto manifold = variable.manifold(); + auto* manifold = variable.manifold(); auto& jacobian = result.A[index]; if (variable.holdConstant()) { - if (manifold) + if (manifold != nullptr) { jacobian.resize(Eigen::NoChange, manifold->TangentSize()); } jacobian.setZero(); } - else if (manifold) + else if (manifold != nullptr) { fuse_core::MatrixXd j(manifold->AmbientSize(), manifold->TangentSize()); manifold->PlusJacobian(variable_values[index], j.data()); @@ -374,14 +374,14 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G auto* loss_function = constraint.lossFunction(); if (loss_function != nullptr) { - double squared_norm = result.b.squaredNorm(); + double const squared_norm = result.b.squaredNorm(); double rho[3]; loss_function->Evaluate(squared_norm, rho); if (fuse_core::Loss::Ownership == ceres::Ownership::TAKE_OWNERSHIP) { delete loss_function; } - double sqrt_rho1 = std::sqrt(rho[1]); + double const sqrt_rho1 = std::sqrt(rho[1]); double alpha = 0.0; if ((squared_norm > 0.0) && (rho[2] > 0.0)) { @@ -462,9 +462,9 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) auto column_offsets = std::vector(); column_offsets.reserve(dense_to_index.size() + 1ul); column_offsets.push_back(0u); - for (size_t dense = 0; dense < dense_to_index.size(); ++dense) + for (unsigned int const dense : dense_to_index) { - column_offsets.push_back(column_offsets.back() + index_to_cols[dense_to_index[dense]]); + column_offsets.push_back(column_offsets.back() + index_to_cols[dense]); } // Construct the Ab matrix @@ -487,7 +487,7 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) } } const auto& b = linear_term.b; - int column_offset = static_cast(column_offsets.back()); + int const column_offset = static_cast(column_offsets.back()); for (int row = 0; row < b.rows(); ++row) { ab(row_offset + row, column_offset) = b(row); diff --git a/fuse_constraints/test/test_marginalize_variables.cpp b/fuse_constraints/test/test_marginalize_variables.cpp index bd74c0d5e..369b906a1 100644 --- a/fuse_constraints/test/test_marginalize_variables.cpp +++ b/fuse_constraints/test/test_marginalize_variables.cpp @@ -71,12 +71,12 @@ class GenericVariable : public fuse_core::Variable { } - size_t size() const override + [[nodiscard]] size_t size() const override { return 1; } - const double* data() const override + [[nodiscard]] const double* data() const override { return &data_; } @@ -138,7 +138,7 @@ class GenericConstraint : public fuse_core::Constraint { } - ceres::CostFunction* costFunction() const override + [[nodiscard]] ceres::CostFunction* costFunction() const override { return nullptr; } @@ -173,7 +173,7 @@ class FixedOrientation3DStamped : public fuse_variables::Orientation3DStamped { } - bool holdConstant() const override + [[nodiscard]] bool holdConstant() const override { return true; } diff --git a/fuse_core/src/timestamp_manager.cpp b/fuse_core/src/timestamp_manager.cpp index e512169ce..93c9297bf 100644 --- a/fuse_core/src/timestamp_manager.cpp +++ b/fuse_core/src/timestamp_manager.cpp @@ -155,11 +155,11 @@ void TimestampManager::query(Transaction& transaction, bool update_variables) TimestampManager::const_stamp_range TimestampManager::stamps() const { - std::function extract_stamp = + std::function const extract_stamp = [](const MotionModelHistory::value_type& element) -> const rclcpp::Time& { return element.first; }; - return const_stamp_range(boost::make_transform_iterator(motion_model_history_.begin(), extract_stamp), - boost::make_transform_iterator(motion_model_history_.end(), extract_stamp)); + return { boost::make_transform_iterator(motion_model_history_.begin(), extract_stamp), + boost::make_transform_iterator(motion_model_history_.end(), extract_stamp) }; } void TimestampManager::addSegment(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, @@ -203,8 +203,8 @@ void TimestampManager::removeSegment(MotionModelHistory::iterator& iter, Transac void TimestampManager::splitSegment(MotionModelHistory::iterator& iter, const rclcpp::Time& stamp, Transaction& transaction) { - rclcpp::Time removed_beginning_stamp = iter->second.beginning_stamp; - rclcpp::Time removed_ending_stamp = iter->second.ending_stamp; + rclcpp::Time const removed_beginning_stamp = iter->second.beginning_stamp; + rclcpp::Time const removed_ending_stamp = iter->second.ending_stamp; // We need to remove the existing constraint. removeSegment(iter, transaction); // And add a new constraint from the beginning of the removed constraint to the provided stamp @@ -226,7 +226,7 @@ void TimestampManager::purgeHistory() // (a) are left with only one entry, OR // (b) the time delta between the beginning and end is within the buffer_length_ We compare with // the ending timestamp of each segment to be conservative - rclcpp::Time ending_stamp = motion_model_history_.rbegin()->first; + rclcpp::Time const ending_stamp = motion_model_history_.rbegin()->first; while ((motion_model_history_.size() > 1) && ((ending_stamp - motion_model_history_.begin()->second.ending_stamp) > buffer_length_)) { diff --git a/fuse_core/src/transaction.cpp b/fuse_core/src/transaction.cpp index cebb04dc4..39b6a3293 100644 --- a/fuse_core/src/transaction.cpp +++ b/fuse_core/src/transaction.cpp @@ -68,11 +68,11 @@ void Transaction::addInvolvedStamp(const rclcpp::Time& stamp) Transaction::const_constraint_range Transaction::addedConstraints() const { - std::function to_constraint_ref = + std::function const to_constraint_ref = [](const Constraint::SharedPtr& constraint) -> const Constraint& { return *constraint; }; - return const_constraint_range(boost::make_transform_iterator(added_constraints_.cbegin(), to_constraint_ref), - boost::make_transform_iterator(added_constraints_.cend(), to_constraint_ref)); + return { boost::make_transform_iterator(added_constraints_.cbegin(), to_constraint_ref), + boost::make_transform_iterator(added_constraints_.cend(), to_constraint_ref) }; } void Transaction::addConstraint(Constraint::SharedPtr constraint, bool overwrite) @@ -126,11 +126,11 @@ void Transaction::removeConstraint(const UUID& constraint_uuid) Transaction::const_variable_range Transaction::addedVariables() const { - std::function to_variable_ref = + std::function const to_variable_ref = [](const Variable::SharedPtr& variable) -> const Variable& { return *variable; }; - return const_variable_range(boost::make_transform_iterator(added_variables_.cbegin(), to_variable_ref), - boost::make_transform_iterator(added_variables_.cend(), to_variable_ref)); + return { boost::make_transform_iterator(added_variables_.cbegin(), to_variable_ref), + boost::make_transform_iterator(added_variables_.cend(), to_variable_ref) }; } bool Transaction::empty() const diff --git a/fuse_core/test/test_variable.cpp b/fuse_core/test/test_variable.cpp index 7af36796a..d2eb4cc34 100644 --- a/fuse_core/test/test_variable.cpp +++ b/fuse_core/test/test_variable.cpp @@ -45,7 +45,7 @@ TEST(Variable, Type) { - ExampleVariable variable; + ExampleVariable const variable; ASSERT_EQ("ExampleVariable", variable.type()); } @@ -135,7 +135,7 @@ TEST(LegacyVariable, ManifoldAdapter) // Run the solver ceres::Solver::Summary summary; - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solve(options, &problem, &summary); // Check diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index 3a3c91534..b99ae0aeb 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -165,14 +165,13 @@ const fuse_core::Constraint& HashGraph::getConstraint(const fuse_core::UUID& con fuse_core::Graph::const_constraint_range HashGraph::getConstraints() const noexcept { - std::function to_constraint_ref = + std::function const to_constraint_ref = [](const Constraints::value_type& uuid_constraint) -> const fuse_core::Constraint& { return *uuid_constraint.second; }; - return fuse_core::Graph::const_constraint_range( - boost::make_transform_iterator(constraints_.cbegin(), to_constraint_ref), - boost::make_transform_iterator(constraints_.cend(), to_constraint_ref)); + return { boost::make_transform_iterator(constraints_.cbegin(), to_constraint_ref), + boost::make_transform_iterator(constraints_.cend(), to_constraint_ref) }; } fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(const fuse_core::UUID& variable_uuid) const @@ -180,15 +179,14 @@ fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(cons auto cross_reference_iter = constraints_by_variable_uuid_.find(variable_uuid); if (cross_reference_iter != constraints_by_variable_uuid_.end()) { - std::function uuid_to_constraint_ref = + std::function const uuid_to_constraint_ref = [this](const fuse_core::UUID& constraint_uuid) -> const fuse_core::Constraint& { return this->getConstraint(constraint_uuid); }; const auto& constraints = cross_reference_iter->second; - return fuse_core::Graph::const_constraint_range( - boost::make_transform_iterator(constraints.cbegin(), uuid_to_constraint_ref), - boost::make_transform_iterator(constraints.cend(), uuid_to_constraint_ref)); + return { boost::make_transform_iterator(constraints.cbegin(), uuid_to_constraint_ref), + boost::make_transform_iterator(constraints.cend(), uuid_to_constraint_ref) }; } if (variableExists(variable_uuid)) { @@ -264,11 +262,11 @@ const fuse_core::Variable& HashGraph::getVariable(const fuse_core::UUID& variabl fuse_core::Graph::const_variable_range HashGraph::getVariables() const noexcept { - std::function to_variable_ref = + std::function const to_variable_ref = [](const Variables::value_type& uuid_variable) -> const fuse_core::Variable& { return *uuid_variable.second; }; - return fuse_core::Graph::const_variable_range(boost::make_transform_iterator(variables_.cbegin(), to_variable_ref), - boost::make_transform_iterator(variables_.cend(), to_variable_ref)); + return { boost::make_transform_iterator(variables_.cbegin(), to_variable_ref), + boost::make_transform_iterator(variables_.cend(), to_variable_ref) }; } void HashGraph::holdVariable(const fuse_core::UUID& variable_uuid, bool hold_constant) @@ -416,7 +414,7 @@ ceres::Solver::Summary HashGraph::optimizeFor(const rclcpp::Duration& max_optimi auto created_problem = clock.now(); // Modify the options to enforce the maximum time - rclcpp::Duration remaining = max_optimization_time - (created_problem - start); + rclcpp::Duration const remaining = max_optimization_time - (created_problem - start); auto time_constrained_options = options; time_constrained_options.max_solver_time_in_seconds = std::max(0.0, remaining.seconds()); @@ -491,7 +489,7 @@ void HashGraph::createProblem(ceres::Problem& problem) const std::vector parameter_blocks; for (auto const& uuid_constraint : constraints_) { - fuse_core::Constraint& constraint = *(uuid_constraint.second); + fuse_core::Constraint const& constraint = *(uuid_constraint.second); // We need the memory address of each variable value referenced by this constraint parameter_blocks.clear(); parameter_blocks.reserve(constraint.variables().size()); diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 6da9a773c..f8816a299 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -187,7 +187,7 @@ void FixedLagSmoother::optimizationLoop() } // Optimize { - std::lock_guard lock(optimization_mutex_); + std::lock_guard const lock(optimization_mutex_); // Apply motion models auto new_transaction = fuse_core::Transaction::make_shared(); // DANGER: processQueue obtains a lock from the pending_transactions_mutex_ @@ -276,13 +276,13 @@ void FixedLagSmoother::optimizerTimerCallback() // will not be waiting on the condition variable signal, so nothing will happen. auto optimization_request = false; { - std::lock_guard lock(pending_transactions_mutex_); + std::lock_guard const lock(pending_transactions_mutex_); optimization_request = !pending_transactions_.empty(); } if (optimization_request) { { - std::lock_guard lock(optimization_requested_mutex_); + std::lock_guard const lock(optimization_requested_mutex_); optimization_request_ = true; optimization_deadline_ = clock_->now() + optimize_timer_->time_until_trigger(); } @@ -293,7 +293,7 @@ void FixedLagSmoother::optimizerTimerCallback() void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const rclcpp::Time& lag_expiration) { // We need to get the pending transactions from the queue - std::lock_guard pending_transactions_lock(pending_transactions_mutex_); + std::lock_guard const pending_transactions_lock(pending_transactions_mutex_); if (pending_transactions_.empty()) { @@ -445,7 +445,7 @@ bool FixedLagSmoother::resetServiceCallback(std::shared_ptr lock(optimization_requested_mutex_); + std::lock_guard const lock(optimization_requested_mutex_); optimization_request_ = false; } started_ = false; @@ -455,10 +455,10 @@ bool FixedLagSmoother::resetServiceCallback(std::shared_ptr lock(optimization_mutex_); + std::lock_guard const lock(optimization_mutex_); // Clear all pending transactions { - std::lock_guard lock(pending_transactions_mutex_); + std::lock_guard const lock(pending_transactions_mutex_); pending_transactions_.clear(); } // Clear the graph and marginal tracking states @@ -489,7 +489,7 @@ void FixedLagSmoother::transactionCallback(const std::string& sensor_name, fuse_ return; } // We need to add the new transaction to the pending_transactions_ queue - std::lock_guard pending_transactions_lock(pending_transactions_mutex_); + std::lock_guard const pending_transactions_lock(pending_transactions_mutex_); // Add the new transaction to the pending set // The pending set is arranged "smallest stamp last" to making popping off the back more @@ -602,7 +602,7 @@ void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrappe status.add("Started", started); { - std::lock_guard lock(pending_transactions_mutex_); + std::lock_guard const lock(pending_transactions_mutex_); status.add("Pending Transactions", pending_transactions_.size()); } diff --git a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp index 0f6042ea3..14b9e21d8 100644 --- a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::AccelerationAngular2DStamped; TEST(AccelerationAngular2DStamped, Type) { - AccelerationAngular2DStamped variable(rclcpp::Time(12345678, 910111213)); + AccelerationAngular2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::AccelerationAngular2DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(AccelerationAngular2DStamped, UUID) { // Verify two accelerations at the same timestamp produce the same UUID { - AccelerationAngular2DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationAngular2DStamped variable2(rclcpp::Time(12345678, 910111213)); + AccelerationAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationAngular2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationAngular2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationAngular2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two accelerations at different timestamps produce different UUIDs { - AccelerationAngular2DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationAngular2DStamped variable2(rclcpp::Time(12345678, 910111214)); - AccelerationAngular2DStamped variable3(rclcpp::Time(12345679, 910111213)); + AccelerationAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationAngular2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + AccelerationAngular2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(AccelerationAngular2DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationAngular2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - AccelerationAngular2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + AccelerationAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + AccelerationAngular2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationAngular2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = AccelerationAngular2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(AccelerationAngular2DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -129,7 +127,7 @@ TEST(AccelerationAngular2DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp index c1fe6232b..ef59cad34 100644 --- a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::AccelerationAngular3DStamped; TEST(AccelerationAngular3DStamped, Type) { - AccelerationAngular3DStamped variable(rclcpp::Time(12345678, 910111213)); + AccelerationAngular3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::AccelerationAngular3DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(AccelerationAngular3DStamped, UUID) { // Verify two accelerations at the same timestamp produce the same UUID { - AccelerationAngular3DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationAngular3DStamped variable2(rclcpp::Time(12345678, 910111213)); + AccelerationAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationAngular3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationAngular3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationAngular3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular3DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular3DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two accelerations at different timestamps produce different UUIDs { - AccelerationAngular3DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationAngular3DStamped variable2(rclcpp::Time(12345678, 910111214)); - AccelerationAngular3DStamped variable3(rclcpp::Time(12345679, 910111213)); + AccelerationAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationAngular3DStamped const variable2(rclcpp::Time(12345678, 910111214)); + AccelerationAngular3DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,16 @@ TEST(AccelerationAngular3DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationAngular3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); - AccelerationAngular3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); + AccelerationAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + AccelerationAngular3DStamped const variable2(rclcpp::Time(12345678, 910111213), + fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationAngular3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = AccelerationAngular3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +101,7 @@ TEST(AccelerationAngular3DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -133,7 +132,7 @@ TEST(AccelerationAngular3DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp index 26370eebf..0513bc0c8 100644 --- a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::AccelerationLinear2DStamped; TEST(AccelerationLinear2DStamped, Type) { - AccelerationLinear2DStamped variable(rclcpp::Time(12345678, 910111213)); + AccelerationLinear2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::AccelerationLinear2DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(AccelerationLinear2DStamped, UUID) { // Verify two accelerations at the same timestamp produce the same UUID { - AccelerationLinear2DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationLinear2DStamped variable2(rclcpp::Time(12345678, 910111213)); + AccelerationLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationLinear2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationLinear2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationLinear2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two accelerations at different timestamps produce different UUIDs { - AccelerationLinear2DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationLinear2DStamped variable2(rclcpp::Time(12345678, 910111214)); - AccelerationLinear2DStamped variable3(rclcpp::Time(12345679, 910111213)); + AccelerationLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationLinear2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + AccelerationLinear2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(AccelerationLinear2DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationLinear2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - AccelerationLinear2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + AccelerationLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + AccelerationLinear2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationLinear2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = AccelerationLinear2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(AccelerationLinear2DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -131,7 +129,7 @@ TEST(AccelerationLinear2DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp index 2b8d0c6ce..caf05a638 100644 --- a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::AccelerationLinear3DStamped; TEST(AccelerationLinear3DStamped, Type) { - AccelerationLinear3DStamped variable(rclcpp::Time(12345678, 910111213)); + AccelerationLinear3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::AccelerationLinear3DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(AccelerationLinear3DStamped, UUID) { // Verify two accelerations at the same timestamp produce the same UUID { - AccelerationLinear3DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationLinear3DStamped variable2(rclcpp::Time(12345678, 910111213)); + AccelerationLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationLinear3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationLinear3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationLinear3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear3DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear3DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two accelerations at different timestamps produce different UUIDs { - AccelerationLinear3DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationLinear3DStamped variable2(rclcpp::Time(12345678, 910111214)); - AccelerationLinear3DStamped variable3(rclcpp::Time(12345679, 910111213)); + AccelerationLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationLinear3DStamped const variable2(rclcpp::Time(12345678, 910111214)); + AccelerationLinear3DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(AccelerationLinear3DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationLinear3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); - AccelerationLinear3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); + AccelerationLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + AccelerationLinear3DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationLinear3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = AccelerationLinear3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(AccelerationLinear3DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -133,7 +131,7 @@ TEST(AccelerationLinear3DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_fixed_size_variable.cpp b/fuse_variables/test/test_fixed_size_variable.cpp index dd358a91e..9c39e2a83 100644 --- a/fuse_variables/test/test_fixed_size_variable.cpp +++ b/fuse_variables/test/test_fixed_size_variable.cpp @@ -47,7 +47,7 @@ class TestVariable : public fuse_variables::FixedSizeVariable<2> TestVariable() : fuse_variables::FixedSizeVariable<2>(fuse_core::uuid::generate()) { } - virtual ~TestVariable() = default; + ~TestVariable() override = default; TestVariable(TestVariable const&) = default; TestVariable(TestVariable&&) = default; TestVariable& operator=(TestVariable const&) = default; @@ -78,7 +78,7 @@ class TestVariable : public fuse_variables::FixedSizeVariable<2> TEST(FixedSizeVariable, Size) { // Verify the expected size is returned - TestVariable variable; + TestVariable const variable; EXPECT_EQ(2u, variable.size()); // base class interface EXPECT_EQ(2u, TestVariable::varSize); // static member variable } diff --git a/fuse_variables/test/test_orientation_2d_stamped.cpp b/fuse_variables/test/test_orientation_2d_stamped.cpp index 1389a1ff2..910aaa6aa 100644 --- a/fuse_variables/test/test_orientation_2d_stamped.cpp +++ b/fuse_variables/test/test_orientation_2d_stamped.cpp @@ -52,7 +52,7 @@ using fuse_variables::Orientation2DStamped; TEST(Orientation2DStamped, Type) { - Orientation2DStamped variable(rclcpp::Time(12345678, 910111213)); + Orientation2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::Orientation2DStamped", variable.type()); } @@ -60,20 +60,20 @@ TEST(Orientation2DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - Orientation2DStamped variable1(rclcpp::Time(12345678, 910111213)); - Orientation2DStamped variable2(rclcpp::Time(12345678, 910111213)); + Orientation2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Orientation2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - Orientation2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - Orientation2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Orientation2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Orientation2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - Orientation2DStamped variable1(rclcpp::Time(12345678, 910111213)); - Orientation2DStamped variable2(rclcpp::Time(12345678, 910111214)); - Orientation2DStamped variable3(rclcpp::Time(12345679, 910111213)); + Orientation2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Orientation2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + Orientation2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -81,15 +81,15 @@ TEST(Orientation2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - Orientation2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - Orientation2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + Orientation2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + Orientation2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(Orientation2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = Orientation2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -127,14 +127,14 @@ using Orientation2DLocalParameterization = TEST(Orientation2DStamped, Plus) { - auto parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); // Simple test { double x[1] = { 1.0 }; double delta[1] = { 0.5 }; double actual[1] = { 0.0 }; - bool success = + bool const success = parameterization->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); @@ -146,7 +146,7 @@ TEST(Orientation2DStamped, Plus) double x[1] = { 2.0 }; double delta[1] = { 3.0 }; double actual[1] = { 0.0 }; - bool success = + bool const success = parameterization->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); @@ -156,7 +156,7 @@ TEST(Orientation2DStamped, Plus) TEST(Orientation2DStamped, PlusJacobian) { - auto parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation2DLocalParameterization(); auto test_values = std::vector{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; @@ -164,7 +164,7 @@ TEST(Orientation2DStamped, PlusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = parameterization->ComputeJacobian(static_cast(x), static_cast(actual)); + bool const success = parameterization->ComputeJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; reference.ComputeJacobian(static_cast(x), static_cast(expected)); @@ -176,14 +176,14 @@ TEST(Orientation2DStamped, PlusJacobian) TEST(Orientation2DStamped, Minus) { - auto parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); // Simple test { double x1[1] = { 1.0 }; double x2[1] = { 1.5 }; double actual[1] = { 0.0 }; - bool success = + bool const success = parameterization->Minus(static_cast(x1), static_cast(x2), static_cast(actual)); EXPECT_TRUE(success); @@ -195,7 +195,7 @@ TEST(Orientation2DStamped, Minus) double x1[1] = { 2.0 }; double x2[1] = { 5 - 2 * M_PI }; double actual[1] = { 0.0 }; - bool success = + bool const success = parameterization->Minus(static_cast(x1), static_cast(x2), static_cast(actual)); EXPECT_TRUE(success); @@ -205,7 +205,7 @@ TEST(Orientation2DStamped, Minus) TEST(Orientation2DStamped, MinusJacobian) { - auto parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation2DLocalParameterization(); auto test_values = std::vector{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; @@ -213,7 +213,7 @@ TEST(Orientation2DStamped, MinusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = parameterization->ComputeMinusJacobian(static_cast(x), static_cast(actual)); + bool const success = parameterization->ComputeMinusJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; reference.ComputeMinusJacobian(static_cast(x), static_cast(expected)); @@ -225,9 +225,7 @@ TEST(Orientation2DStamped, MinusJacobian) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -258,7 +256,7 @@ TEST(Orientation2DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); @@ -325,7 +323,8 @@ TEST(Orientation2DStamped, ManifoldPlus) double x[1] = { 1.0 }; double delta[1] = { 0.5 }; double actual[1] = { 0.0 }; - bool success = manifold->Plus(static_cast(x), static_cast(delta), static_cast(actual)); + bool const success = + manifold->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(1.5, actual[0], 1.0e-5); @@ -336,7 +335,8 @@ TEST(Orientation2DStamped, ManifoldPlus) double x[1] = { 2.0 }; double delta[1] = { 3.0 }; double actual[1] = { 0.0 }; - bool success = manifold->Plus(static_cast(x), static_cast(delta), static_cast(actual)); + bool const success = + manifold->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(5 - 2 * M_PI, actual[0], 1.0e-5); @@ -353,7 +353,7 @@ TEST(Orientation2DStamped, ManifoldPlusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = manifold->PlusJacobian(static_cast(x), static_cast(actual)); + bool const success = manifold->PlusJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; reference.PlusJacobian(static_cast(x), static_cast(expected)); @@ -372,7 +372,8 @@ TEST(Orientation2DStamped, ManifoldMinus) double x1[1] = { 1.0 }; double x2[1] = { 1.5 }; double actual[1] = { 0.0 }; - bool success = manifold->Minus(static_cast(x2), static_cast(x1), static_cast(actual)); + bool const success = + manifold->Minus(static_cast(x2), static_cast(x1), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(0.5, actual[0], 1.0e-5); @@ -383,7 +384,8 @@ TEST(Orientation2DStamped, ManifoldMinus) double x1[1] = { 2.0 }; double x2[1] = { 5 - 2 * M_PI }; double actual[1] = { 0.0 }; - bool success = manifold->Minus(static_cast(x2), static_cast(x1), static_cast(actual)); + bool const success = + manifold->Minus(static_cast(x2), static_cast(x1), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(3.0, actual[0], 1.0e-5); @@ -400,7 +402,7 @@ TEST(Orientation2DStamped, ManifoldMinusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = manifold->MinusJacobian(static_cast(x), static_cast(actual)); + bool const success = manifold->MinusJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; reference.MinusJacobian(x, expected); diff --git a/fuse_variables/test/test_orientation_3d_stamped.cpp b/fuse_variables/test/test_orientation_3d_stamped.cpp index 740c04461..956c4a920 100644 --- a/fuse_variables/test/test_orientation_3d_stamped.cpp +++ b/fuse_variables/test/test_orientation_3d_stamped.cpp @@ -54,7 +54,7 @@ using fuse_variables::Orientation3DStamped; TEST(Orientation3DStamped, Type) { - Orientation3DStamped variable(rclcpp::Time(12345678, 910111213)); + Orientation3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::Orientation3DStamped", variable.type()); } @@ -62,8 +62,8 @@ TEST(Orientation3DStamped, UUID) { // Verify two orientations at the same timestamp produce the same UUID { - Orientation3DStamped variable1(rclcpp::Time(12345678, 910111213)); - Orientation3DStamped variable2(rclcpp::Time(12345678, 910111213)); + Orientation3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Orientation3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } @@ -72,40 +72,40 @@ TEST(Orientation3DStamped, UUID) // Verify two orientations at the same timestamp and same hardware ID produce the same UUID { - Orientation3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable2(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable2(rclcpp::Time(12345678, 910111213), uuid_1); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two orientations with the same timestamp but different hardware IDs generate different // UUIDs { - Orientation3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable2(rclcpp::Time(12345678, 910111213), uuid_2); + Orientation3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable2(rclcpp::Time(12345678, 910111213), uuid_2); EXPECT_NE(variable1.uuid(), variable2.uuid()); } // Verify two orientations with the same hardware ID and different timestamps produce different // UUIDs { - Orientation3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable2(rclcpp::Time(12345678, 910111214), uuid_1); + Orientation3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable2(rclcpp::Time(12345678, 910111214), uuid_1); EXPECT_NE(variable1.uuid(), variable2.uuid()); - Orientation3DStamped variable3(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable4(rclcpp::Time(12345679, 910111213), uuid_1); + Orientation3DStamped const variable3(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable4(rclcpp::Time(12345679, 910111213), uuid_1); EXPECT_NE(variable3.uuid(), variable4.uuid()); } // Verify two orientations with different hardware IDs and different timestamps produce different // UUIDs { - Orientation3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable2(rclcpp::Time(12345678, 910111214), uuid_2); + Orientation3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable2(rclcpp::Time(12345678, 910111214), uuid_2); EXPECT_NE(variable1.uuid(), variable2.uuid()); - Orientation3DStamped variable3(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable4(rclcpp::Time(12345679, 910111213), uuid_2); + Orientation3DStamped const variable3(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable4(rclcpp::Time(12345679, 910111213), uuid_2); EXPECT_NE(variable3.uuid(), variable4.uuid()); } } @@ -150,12 +150,12 @@ using Orientation3DLocalParameterization = TEST(Orientation3DStamped, Plus) { - auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); double x[4] = { 0.842614977, 0.2, 0.3, 0.4 }; double delta[3] = { 0.15, -0.2, 0.433012702 }; double result[4] = { 0.0, 0.0, 0.0, 0.0 }; - bool success = parameterization->Plus(x, delta, result); + bool const success = parameterization->Plus(x, delta, result); EXPECT_TRUE(success); EXPECT_NEAR(0.745561, result[0], 1.0e-5); @@ -166,12 +166,12 @@ TEST(Orientation3DStamped, Plus) TEST(Orientation3DStamped, Minus) { - auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); double x1[4] = { 0.842614977, 0.2, 0.3, 0.4 }; double x2[4] = { 0.745561, 0.360184, 0.194124, 0.526043 }; double result[3] = { 0.0, 0.0, 0.0 }; - bool success = parameterization->Minus(x1, x2, result); + bool const success = parameterization->Minus(x1, x2, result); EXPECT_TRUE(success); EXPECT_NEAR(0.15, result[0], 1.0e-5); @@ -181,7 +181,7 @@ TEST(Orientation3DStamped, Minus) TEST(Orientation3DStamped, PlusJacobian) { - auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation3DLocalParameterization(); for (double qx = -0.5; qx < 0.5; qx += 0.1) @@ -190,14 +190,14 @@ TEST(Orientation3DStamped, PlusJacobian) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { - double qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); + double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(4, 3); /* *INDENT-OFF* */ actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ - bool success = parameterization->ComputeJacobian(x, actual.data()); + bool const success = parameterization->ComputeJacobian(x, actual.data()); fuse_core::MatrixXd expected(4, 3); /* *INDENT-OFF* */ @@ -206,7 +206,7 @@ TEST(Orientation3DStamped, PlusJacobian) reference.ComputeJacobian(x, expected.data()); EXPECT_TRUE(success); - Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); + Eigen::IOFormat const clean(4, 0, ", ", "\n", "[", "]"); EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" << "Actual is:\n" @@ -220,7 +220,7 @@ TEST(Orientation3DStamped, PlusJacobian) TEST(Orientation3DStamped, MinusJacobian) { - auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation3DLocalParameterization(); for (double qx = -0.5; qx < 0.5; qx += 0.1) @@ -229,14 +229,14 @@ TEST(Orientation3DStamped, MinusJacobian) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { - double qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); + double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(3, 4); /* *INDENT-OFF* */ actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ - bool success = parameterization->ComputeMinusJacobian(x, actual.data()); + bool const success = parameterization->ComputeMinusJacobian(x, actual.data()); fuse_core::MatrixXd expected(3, 4); /* *INDENT-OFF* */ @@ -245,7 +245,7 @@ TEST(Orientation3DStamped, MinusJacobian) reference.ComputeMinusJacobian(x, expected.data()); EXPECT_TRUE(success); - Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); + Eigen::IOFormat const clean(4, 0, ", ", "\n", "[", "]"); EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" << "Actual is:\n" @@ -259,7 +259,7 @@ TEST(Orientation3DStamped, MinusJacobian) TEST(Orientation3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = Orientation3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -330,7 +330,7 @@ TEST(Orientation3DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); @@ -434,12 +434,12 @@ using Orientation3DManifold = ceres::AutoDiffManifoldPlus(x, delta, result); + bool const success = manifold->Plus(x, delta, result); EXPECT_TRUE(success); EXPECT_NEAR(0.745561, result[0], 1.0e-5); @@ -450,7 +450,7 @@ TEST(Orientation3DStamped, ManifoldPlus) TEST(Orientation3DStamped, ManifoldPlusJacobian) { - auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); + auto* manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation3DManifold(); for (double qx = -0.5; qx < 0.5; qx += 0.1) @@ -459,14 +459,14 @@ TEST(Orientation3DStamped, ManifoldPlusJacobian) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { - double qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); + double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(4, 3); /* *INDENT-OFF* */ actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ - bool success = manifold->PlusJacobian(x, actual.data()); + bool const success = manifold->PlusJacobian(x, actual.data()); fuse_core::MatrixXd expected(4, 3); /* *INDENT-OFF* */ @@ -475,7 +475,7 @@ TEST(Orientation3DStamped, ManifoldPlusJacobian) reference.PlusJacobian(x, expected.data()); EXPECT_TRUE(success); - Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); + Eigen::IOFormat const clean(4, 0, ", ", "\n", "[", "]"); EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" << "Actual is:\n" @@ -493,8 +493,8 @@ TEST(Orientation3DStamped, ManifoldMinus) double x2[4] = { 0.745561, 0.360184, 0.194124, 0.526043 }; double result[3] = { 0.0, 0.0, 0.0 }; - auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); - bool success = manifold->Minus(x2, x1, result); + auto* manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); + bool const success = manifold->Minus(x2, x1, result); EXPECT_TRUE(success); EXPECT_NEAR(0.15, result[0], 1.0e-5); @@ -504,7 +504,7 @@ TEST(Orientation3DStamped, ManifoldMinus) TEST(Orientation3DStamped, ManifoldMinusJacobian) { - auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); + auto* manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation3DManifold(); for (double qx = -0.5; qx < 0.5; qx += 0.1) @@ -513,14 +513,14 @@ TEST(Orientation3DStamped, ManifoldMinusJacobian) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { - double qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); + double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(3, 4); /* *INDENT-OFF* */ actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ - bool success = manifold->MinusJacobian(x, actual.data()); + bool const success = manifold->MinusJacobian(x, actual.data()); fuse_core::MatrixXd expected(3, 4); /* *INDENT-OFF* */ @@ -529,7 +529,7 @@ TEST(Orientation3DStamped, ManifoldMinusJacobian) reference.MinusJacobian(x, expected.data()); EXPECT_TRUE(success); - Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); + Eigen::IOFormat const clean(4, 0, ", ", "\n", "[", "]"); EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" << "Actual is:\n" diff --git a/fuse_variables/test/test_point_2d_fixed_landmark.cpp b/fuse_variables/test/test_point_2d_fixed_landmark.cpp index 8ad4b7050..084176cc8 100644 --- a/fuse_variables/test/test_point_2d_fixed_landmark.cpp +++ b/fuse_variables/test/test_point_2d_fixed_landmark.cpp @@ -48,7 +48,7 @@ using fuse_variables::Point2DFixedLandmark; TEST(Point2DFixedLandmark, Type) { - Point2DFixedLandmark variable(0); + Point2DFixedLandmark const variable(0); EXPECT_EQ("fuse_variables::Point2DFixedLandmark", variable.type()); } @@ -56,24 +56,22 @@ TEST(Point2DFixedLandmark, UUID) { // Verify two positions with the same landmark ids produce the same uuids { - Point2DFixedLandmark variable1(0); - Point2DFixedLandmark variable2(0); + Point2DFixedLandmark const variable1(0); + Point2DFixedLandmark const variable2(0); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two positions with the different landmark ids produce different uuids { - Point2DFixedLandmark variable1(0); - Point2DFixedLandmark variable2(1); + Point2DFixedLandmark const variable1(0); + Point2DFixedLandmark const variable2(1); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -106,7 +104,7 @@ TEST(Point2DFixedLandmark, Optimization) } // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_point_2d_landmark.cpp b/fuse_variables/test/test_point_2d_landmark.cpp index 8428aab0c..bc4dfcb83 100644 --- a/fuse_variables/test/test_point_2d_landmark.cpp +++ b/fuse_variables/test/test_point_2d_landmark.cpp @@ -48,7 +48,7 @@ using fuse_variables::Point2DLandmark; TEST(Point2DLandmark, Type) { - Point2DLandmark variable(0); + Point2DLandmark const variable(0); EXPECT_EQ("fuse_variables::Point2DLandmark", variable.type()); } @@ -56,24 +56,22 @@ TEST(Point2DLandmark, UUID) { // Verify two positions with the same landmark ids produce the same uuids { - Point2DLandmark variable1(0); - Point2DLandmark variable2(0); + Point2DLandmark const variable1(0); + Point2DLandmark const variable2(0); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two positions with the different landmark ids produce different uuids { - Point2DLandmark variable1(0); - Point2DLandmark variable2(1); + Point2DLandmark const variable1(0); + Point2DLandmark const variable2(1); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -102,7 +100,7 @@ TEST(Point2DLandmark, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_point_3d_fixed_landmark.cpp b/fuse_variables/test/test_point_3d_fixed_landmark.cpp index e7fffa9a9..282b6e2c8 100644 --- a/fuse_variables/test/test_point_3d_fixed_landmark.cpp +++ b/fuse_variables/test/test_point_3d_fixed_landmark.cpp @@ -48,7 +48,7 @@ using fuse_variables::Point3DFixedLandmark; TEST(Point3DFixedLandmark, Type) { - Point3DFixedLandmark variable(0); + Point3DFixedLandmark const variable(0); EXPECT_EQ("fuse_variables::Point3DFixedLandmark", variable.type()); } @@ -56,24 +56,22 @@ TEST(Point3DFixedLandmark, UUID) { // Verify two positions with the same landmark ids produce the same uuids { - Point3DFixedLandmark variable1(0); - Point3DFixedLandmark variable2(0); + Point3DFixedLandmark const variable1(0); + Point3DFixedLandmark const variable2(0); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two positions with the different landmark ids produce different uuids { - Point3DFixedLandmark variable1(0); - Point3DFixedLandmark variable2(1); + Point3DFixedLandmark const variable1(0); + Point3DFixedLandmark const variable2(1); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -108,7 +106,7 @@ TEST(Point3DFixedLandmark, Optimization) } // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_point_3d_landmark.cpp b/fuse_variables/test/test_point_3d_landmark.cpp index fd5afed7d..3531074cb 100644 --- a/fuse_variables/test/test_point_3d_landmark.cpp +++ b/fuse_variables/test/test_point_3d_landmark.cpp @@ -48,7 +48,7 @@ using fuse_variables::Point3DLandmark; TEST(Point3DLandmark, Type) { - Point3DLandmark variable(0); + Point3DLandmark const variable(0); EXPECT_EQ("fuse_variables::Point3DLandmark", variable.type()); } @@ -56,24 +56,22 @@ TEST(Point3DLandmark, UUID) { // Verify two positions with the same landmark ids produce the same uuids { - Point3DLandmark variable1(0); - Point3DLandmark variable2(0); + Point3DLandmark const variable1(0); + Point3DLandmark const variable2(0); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two positions with the different landmark ids produce different uuids { - Point3DLandmark variable1(0); - Point3DLandmark variable2(1); + Point3DLandmark const variable1(0); + Point3DLandmark const variable2(1); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -104,7 +102,7 @@ TEST(Point3DLandmark, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_position_2d_stamped.cpp b/fuse_variables/test/test_position_2d_stamped.cpp index 0141c0062..0feeafe16 100644 --- a/fuse_variables/test/test_position_2d_stamped.cpp +++ b/fuse_variables/test/test_position_2d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::Position2DStamped; TEST(Position2DStamped, Type) { - Position2DStamped variable(rclcpp::Time(12345678, 910111213)); + Position2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::Position2DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(Position2DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - Position2DStamped variable1(rclcpp::Time(12345678, 910111213)); - Position2DStamped variable2(rclcpp::Time(12345678, 910111213)); + Position2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Position2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - Position2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - Position2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Position2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Position2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - Position2DStamped variable1(rclcpp::Time(12345678, 910111213)); - Position2DStamped variable2(rclcpp::Time(12345678, 910111214)); - Position2DStamped variable3(rclcpp::Time(12345679, 910111213)); + Position2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Position2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + Position2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(Position2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - Position2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - Position2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + Position2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + Position2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(Position2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = Position2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(Position2DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -131,7 +129,7 @@ TEST(Position2DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_position_3d_stamped.cpp b/fuse_variables/test/test_position_3d_stamped.cpp index 7d4d32a1c..3e4052012 100644 --- a/fuse_variables/test/test_position_3d_stamped.cpp +++ b/fuse_variables/test/test_position_3d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::Position3DStamped; TEST(Position3DStamped, Type) { - Position3DStamped variable(rclcpp::Time(12345678, 910111213)); + Position3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::Position3DStamped", variable.type()); } @@ -56,8 +56,8 @@ TEST(Position3DStamped, UUID) { // Verify two positions at the same timestamp produce the same UUID { - Position3DStamped variable1(rclcpp::Time(12345678, 910111213)); - Position3DStamped variable2(rclcpp::Time(12345678, 910111213)); + Position3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Position3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } @@ -66,46 +66,46 @@ TEST(Position3DStamped, UUID) // Verify two positions at the same timestamp and same hardware ID produce the same UUID { - Position3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable2(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable2(rclcpp::Time(12345678, 910111213), uuid_1); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two positions with the same timestamp but different hardware IDs generate different // UUIDs { - Position3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable2(rclcpp::Time(12345678, 910111213), uuid_2); + Position3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable2(rclcpp::Time(12345678, 910111213), uuid_2); EXPECT_NE(variable1.uuid(), variable2.uuid()); } // Verify two positions with the same hardware ID and different timestamps produce different UUIDs { - Position3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable2(rclcpp::Time(12345678, 910111214), uuid_1); + Position3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable2(rclcpp::Time(12345678, 910111214), uuid_1); EXPECT_NE(variable1.uuid(), variable2.uuid()); - Position3DStamped variable3(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable4(rclcpp::Time(12345679, 910111213), uuid_1); + Position3DStamped const variable3(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable4(rclcpp::Time(12345679, 910111213), uuid_1); EXPECT_NE(variable3.uuid(), variable4.uuid()); } // Verify two positions with different hardware IDs and different timestamps produce different // UUIDs { - Position3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable2(rclcpp::Time(12345678, 910111214), uuid_2); + Position3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable2(rclcpp::Time(12345678, 910111214), uuid_2); EXPECT_NE(variable1.uuid(), variable2.uuid()); - Position3DStamped variable3(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable4(rclcpp::Time(12345679, 910111213), uuid_2); + Position3DStamped const variable3(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable4(rclcpp::Time(12345679, 910111213), uuid_2); EXPECT_NE(variable3.uuid(), variable4.uuid()); } } TEST(Position3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = Position3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -120,9 +120,7 @@ TEST(Position3DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -153,7 +151,7 @@ TEST(Position3DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp index dcdfe0cc9..51d290c7d 100644 --- a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::VelocityAngular2DStamped; TEST(VelocityAngular2DStamped, Type) { - VelocityAngular2DStamped variable(rclcpp::Time(12345678, 910111213)); + VelocityAngular2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::VelocityAngular2DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(VelocityAngular2DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - VelocityAngular2DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityAngular2DStamped variable2(rclcpp::Time(12345678, 910111213)); + VelocityAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityAngular2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityAngular2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - VelocityAngular2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - VelocityAngular2DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityAngular2DStamped variable2(rclcpp::Time(12345678, 910111214)); - VelocityAngular2DStamped variable3(rclcpp::Time(12345679, 910111213)); + VelocityAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityAngular2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + VelocityAngular2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(VelocityAngular2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityAngular2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - VelocityAngular2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + VelocityAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + VelocityAngular2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityAngular2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = VelocityAngular2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(VelocityAngular2DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -129,7 +127,7 @@ TEST(VelocityAngular2DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp index 608ad1608..3dfb391c2 100644 --- a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::VelocityAngular3DStamped; TEST(VelocityAngular3DStamped, Type) { - VelocityAngular3DStamped variable(rclcpp::Time(12345678, 910111213)); + VelocityAngular3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::VelocityAngular3DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(VelocityAngular3DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - VelocityAngular3DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityAngular3DStamped variable2(rclcpp::Time(12345678, 910111213)); + VelocityAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityAngular3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityAngular3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - VelocityAngular3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular3DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular3DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - VelocityAngular3DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityAngular3DStamped variable2(rclcpp::Time(12345678, 910111214)); - VelocityAngular3DStamped variable3(rclcpp::Time(12345679, 910111213)); + VelocityAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityAngular3DStamped const variable2(rclcpp::Time(12345678, 910111214)); + VelocityAngular3DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(VelocityAngular3DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityAngular3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); - VelocityAngular3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); + VelocityAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + VelocityAngular3DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityAngular3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = VelocityAngular3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(VelocityAngular3DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -133,7 +131,7 @@ TEST(VelocityAngular3DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp index 57d51555d..a1f17855d 100644 --- a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::VelocityLinear2DStamped; TEST(VelocityLinear2DStamped, Type) { - VelocityLinear2DStamped variable(rclcpp::Time(12345678, 910111213)); + VelocityLinear2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::VelocityLinear2DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(VelocityLinear2DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - VelocityLinear2DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityLinear2DStamped variable2(rclcpp::Time(12345678, 910111213)); + VelocityLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityLinear2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityLinear2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - VelocityLinear2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - VelocityLinear2DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityLinear2DStamped variable2(rclcpp::Time(12345678, 910111214)); - VelocityLinear2DStamped variable3(rclcpp::Time(12345679, 910111213)); + VelocityLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityLinear2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + VelocityLinear2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(VelocityLinear2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityLinear2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - VelocityLinear2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + VelocityLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + VelocityLinear2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityLinear2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = VelocityLinear2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(VelocityLinear2DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -131,7 +129,7 @@ TEST(VelocityLinear2DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp index 9e69250ba..f37a38b07 100644 --- a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::VelocityLinear3DStamped; TEST(VelocityLinear3DStamped, Type) { - VelocityLinear3DStamped variable(rclcpp::Time(12345678, 910111213)); + VelocityLinear3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::VelocityLinear3DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(VelocityLinear3DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - VelocityLinear3DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityLinear3DStamped variable2(rclcpp::Time(12345678, 910111213)); + VelocityLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityLinear3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityLinear3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - VelocityLinear3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear3DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear3DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - VelocityLinear3DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityLinear3DStamped variable2(rclcpp::Time(12345678, 910111214)); - VelocityLinear3DStamped variable3(rclcpp::Time(12345679, 910111213)); + VelocityLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityLinear3DStamped const variable2(rclcpp::Time(12345678, 910111214)); + VelocityLinear3DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(VelocityLinear3DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityLinear3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); - VelocityLinear3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); + VelocityLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + VelocityLinear3DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityLinear3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = VelocityLinear3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(VelocityLinear3DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -133,7 +131,7 @@ TEST(VelocityLinear3DStamped, Optimization) problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); From c45c11e7d89c1d77628a01cbedff881184d90cfa Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 5 Nov 2024 20:14:19 +0000 Subject: [PATCH 13/39] clang-tidy fixes --- fuse_variables/test/test_orientation_3d_stamped.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/fuse_variables/test/test_orientation_3d_stamped.cpp b/fuse_variables/test/test_orientation_3d_stamped.cpp index 956c4a920..44acad27c 100644 --- a/fuse_variables/test/test_orientation_3d_stamped.cpp +++ b/fuse_variables/test/test_orientation_3d_stamped.cpp @@ -110,14 +110,17 @@ TEST(Orientation3DStamped, UUID) } } +namespace +{ template -inline static void QuaternionInverse(const T in[4], T out[4]) +inline void QuaternionInverse(const T in[4], T out[4]) { out[0] = in[0]; out[1] = -in[1]; out[2] = -in[2]; out[3] = -in[3]; } +} // namespace struct Orientation3DPlus { @@ -184,12 +187,14 @@ TEST(Orientation3DStamped, PlusJacobian) auto* parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation3DLocalParameterization(); + // NOLINTBEGIN(clang-analyzer-security.FloatLoopCounter) for (double qx = -0.5; qx < 0.5; qx += 0.1) { for (double qy = -0.5; qy < 0.5; qy += 0.1) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { + // NOLINTEND(clang-analyzer-security.FloatLoopCounter) double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; @@ -223,12 +228,14 @@ TEST(Orientation3DStamped, MinusJacobian) auto* parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation3DLocalParameterization(); + // NOLINTBEGIN(clang-analyzer-security.FloatLoopCounter) for (double qx = -0.5; qx < 0.5; qx += 0.1) { for (double qy = -0.5; qy < 0.5; qy += 0.1) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { + // NOLINTEND(clang-analyzer-security.FloatLoopCounter) double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; @@ -453,12 +460,14 @@ TEST(Orientation3DStamped, ManifoldPlusJacobian) auto* manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation3DManifold(); + // NOLINTBEGIN(clang-analyzer-security.FloatLoopCounter) for (double qx = -0.5; qx < 0.5; qx += 0.1) { for (double qy = -0.5; qy < 0.5; qy += 0.1) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { + // NOLINTEND(clang-analyzer-security.FloatLoopCounter) double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; @@ -507,12 +516,14 @@ TEST(Orientation3DStamped, ManifoldMinusJacobian) auto* manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation3DManifold(); + // NOLINTBEGIN(clang-analyzer-security.FloatLoopCounter) for (double qx = -0.5; qx < 0.5; qx += 0.1) { for (double qy = -0.5; qy < 0.5; qy += 0.1) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { + // NOLINTEND(clang-analyzer-security.FloatLoopCounter) double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; From 7d180e3eb110bcf0a2f56b01d6d6327611f09d6a Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 5 Nov 2024 21:02:01 +0000 Subject: [PATCH 14/39] add skeleton of apriltag tutorial --- fuse_models/fuse_plugins.xml | 5 + fuse_tutorials/CMakeLists.txt | 4 + .../config/fuse_apriltag_tutorial.yaml | 52 +++ .../launch/fuse_apriltag_tutorial.launch.py | 67 ++++ fuse_tutorials/src/apriltag_simulator.cpp | 310 ++++++++++++++++++ .../src/three_dimensional_simulator.cpp | 66 ++-- 6 files changed, 471 insertions(+), 33 deletions(-) create mode 100644 fuse_tutorials/config/fuse_apriltag_tutorial.yaml create mode 100644 fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py create mode 100644 fuse_tutorials/src/apriltag_simulator.cpp diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index 7830e9a81..f01a86f5a 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -79,6 +79,11 @@ published by another node + + + An adapter-type sensor that produces pose constraints from AprilTag detections published from another node + + An adapter-type sensor that produces absolute or relative pose constraints from information published by diff --git a/fuse_tutorials/CMakeLists.txt b/fuse_tutorials/CMakeLists.txt index 83387db16..2bde24db1 100644 --- a/fuse_tutorials/CMakeLists.txt +++ b/fuse_tutorials/CMakeLists.txt @@ -48,6 +48,9 @@ target_link_libraries(range_sensor_simulator ${PROJECT_NAME}) add_executable(three_dimensional_simulator src/three_dimensional_simulator.cpp) target_link_libraries(three_dimensional_simulator ${PROJECT_NAME}) +add_executable(apriltag_simulator src/apriltag_simulator.cpp) +target_link_libraries(apriltag_simulator ${PROJECT_NAME}) + # ############################################################################## # Install ## # ############################################################################## @@ -63,6 +66,7 @@ install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) install(TARGETS range_sensor_simulator DESTINATION lib/${PROJECT_NAME}) install(TARGETS three_dimensional_simulator DESTINATION lib/${PROJECT_NAME}) +install(TARGETS apriltag_simulator DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch config data DESTINATION share/${PROJECT_NAME}) diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml new file mode 100644 index 000000000..6a72f5939 --- /dev/null +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -0,0 +1,52 @@ +# this yaml file is adapted from `fuse_simple_tutorial.yaml` +state_estimator: + ros__parameters: + # Fixed-lag smoother configuration + optimization_frequency: 20.0 + transaction_timeout: 0.01 + lag_duration: 0.5 + + # all our sensors will be using this motion model + motion_models: + 3d_motion_model: + type: fuse_models::Omnidirectional3D + + 3d_motion_model: + # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc + process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + + sensor_models: + initial_localization_sensor: + type: fuse_models::Omnidirectional3DIgnition + motion_models: [3d_motion_model] + ignition: true + apriltag_sensor: + type: fuse_models::AprilTagPose + motion_models: [3d_motion_model] + + initial_localization_sensor: + publish_on_startup: true + # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc + initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + + apriltag_sensor: + topic: 'april_tf' + twist_target_frame: 'base_link' + position_dimensions: ['x', 'y', 'z'] + orientation_dimensions: ['roll', 'pitch', 'yaw'] + + # this publishes our estimated odometry + publishers: + filtered_publisher: + type: fuse_models::Odometry3DPublisher + + # the configuration of our output publisher + filtered_publisher: + topic: 'odom_filtered' + base_link_frame_id: 'base_link' + odom_frame_id: 'odom' + map_frame_id: 'map' + world_frame_id: 'odom' + publish_tf: true + publish_frequency: 10.0 diff --git a/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py new file mode 100644 index 000000000..a45d46726 --- /dev/null +++ b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py @@ -0,0 +1,67 @@ +#! /usr/bin/env python3 + +# Copyright 2024 PickNik Robotics +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + pkg_dir = FindPackageShare("fuse_tutorials") + + return LaunchDescription( + [ + # tell tf2 that map is the same as odom + # without this, visualization won't work as we have no reference + Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0", "0", "0", "0", "0", "0", "map", "odom"], + ), + # run our simulator + Node( + package="fuse_tutorials", + executable="apriltag_simulator", + name="apriltag_simulator", + output="screen", + ), + # run our estimator + Node( + package="fuse_optimizers", + executable="fixed_lag_smoother_node", + name="state_estimator", + parameters=[ + PathJoinSubstitution([pkg_dir, "config", "fuse_3d_tutorial.yaml"]) + ], + ), + # run visualization + Node( + package="rviz2", + executable="rviz2", + name="rviz", + arguments=[ + "-d", + [ + PathJoinSubstitution( + [pkg_dir, "config", "fuse_3d_tutorial.rviz"] + ) + ], + ], + ), + ] + ) diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp new file mode 100644 index 000000000..bc0f8dd67 --- /dev/null +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -0,0 +1,310 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include "tf2_msgs/msg/tf_message.hpp" + +namespace +{ +constexpr char baselinkFrame[] = "base_link"; //!< The base_link frame id used when + //!< publishing sensor data +constexpr char mapFrame[] = "map"; //!< The map frame id used when publishing ground truth + //!< data +constexpr double aprilTagPositionSigma = 0.1; //!< the april tag position std dev +constexpr double aprilTagOrientationSigma = 0.5; //!< the april tag orientation std dev +} // namespace + +/** + * @brief The true pose and velocity of the robot + */ +struct Robot +{ + rclcpp::Time stamp; + + double mass{}; + + double x = 0; + double y = 0; + double z = 0; + double roll = 0; + double pitch = 0; + double yaw = 0; + double vx = 0; + double vy = 0; + double vz = 0; + double vroll = 0; + double vpitch = 0; + double vyaw = 0; + double ax = 0; + double ay = 0; + double az = 0; +}; + +/** + * @brief Convert the robot state into a ground truth odometry message + */ +nav_msgs::msg::Odometry robotToOdometry(Robot const& state) +{ + nav_msgs::msg::Odometry msg; + msg.header.stamp = state.stamp; + msg.header.frame_id = mapFrame; + msg.child_frame_id = baselinkFrame; + msg.pose.pose.position.x = state.x; + msg.pose.pose.position.y = state.y; + msg.pose.pose.position.z = state.z; + + tf2::Quaternion q; + q.setEuler(state.yaw, state.pitch, state.roll); + msg.pose.pose.orientation.w = q.w(); + msg.pose.pose.orientation.x = q.x(); + msg.pose.pose.orientation.y = q.y(); + msg.pose.pose.orientation.z = q.z(); + msg.twist.twist.linear.x = state.vx; + msg.twist.twist.linear.y = state.vy; + msg.twist.twist.linear.z = state.vz; + msg.twist.twist.angular.x = state.vroll; + msg.twist.twist.angular.y = state.vpitch; + msg.twist.twist.angular.z = state.vyaw; + + // set covariances + msg.pose.covariance[0] = 0.1; + msg.pose.covariance[7] = 0.1; + msg.pose.covariance[14] = 0.1; + msg.pose.covariance[21] = 0.1; + msg.pose.covariance[28] = 0.1; + msg.pose.covariance[35] = 0.1; + msg.twist.covariance[0] = 0.1; + msg.twist.covariance[7] = 0.1; + msg.twist.covariance[14] = 0.1; + msg.twist.covariance[21] = 0.1; + msg.twist.covariance[28] = 0.1; + msg.twist.covariance[35] = 0.1; + return msg; +} + +/** + * @brief Compute the next robot state given the current robot state and a simulated step time + */ +Robot simulateRobotMotion(Robot const& previous_state, rclcpp::Time const& now, Eigen::Vector3d const& external_force) +{ + auto const dt = (now - previous_state.stamp).seconds(); + auto next_state = Robot(); + next_state.stamp = now; + next_state.mass = previous_state.mass; + + // just euler integrate to get the next position and velocity + next_state.x = previous_state.x + previous_state.vx * dt; + next_state.y = previous_state.y + previous_state.vy * dt; + next_state.z = previous_state.z + previous_state.vz * dt; + next_state.vx = previous_state.vx + previous_state.ax * dt; + next_state.vy = previous_state.vy + previous_state.ay * dt; + next_state.vz = previous_state.vz + previous_state.az * dt; + + // let's not deal with 3D orientation dynamics for this tutorial + next_state.roll = 0; + next_state.pitch = 0; + next_state.yaw = 0; + next_state.vroll = 0; + next_state.vpitch = 0; + next_state.vyaw = 0; + + // get the current acceleration from the current applied force + next_state.ax = external_force.x() / next_state.mass; + next_state.ay = external_force.y() / next_state.mass; + next_state.az = external_force.z() / next_state.mass; + + return next_state; +} + +tf2_msgs::msg::TFMessage simulateAprilTag(const Robot& /*robot*/) +{ + static std::random_device rd{}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> position_noise{ 0.0, aprilTagPositionSigma }; + static std::normal_distribution<> orientation_noise{ 0.0, aprilTagOrientationSigma }; + + tf2_msgs::msg::TFMessage msg; + return msg; +} + +void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces interfaces, + const Robot& state, const rclcpp::Clock::SharedPtr& clock, const rclcpp::Logger& logger) +{ + // Send the initial localization signal to the state estimator + auto srv = std::make_shared(); + srv->pose.header.frame_id = mapFrame; + srv->pose.pose.pose.position.x = state.x; + srv->pose.pose.pose.position.y = state.y; + srv->pose.pose.pose.position.z = state.z; + tf2::Quaternion q; + q.setEuler(state.yaw, state.pitch, state.roll); + srv->pose.pose.pose.orientation.w = q.w(); + srv->pose.pose.pose.orientation.x = q.x(); + srv->pose.pose.pose.orientation.y = q.y(); + srv->pose.pose.pose.orientation.z = q.z(); + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 1.0; + srv->pose.pose.covariance[14] = 1.0; + srv->pose.pose.covariance[21] = 1.0; + srv->pose.pose.covariance[28] = 1.0; + srv->pose.pose.covariance[35] = 1.0; + + auto const client = rclcpp::create_client( + interfaces.get_node_base_interface(), interfaces.get_node_graph_interface(), + interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", rclcpp::ServicesQoS()); + + while (!client->wait_for_service(std::chrono::seconds(30)) && + interfaces.get_node_base_interface()->get_context()->is_valid()) + { + RCLCPP_WARN_STREAM(logger, "Waiting for '" << client->get_service_name() << "' service to become available."); + } + + auto success = false; + while (!success) + { + clock->sleep_for(std::chrono::milliseconds(100)); + srv->pose.header.stamp = clock->now(); + auto result_future = client->async_send_request(srv); + + if (rclcpp::spin_until_future_complete(interfaces.get_node_base_interface(), result_future, + std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(logger, "set pose service call failed"); + client->remove_pending_request(result_future); + return; + } + success = result_future.get()->success; + } +} + +int main(int argc, char** argv) +{ + // set up our ROS node + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("three_dimensional_simulator"); + + // create our sensor publishers + auto tf_publisher = node->create_publisher("tf", 1); + + // create the ground truth publisher + auto ground_truth_publisher = node->create_publisher("ground_truth", 1); + + // Initialize the robot state (state variables are zero-initialized) + auto state = Robot(); + state.stamp = node->now(); + state.mass = 10; // kg + + // you can modify the rate at which this loop runs to see the different performance of the estimator and the effect of + // integration inaccuracy on the ground truth + auto rate = rclcpp::Rate(100.0); + + // normally we would have to initialize the state estimation, but we included an ignition 'sensor' in our config, + // which takes care of that. + + // parameters that control the motion pattern of the robot + double const motion_duration = 5; // length of time to oscillate on a given axis, in seconds + double const n_cycles = 2; // number of oscillations per `motion_duration` + + while (rclcpp::ok()) + { + // store the first time this runs (since it won't start running exactly at a multiple of `motion_duration`) + static auto const firstTime = node->now(); + auto const now = node->now(); + + // compensate for the original time offset + double now_d = (now - firstTime).seconds(); + // store how long it has been (resetting at `motion_duration` seconds) + double mod_time = std::fmod(now_d, motion_duration); + + // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) + double const force_magnitude = 100 * std::cos(2 * M_PI * n_cycles * mod_time / motion_duration); + Eigen::Vector3d external_force = { 0, 0, 0 }; + + // switch oscillation axes every `motion_duration` seconds (with one 'rest period') + if (std::fmod(now_d, 4 * motion_duration) < motion_duration) + { + external_force.x() = force_magnitude; + } + else if (std::fmod(now_d, 4 * motion_duration) < 2 * motion_duration) + { + external_force.y() = force_magnitude; + } + else if (std::fmod(now_d, 4 * motion_duration) < 3 * motion_duration) + { + external_force.z() = force_magnitude; + } + else + { + // reset the robot's position and velocity, leave the external force as 0 + // we do this so the ground truth doesn't drift (due to inaccuracy from euler integration) + state.x = 0; + state.y = 0; + state.z = 0; + state.vx = 0; + state.vy = 0; + state.vz = 0; + } + + // Simulate the robot motion + auto new_state = simulateRobotMotion(state, now, external_force); + + // Publish the new ground truth + ground_truth_publisher->publish(robotToOdometry(new_state)); + + // Generate and publish simulated measurements from the new robot state + tf_publisher->publish(simulateAprilTag(new_state)); + + // Wait for the next time step + state = new_state; + rclcpp::spin_some(node); + rate.sleep(); + } + + rclcpp::shutdown(); + return 0; +} diff --git a/fuse_tutorials/src/three_dimensional_simulator.cpp b/fuse_tutorials/src/three_dimensional_simulator.cpp index 6bfb873fa..267589585 100644 --- a/fuse_tutorials/src/three_dimensional_simulator.cpp +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -50,14 +50,14 @@ namespace { -static constexpr char BASELINK_FRAME[] = "base_link"; //!< The base_link frame id used when - //!< publishing sensor data -static constexpr char MAP_FRAME[] = "map"; //!< The map frame id used when publishing ground truth - //!< data -static constexpr double IMU_SIGMA = 0.1; //!< Std dev of simulated Imu measurement noise -static constexpr char ODOM_FRAME[] = "odom"; //!< The odom frame id used when publishing wheel -static constexpr double ODOM_POSITION_SIGMA = 0.5; //!< Std dev of simulated odom position measurement noise -static constexpr double ODOM_ORIENTATION_SIGMA = 0.1; //!< Std dev of simulated odom orientation measurement noise +constexpr char baselinkFrame[] = "base_link"; //!< The base_link frame id used when + //!< publishing sensor data +constexpr char mapFrame[] = "map"; //!< The map frame id used when publishing ground truth + //!< data +constexpr double imuSigma = 0.1; //!< Std dev of simulated Imu measurement noise +constexpr char odomFrame[] = "odom"; //!< The odom frame id used when publishing wheel +constexpr double odomPositionSigma = 0.5; //!< Std dev of simulated odom position measurement noise +constexpr double odomOrientationSigma = 0.1; //!< Std dev of simulated odom orientation measurement noise } // namespace /** @@ -67,7 +67,7 @@ struct Robot { rclcpp::Time stamp; - double mass; + double mass{}; double x = 0; double y = 0; @@ -93,8 +93,8 @@ nav_msgs::msg::Odometry robotToOdometry(Robot const& state) { nav_msgs::msg::Odometry msg; msg.header.stamp = state.stamp; - msg.header.frame_id = MAP_FRAME; - msg.child_frame_id = BASELINK_FRAME; + msg.header.frame_id = mapFrame; + msg.child_frame_id = baselinkFrame; msg.pose.pose.position.x = state.x; msg.pose.pose.position.y = state.y; msg.pose.pose.position.z = state.z; @@ -169,19 +169,19 @@ sensor_msgs::msg::Imu simulateImu(Robot const& robot) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; - static std::normal_distribution<> noise{ 0.0, IMU_SIGMA }; + static std::normal_distribution<> noise{ 0.0, imuSigma }; sensor_msgs::msg::Imu msg; msg.header.stamp = robot.stamp; - msg.header.frame_id = BASELINK_FRAME; + msg.header.frame_id = baselinkFrame; // measure accel msg.linear_acceleration.x = robot.ax + noise(generator); msg.linear_acceleration.y = robot.ay + noise(generator); msg.linear_acceleration.z = robot.az + noise(generator); - msg.linear_acceleration_covariance[0] = IMU_SIGMA * IMU_SIGMA; - msg.linear_acceleration_covariance[4] = IMU_SIGMA * IMU_SIGMA; - msg.linear_acceleration_covariance[8] = IMU_SIGMA * IMU_SIGMA; + msg.linear_acceleration_covariance[0] = imuSigma * imuSigma; + msg.linear_acceleration_covariance[4] = imuSigma * imuSigma; + msg.linear_acceleration_covariance[8] = imuSigma * imuSigma; // Simulated IMU does not provide orientation (negative covariance indicates this) msg.orientation_covariance[0] = -1; @@ -191,9 +191,9 @@ sensor_msgs::msg::Imu simulateImu(Robot const& robot) msg.angular_velocity.x = robot.vroll + noise(generator); msg.angular_velocity.y = robot.vpitch + noise(generator); msg.angular_velocity.z = robot.vyaw + noise(generator); - msg.angular_velocity_covariance[0] = IMU_SIGMA * IMU_SIGMA; - msg.angular_velocity_covariance[4] = IMU_SIGMA * IMU_SIGMA; - msg.angular_velocity_covariance[8] = IMU_SIGMA * IMU_SIGMA; + msg.angular_velocity_covariance[0] = imuSigma * imuSigma; + msg.angular_velocity_covariance[4] = imuSigma * imuSigma; + msg.angular_velocity_covariance[8] = imuSigma * imuSigma; return msg; } @@ -201,20 +201,20 @@ nav_msgs::msg::Odometry simulateOdometry(const Robot& robot) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; - static std::normal_distribution<> position_noise{ 0.0, ODOM_POSITION_SIGMA }; + static std::normal_distribution<> position_noise{ 0.0, odomPositionSigma }; nav_msgs::msg::Odometry msg; msg.header.stamp = robot.stamp; - msg.header.frame_id = ODOM_FRAME; - msg.child_frame_id = BASELINK_FRAME; + msg.header.frame_id = odomFrame; + msg.child_frame_id = baselinkFrame; // noisy position measurement msg.pose.pose.position.x = robot.x + position_noise(generator); msg.pose.pose.position.y = robot.y + position_noise(generator); msg.pose.pose.position.z = robot.z + position_noise(generator); - msg.pose.covariance[0] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; - msg.pose.covariance[7] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; - msg.pose.covariance[14] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + msg.pose.covariance[0] = odomPositionSigma * odomPositionSigma; + msg.pose.covariance[7] = odomPositionSigma * odomPositionSigma; + msg.pose.covariance[14] = odomPositionSigma * odomPositionSigma; // noisy orientation measurement tf2::Quaternion q; @@ -223,9 +223,9 @@ nav_msgs::msg::Odometry simulateOdometry(const Robot& robot) msg.pose.pose.orientation.x = q.x(); msg.pose.pose.orientation.y = q.y(); msg.pose.pose.orientation.z = q.z(); - msg.pose.covariance[21] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; - msg.pose.covariance[28] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; - msg.pose.covariance[35] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + msg.pose.covariance[21] = odomOrientationSigma * odomOrientationSigma; + msg.pose.covariance[28] = odomOrientationSigma * odomOrientationSigma; + msg.pose.covariance[35] = odomOrientationSigma * odomOrientationSigma; return msg; } @@ -234,7 +234,7 @@ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces(); - srv->pose.header.frame_id = MAP_FRAME; + srv->pose.header.frame_id = mapFrame; srv->pose.pose.pose.position.x = state.x; srv->pose.pose.pose.position.y = state.y; srv->pose.pose.pose.position.z = state.z; @@ -306,21 +306,21 @@ int main(int argc, char** argv) // parameters that control the motion pattern of the robot double const motion_duration = 5; // length of time to oscillate on a given axis, in seconds - double const N_cycles = 2; // number of oscillations per `motion_duration` + double const n_cycles = 2; // number of oscillations per `motion_duration` while (rclcpp::ok()) { // store the first time this runs (since it won't start running exactly at a multiple of `motion_duration`) - static auto const first_time = node->now(); + static auto const firstTime = node->now(); auto const now = node->now(); // compensate for the original time offset - double now_d = (now - first_time).seconds(); + double now_d = (now - firstTime).seconds(); // store how long it has been (resetting at `motion_duration` seconds) double mod_time = std::fmod(now_d, motion_duration); // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) - double const force_magnitude = 100 * std::cos(2 * M_PI * N_cycles * mod_time / motion_duration); + double const force_magnitude = 100 * std::cos(2 * M_PI * n_cycles * mod_time / motion_duration); Eigen::Vector3d external_force = { 0, 0, 0 }; // switch oscillation axes every `motion_duration` seconds (with one 'rest period') From 23d5f817679f9b894e7efca2f0e5f6a568446b09 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 5 Nov 2024 21:08:35 +0000 Subject: [PATCH 15/39] remove now-unused compile options --- fuse_graphs/CMakeLists.txt | 52 -------------------------------------- 1 file changed, 52 deletions(-) diff --git a/fuse_graphs/CMakeLists.txt b/fuse_graphs/CMakeLists.txt index 45838b883..9057891bc 100644 --- a/fuse_graphs/CMakeLists.txt +++ b/fuse_graphs/CMakeLists.txt @@ -20,58 +20,6 @@ find_package(Ceres REQUIRED) include(boost-extras.cmake) -# ############################################################################## -# Build ## -# ############################################################################## -# lint_cmake: -linelength Disable warnings about maybe uninitialized variables -# with -Wno-maybe-uninitialized until we fix the following error: -# -# In file included from include/c++/12.2.0/functional:59, from -# include/eigen3/Eigen/Core:85, from include/fuse_core/fuse_macros.h:63, from -# include/fuse_core/loss.h:37, from include/fuse_core/constraint.h:37, from -# src/fuse/fuse_graphs/include/fuse_graphs/hash_graph.h:38, from -# src/fuse/fuse_graphs/src/hash_graph.cpp:34: In copy constructor -# ‘std::function<_Res(_ArgTypes ...)>::function(const -# std::function<_Res(_ArgTypes ...)>&) [with _Res = const fuse_core::Variable&; -# _ArgTypes = {const std::pair >&}]’, inlined from -# ‘boost::iterators::transform_iterator::transform_iterator(const Iterator&, UnaryFunc) [with UnaryFunc = -# std::function >&)>; Iterator = -# std::__detail::_Node_const_iterator >, false, true>; Reference = -# boost::use_default; Value = boost::use_default]’ at -# include/boost/iterator/transform_iterator.hpp:96:21, inlined from -# ‘boost::iterators::transform_iterator -# boost::iterators::make_transform_iterator(Iterator, UnaryFunc) [with UnaryFunc -# = std::function >&)>; Iterator = -# std::__detail::_Node_const_iterator >, false, true>]’ at -# include/boost/iterator/transform_iterator.hpp:141:61, inlined from ‘virtual -# fuse_core::Graph::const_variable_range fuse_graphs::HashGraph::getVariables() -# const’ at fuse/fuse_graphs/src/hash_graph.cpp:284:35: -# /nix/store/b7hvml0m3qmqraz1022fwvyyg6fc1vdy-gcc-12.2.0/include/c++/12.2.0/bits/std_function.h:391:17: -# error: ‘’ may be used uninitialized [-Werror=maybe-uninitialized] -# 391 | __x._M_manager(_M_functor, __x._M_functor, __clone_functor); -# | ~~~~^~~~~~~~~~ -# /nix/store/b7hvml0m3qmqraz1022fwvyyg6fc1vdy-gcc-12.2.0/include/c++/12.2.0/bits/std_function.h: -# In member function ‘virtual fuse_core::Graph::const_variable_range -# fuse_graphs::HashGraph::getVariables() const’: -# /nix/store/b7hvml0m3qmqraz1022fwvyyg6fc1vdy-gcc-12.2.0/include/c++/12.2.0/bits/std_function.h:267:7: -# note: by argument 2 of type ‘const std::_Any_data&’ to ‘static bool -# std::_Function_handler<_Res(_ArgTypes ...), -# _Functor>::_M_manager(std::_Any_data&, const std::_Any_data&, -# std::_Manager_operation) [with _Res = const fuse_core::Variable&; _Functor = -# fuse_graphs::HashGraph::getVariables() const::, -# boost::hash >::value_type&)>; _ArgTypes = {const -# std::pair >&}]’ -# declared here 267 | _M_manager(_Any_data& __dest, const _Any_data& -# __source, | ^~~~~~~~~~ -add_compile_options(-Wall -Werror -Wno-maybe-uninitialized) - # fuse_graphs library add_library(${PROJECT_NAME} src/hash_graph.cpp) target_include_directories( From 51363c60442594ddf0b3f30c75d784d6829fc2fd Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 6 Nov 2024 00:30:27 +0000 Subject: [PATCH 16/39] finish april tag simulator --- .../parameters/april_tag_pose_params.hpp | 5 +- fuse_models/src/april_tag_pose.cpp | 4 +- .../config/fuse_apriltag_tutorial.yaml | 2 +- .../launch/fuse_apriltag_tutorial.launch.py | 4 +- fuse_tutorials/src/apriltag_simulator.cpp | 102 ++++++++++++++++-- .../src/three_dimensional_simulator.cpp | 4 +- 6 files changed, 104 insertions(+), 17 deletions(-) diff --git a/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp b/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp index d7605e283..50b470085 100644 --- a/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp +++ b/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp @@ -81,8 +81,7 @@ struct AprilTagPoseParams : public ParameterBase fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); - pose_target_frame = - fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_target_frame"), pose_target_frame); + target_frame = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame); pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); pose_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_covariance"), @@ -98,7 +97,7 @@ struct AprilTagPoseParams : public ParameterBase std::vector pose_covariance; //!< The diagonal elements of the tag pose covariance int queue_size{ 10 }; std::string topic; - std::string pose_target_frame; + std::string target_frame; std::vector position_indices; std::vector orientation_indices; fuse_core::Loss::SharedPtr pose_loss; diff --git a/fuse_models/src/april_tag_pose.cpp b/fuse_models/src/april_tag_pose.cpp index 919f6e419..80b5e2470 100644 --- a/fuse_models/src/april_tag_pose.cpp +++ b/fuse_models/src/april_tag_pose.cpp @@ -31,6 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include @@ -124,6 +125,7 @@ void AprilTagPose::process(MessageType const& msg) // Create the pose from the transform auto pose = std::make_unique(); pose->header = transform.header; + pose->header.frame_id = pose->header.frame_id; pose->pose.pose.orientation = transform.transform.rotation; pose->pose.pose.position.x = transform.transform.translation.x; pose->pose.pose.position.y = transform.transform.translation.y; @@ -136,7 +138,7 @@ void AprilTagPose::process(MessageType const& msg) } const bool validate = !params_.disable_checks; - common::processAbsolutePose3DWithCovariance(name(), device_id_, *pose, params_.pose_loss, params_.pose_target_frame, + common::processAbsolutePose3DWithCovariance(name(), device_id_, *pose, params_.pose_loss, params_.target_frame, params_.position_indices, params_.orientation_indices, *tf_buffer_, validate, *transaction, params_.tf_timeout); diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml index 6a72f5939..f395def3e 100644 --- a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -32,7 +32,7 @@ state_estimator: apriltag_sensor: topic: 'april_tf' - twist_target_frame: 'base_link' + target_frame: 'base_link' position_dimensions: ['x', 'y', 'z'] orientation_dimensions: ['roll', 'pitch', 'yaw'] diff --git a/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py index a45d46726..4beddf0b3 100644 --- a/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py +++ b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py @@ -46,7 +46,9 @@ def generate_launch_description(): executable="fixed_lag_smoother_node", name="state_estimator", parameters=[ - PathJoinSubstitution([pkg_dir, "config", "fuse_3d_tutorial.yaml"]) + PathJoinSubstitution( + [pkg_dir, "config", "fuse_apriltag_tutorial.yaml"] + ) ], ), # run visualization diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index bc0f8dd67..d430bb1f4 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -47,16 +47,22 @@ #include #include #include +#include +#include +#include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" namespace { -constexpr char baselinkFrame[] = "base_link"; //!< The base_link frame id used when - //!< publishing sensor data -constexpr char mapFrame[] = "map"; //!< The map frame id used when publishing ground truth - //!< data -constexpr double aprilTagPositionSigma = 0.1; //!< the april tag position std dev -constexpr double aprilTagOrientationSigma = 0.5; //!< the april tag orientation std dev +constexpr char baselinkFrame[] = "base_link"; //!< The base_link frame id used when + //!< publishing sensor data +constexpr char mapFrame[] = "map"; //!< The map frame id used when publishing ground truth + //!< data +constexpr double aprilTagPositionSigma = 0.1; //!< the april tag position std dev +constexpr double aprilTagOrientationSigma = 0.25; //!< the april tag orientation std dev +constexpr size_t numAprilTags = 8; //!< the number of april tags +constexpr double detectionProbability = + 0.5; //!< the probability that any given april tag is detectable on a given tick of the simulation } // namespace /** @@ -161,14 +167,89 @@ Robot simulateRobotMotion(Robot const& previous_state, rclcpp::Time const& now, return next_state; } -tf2_msgs::msg::TFMessage simulateAprilTag(const Robot& /*robot*/) +tf2_msgs::msg::TFMessage aprilTagPoses(Robot const& robot) +{ + tf2_msgs::msg::TFMessage msg; + + // publish the april tag positions relative to the base + for (std::size_t i = 0; i < numAprilTags; ++i) + { + geometry_msgs::msg::TransformStamped april_to_base; + april_to_base.child_frame_id = "base_link"; + + // april tag names start at 1 + april_to_base.header.frame_id = "april_" + std::to_string(i + 1); + april_to_base.header.stamp = robot.stamp; + + april_to_base.transform.rotation.w = 1; + april_to_base.transform.rotation.x = 0; + april_to_base.transform.rotation.y = 0; + april_to_base.transform.rotation.z = 0; + + // calculate offset of each april tag + // we start with offset 1, 1, 1 and switch the z, y, then x as if they were binary digits based off of the april tag + // number see the launch file for a more readable offset for each april tag + bool x_positive = ((i >> 3) & 1) == 0u; + bool y_positive = ((i >> 2) & 1) == 0u; + bool z_positive = ((i >> 1) & 1) == 0u; + + // robot position with offset and noise + april_to_base.transform.translation.x = x_positive ? 1. : -1.; + april_to_base.transform.translation.y = y_positive ? 1. : -1.; + april_to_base.transform.translation.z = z_positive ? 1. : -1.; + msg.transforms.push_back(april_to_base); + } + return msg; +} + +tf2_msgs::msg::TFMessage simulateAprilTag(const Robot& robot) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; static std::normal_distribution<> position_noise{ 0.0, aprilTagPositionSigma }; static std::normal_distribution<> orientation_noise{ 0.0, aprilTagOrientationSigma }; + static std::bernoulli_distribution april_tag_detectable(detectionProbability); tf2_msgs::msg::TFMessage msg; + + // publish the april tag positions + for (std::size_t i = 0; i < numAprilTags; ++i) + { + geometry_msgs::msg::TransformStamped april_to_world; + april_to_world.child_frame_id = "odom"; + // april tag names start at 1 + april_to_world.header.frame_id = "april_" + std::to_string(i + 1); + april_to_world.header.stamp = robot.stamp; + tf2::Quaternion q; + // robot orientation with noise + q.setRPY(robot.roll + orientation_noise(generator), robot.pitch + orientation_noise(generator), + robot.yaw + orientation_noise(generator)); + april_to_world.transform.rotation.w = q.w(); + april_to_world.transform.rotation.x = q.x(); + april_to_world.transform.rotation.y = q.y(); + april_to_world.transform.rotation.z = q.z(); + + // calculate offset of each april tag + // we start with offset 1, 1, 1 and switch the z, y, then x as if they were binary digits based off of the april tag + // number see the launch file for a more readable offset for each april tag + bool x_positive = ((i >> 3) & 1) == 0u; + bool y_positive = ((i >> 2) & 1) == 0u; + bool z_positive = ((i >> 1) & 1) == 0u; + + double x_offset = x_positive ? 1. : -1.; + double y_offset = y_positive ? 1. : -1.; + double z_offset = z_positive ? 1. : -1.; + + // robot position with offset and noise + april_to_world.transform.translation.x = robot.x + x_offset + position_noise(generator); + april_to_world.transform.translation.y = robot.y + y_offset + position_noise(generator); + april_to_world.transform.translation.z = robot.z + z_offset + position_noise(generator); + + if (april_tag_detectable(generator)) + { + msg.transforms.push_back(april_to_world); + } + } return msg; } @@ -229,6 +310,7 @@ int main(int argc, char** argv) auto node = rclcpp::Node::make_shared("three_dimensional_simulator"); // create our sensor publishers + auto april_tf_publisher = node->create_publisher("april_tf", 1); auto tf_publisher = node->create_publisher("tf", 1); // create the ground truth publisher @@ -241,7 +323,7 @@ int main(int argc, char** argv) // you can modify the rate at which this loop runs to see the different performance of the estimator and the effect of // integration inaccuracy on the ground truth - auto rate = rclcpp::Rate(100.0); + auto rate = rclcpp::Rate(1000.0); // normally we would have to initialize the state estimation, but we included an ignition 'sensor' in our config, // which takes care of that. @@ -297,7 +379,9 @@ int main(int argc, char** argv) ground_truth_publisher->publish(robotToOdometry(new_state)); // Generate and publish simulated measurements from the new robot state - tf_publisher->publish(simulateAprilTag(new_state)); + tf_publisher->publish(aprilTagPoses(new_state)); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + april_tf_publisher->publish(simulateAprilTag(new_state)); // Wait for the next time step state = new_state; diff --git a/fuse_tutorials/src/three_dimensional_simulator.cpp b/fuse_tutorials/src/three_dimensional_simulator.cpp index 267589585..52dd53490 100644 --- a/fuse_tutorials/src/three_dimensional_simulator.cpp +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -315,9 +315,9 @@ int main(int argc, char** argv) auto const now = node->now(); // compensate for the original time offset - double now_d = (now - firstTime).seconds(); + double const now_d = (now - firstTime).seconds(); // store how long it has been (resetting at `motion_duration` seconds) - double mod_time = std::fmod(now_d, motion_duration); + double const mod_time = std::fmod(now_d, motion_duration); // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) double const force_magnitude = 100 * std::cos(2 * M_PI * n_cycles * mod_time / motion_duration); From 20d67d6fdb2ad1bd2180269fed3295f761250400 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 6 Nov 2024 01:20:17 +0000 Subject: [PATCH 17/39] clang tidy fixes --- fuse_tutorials/src/apriltag_simulator.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index d430bb1f4..6d2fd95da 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -189,9 +189,9 @@ tf2_msgs::msg::TFMessage aprilTagPoses(Robot const& robot) // calculate offset of each april tag // we start with offset 1, 1, 1 and switch the z, y, then x as if they were binary digits based off of the april tag // number see the launch file for a more readable offset for each april tag - bool x_positive = ((i >> 3) & 1) == 0u; - bool y_positive = ((i >> 2) & 1) == 0u; - bool z_positive = ((i >> 1) & 1) == 0u; + bool const x_positive = ((i >> 3) & 1) == 0u; + bool const y_positive = ((i >> 2) & 1) == 0u; + bool const z_positive = ((i >> 1) & 1) == 0u; // robot position with offset and noise april_to_base.transform.translation.x = x_positive ? 1. : -1.; @@ -232,13 +232,13 @@ tf2_msgs::msg::TFMessage simulateAprilTag(const Robot& robot) // calculate offset of each april tag // we start with offset 1, 1, 1 and switch the z, y, then x as if they were binary digits based off of the april tag // number see the launch file for a more readable offset for each april tag - bool x_positive = ((i >> 3) & 1) == 0u; - bool y_positive = ((i >> 2) & 1) == 0u; - bool z_positive = ((i >> 1) & 1) == 0u; + bool const x_positive = ((i >> 3) & 1) == 0u; + bool const y_positive = ((i >> 2) & 1) == 0u; + bool const z_positive = ((i >> 1) & 1) == 0u; - double x_offset = x_positive ? 1. : -1.; - double y_offset = y_positive ? 1. : -1.; - double z_offset = z_positive ? 1. : -1.; + double const x_offset = x_positive ? 1. : -1.; + double const y_offset = y_positive ? 1. : -1.; + double const z_offset = z_positive ? 1. : -1.; // robot position with offset and noise april_to_world.transform.translation.x = robot.x + x_offset + position_noise(generator); @@ -339,9 +339,9 @@ int main(int argc, char** argv) auto const now = node->now(); // compensate for the original time offset - double now_d = (now - firstTime).seconds(); + double const now_d = (now - firstTime).seconds(); // store how long it has been (resetting at `motion_duration` seconds) - double mod_time = std::fmod(now_d, motion_duration); + double const mod_time = std::fmod(now_d, motion_duration); // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) double const force_magnitude = 100 * std::cos(2 * M_PI * n_cycles * mod_time / motion_duration); From 7318e7a467d233340dd04e8a2564d995ab8cee3a Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 6 Nov 2024 16:06:32 +0000 Subject: [PATCH 18/39] remove unknown compile warning --- fuse_constraints/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 5f3a5d4cc..f88a54439 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -116,7 +116,7 @@ include(suitesparse-extras.cmake) # plain_matrix_type::type tmp(src); | ^~~ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 12.0) - add_compile_options(-Wall -Werror -Wno-array-bounds -Wno-stringop-overread) + add_compile_options(-Wall -Werror -Wno-array-bounds) else() add_compile_options(-Wall -Werror) endif() From b6e733e248b77218d736769a0472f89747b75c26 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 6 Nov 2024 17:31:41 +0000 Subject: [PATCH 19/39] fix memory leak --- fuse_constraints/src/marginalize_variables.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index 92cce1db8..617f03c41 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -367,6 +367,7 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G manifold->PlusJacobian(variable_values[index], j.data()); jacobian *= j; } + delete manifold; #endif } From 7b329afc2c88342823f8649ff395cb93cd01a8df Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 8 Nov 2024 17:21:56 +0000 Subject: [PATCH 20/39] applied PR feedback --- fuse_models/CMakeLists.txt | 2 +- fuse_models/fuse_plugins.xml | 2 +- ...params.hpp => transform_sensor_params.hpp} | 2 +- ...pril_tag_pose.hpp => transform_sensor.hpp} | 26 ++++++------ ...pril_tag_pose.cpp => transform_sensor.cpp} | 42 +++++++++---------- .../config/fuse_apriltag_tutorial.yaml | 6 +-- .../launch/fuse_apriltag_tutorial.launch.py | 3 +- 7 files changed, 40 insertions(+), 43 deletions(-) rename fuse_models/include/fuse_models/parameters/{april_tag_pose_params.hpp => transform_sensor_params.hpp} (98%) rename fuse_models/include/fuse_models/{april_tag_pose.hpp => transform_sensor.hpp} (89%) rename fuse_models/src/{april_tag_pose.cpp => transform_sensor.cpp} (77%) diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index 3e5a55dcf..355ffc8be 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -43,7 +43,6 @@ find_package(Eigen3 REQUIRED) # Declare a C++ library add_library( ${PROJECT_NAME} - src/april_tag_pose.cpp src/acceleration_2d.cpp src/graph_ignition.cpp src/imu_2d.cpp @@ -55,6 +54,7 @@ add_library( src/odometry_3d_publisher.cpp src/pose_2d.cpp src/transaction.cpp + src/transform_sensor.cpp src/twist_2d.cpp src/unicycle_2d.cpp src/unicycle_2d_ignition.cpp diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index f01a86f5a..dbffe9dca 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -79,7 +79,7 @@ published by another node - + An adapter-type sensor that produces pose constraints from AprilTag detections published from another node diff --git a/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp similarity index 98% rename from fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp rename to fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp index 50b470085..f2e87cfbb 100644 --- a/fuse_models/include/fuse_models/parameters/april_tag_pose_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp @@ -50,7 +50,7 @@ namespace fuse_models::parameters /** * @brief Defines the set of parameters required by the Odometry3D class */ -struct AprilTagPoseParams : public ParameterBase +struct TransformSensorParams : public ParameterBase { public: /** diff --git a/fuse_models/include/fuse_models/april_tag_pose.hpp b/fuse_models/include/fuse_models/transform_sensor.hpp similarity index 89% rename from fuse_models/include/fuse_models/april_tag_pose.hpp rename to fuse_models/include/fuse_models/transform_sensor.hpp index 1cb4efb38..14c9e3577 100644 --- a/fuse_models/include/fuse_models/april_tag_pose.hpp +++ b/fuse_models/include/fuse_models/transform_sensor.hpp @@ -31,8 +31,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FUSE_MODELS__IMU_3D_HPP_ -#define FUSE_MODELS__IMU_3D_HPP_ +#ifndef FUSE_MODELS__TRANSFORM_SENSOR_HPP_ +#define FUSE_MODELS__TRANSFORM_SENSOR_HPP_ #include #include @@ -40,7 +40,7 @@ #include #include -#include +#include #include #include @@ -74,27 +74,27 @@ namespace fuse_models * Subscribes: * - \p topic (MessageType) IMU data at a given timestep */ -class AprilTagPose : public fuse_core::AsyncSensorModel +class TransformSensor : public fuse_core::AsyncSensorModel { public: - FUSE_SMART_PTR_DEFINITIONS(AprilTagPose) - using ParameterType = parameters::AprilTagPoseParams; + FUSE_SMART_PTR_DEFINITIONS(TransformSensor) + using ParameterType = parameters::TransformSensorParams; using MessageType = tf2_msgs::msg::TFMessage; /** * @brief Default constructor */ - AprilTagPose(); + TransformSensor(); /** * @brief Destructor */ - virtual ~AprilTagPose() = default; + virtual ~TransformSensor() = default; - AprilTagPose(AprilTagPose const&) = delete; - AprilTagPose(AprilTagPose&&) = delete; - AprilTagPose& operator=(AprilTagPose const&) = delete; - AprilTagPose& operator=(AprilTagPose&&) = delete; + TransformSensor(TransformSensor const&) = delete; + TransformSensor(TransformSensor&&) = delete; + TransformSensor& operator=(TransformSensor const&) = delete; + TransformSensor& operator=(TransformSensor&&) = delete; /** * @brief Shadowing extension to the AsyncSensorModel::initialize call @@ -153,4 +153,4 @@ class AprilTagPose : public fuse_core::AsyncSensorModel } // namespace fuse_models -#endif // FUSE_MODELS__IMU_3D_HPP_ +#endif // FUSE_MODELS__TRANSFORM_SENSOR_HPP_ diff --git a/fuse_models/src/april_tag_pose.cpp b/fuse_models/src/transform_sensor.cpp similarity index 77% rename from fuse_models/src/april_tag_pose.cpp rename to fuse_models/src/transform_sensor.cpp index 80b5e2470..176b90ee8 100644 --- a/fuse_models/src/april_tag_pose.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -37,40 +37,41 @@ #include #include #include -#include +#include #include #include #include #include #include +#include // Register this sensor model with ROS as a plugin. -PLUGINLIB_EXPORT_CLASS(fuse_models::AprilTagPose, fuse_core::SensorModel) +PLUGINLIB_EXPORT_CLASS(fuse_models::TransformSensor, fuse_core::SensorModel) namespace fuse_models { -AprilTagPose::AprilTagPose() +TransformSensor::TransformSensor() : fuse_core::AsyncSensorModel(1) , device_id_(fuse_core::uuid::NIL) , logger_(rclcpp::get_logger("uninitialized")) - , throttled_callback_(std::bind(&AprilTagPose::process, this, std::placeholders::_1)) + , throttled_callback_(std::bind(&TransformSensor::process, this, std::placeholders::_1)) { } -void AprilTagPose::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) +void TransformSensor::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 AprilTagPose::onInit() +void TransformSensor::onInit() { logger_ = interfaces_.get_node_logging_interface()->get_logger(); clock_ = interfaces_.get_node_clock_interface()->get_clock(); - // Read settings from the parameter sever + // Read settings from the parameter server device_id_ = fuse_variables::loadDeviceId(interfaces_); params_.loadFromROS(interfaces_, name_); @@ -84,8 +85,8 @@ void AprilTagPose::onInit() if (params_.position_indices.empty() && params_.orientation_indices.empty()) { - RCLCPP_WARN_STREAM(logger_, - "No dimensions were specified. Data from topic " << params_.topic << " will be ignored."); + throw std::runtime_error("No dimensions specified, so this sensor would not do anything (data from topic " + + params_.topic + " would be ignored)."); } tf_buffer_ = std::make_unique(clock_); @@ -95,26 +96,23 @@ void AprilTagPose::onInit() interfaces_.get_node_topics_interface()); } -void AprilTagPose::onStart() +void TransformSensor::onStart() { - if (!params_.position_indices.empty() || !params_.orientation_indices.empty()) - { - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = cb_group_; + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; - sub_ = rclcpp::create_subscription(interfaces_, params_.topic, params_.queue_size, - std::bind(&AprilTagThrottledCallback::callback, - &throttled_callback_, std::placeholders::_1), - sub_options); - } + sub_ = rclcpp::create_subscription(interfaces_, params_.topic, params_.queue_size, + std::bind(&AprilTagThrottledCallback::callback, + &throttled_callback_, std::placeholders::_1), + sub_options); } -void AprilTagPose::onStop() +void TransformSensor::onStop() { sub_.reset(); } -void AprilTagPose::process(MessageType const& msg) +void TransformSensor::process(MessageType const& msg) { for (auto const& transform : msg.transforms) { diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml index f395def3e..d91d47523 100644 --- a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -20,8 +20,8 @@ state_estimator: type: fuse_models::Omnidirectional3DIgnition motion_models: [3d_motion_model] ignition: true - apriltag_sensor: - type: fuse_models::AprilTagPose + transform_sensor: + type: fuse_models::TransformSensor motion_models: [3d_motion_model] initial_localization_sensor: @@ -30,7 +30,7 @@ state_estimator: initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - apriltag_sensor: + transform_sensor: topic: 'april_tf' target_frame: 'base_link' position_dimensions: ['x', 'y', 'z'] diff --git a/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py index 4beddf0b3..6df7100cd 100644 --- a/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py +++ b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py @@ -26,8 +26,7 @@ def generate_launch_description(): return LaunchDescription( [ - # tell tf2 that map is the same as odom - # without this, visualization won't work as we have no reference + # create a fixed transform between map and odom for visualization purposes Node( package="tf2_ros", executable="static_transform_publisher", From db45292cb4582ec481facc3b91818d5eacaf3544 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 8 Nov 2024 20:30:11 +0000 Subject: [PATCH 21/39] remove extraneous sleep --- fuse_tutorials/src/apriltag_simulator.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index 6d2fd95da..a35a71718 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -48,7 +48,6 @@ #include #include #include -#include #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -380,13 +379,14 @@ int main(int argc, char** argv) // Generate and publish simulated measurements from the new robot state tf_publisher->publish(aprilTagPoses(new_state)); - std::this_thread::sleep_for(std::chrono::milliseconds(20)); - april_tf_publisher->publish(simulateAprilTag(new_state)); // Wait for the next time step state = new_state; rclcpp::spin_some(node); rate.sleep(); + + // publish simulated position after the static april tag poses since we need them to be in the tf buffer to run + april_tf_publisher->publish(simulateAprilTag(new_state)); } rclcpp::shutdown(); From b48be31d85188a96133d0af489545076a5e44387 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 8 Nov 2024 20:33:59 +0000 Subject: [PATCH 22/39] use more visually pleasing parameters --- fuse_tutorials/config/fuse_apriltag_tutorial.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml index d91d47523..86bbfd8a1 100644 --- a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -2,9 +2,9 @@ state_estimator: ros__parameters: # Fixed-lag smoother configuration - optimization_frequency: 20.0 + optimization_frequency: 100.0 transaction_timeout: 0.01 - lag_duration: 0.5 + lag_duration: 0.1 # all our sensors will be using this motion model motion_models: @@ -49,4 +49,4 @@ state_estimator: map_frame_id: 'map' world_frame_id: 'odom' publish_tf: true - publish_frequency: 10.0 + publish_frequency: 100.0 From 9d32f8af2796a55ef237a4ee792516dbf4311b9b Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 8 Nov 2024 20:50:26 +0000 Subject: [PATCH 23/39] improve the tutorial config file --- fuse_tutorials/config/fuse_apriltag_tutorial.yaml | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml index 86bbfd8a1..51531cbe9 100644 --- a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -4,7 +4,7 @@ state_estimator: # Fixed-lag smoother configuration optimization_frequency: 100.0 transaction_timeout: 0.01 - lag_duration: 0.1 + lag_duration: 0.01 # all our sensors will be using this motion model motion_models: @@ -13,7 +13,8 @@ state_estimator: 3d_motion_model: # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc - process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + # use high values for the acceleration process noise because we aren't measuring the applied forces + process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10., 10., 10.] sensor_models: initial_localization_sensor: @@ -35,6 +36,8 @@ state_estimator: target_frame: 'base_link' position_dimensions: ['x', 'y', 'z'] orientation_dimensions: ['roll', 'pitch', 'yaw'] + # these are the true covariance values used by the simulator; what happens if we change these? + pose_covariance: [0.1, 0.1, 0.1, 0.25, 0.25, 0.25] # this publishes our estimated odometry publishers: From 3456e8ea3bb0c4a58deb0f2424eeef2feb7ee2e2 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 8 Nov 2024 21:12:21 +0000 Subject: [PATCH 24/39] fix bit shift logic :woozy: --- fuse_tutorials/src/apriltag_simulator.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index a35a71718..c1bce41d1 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -188,9 +188,9 @@ tf2_msgs::msg::TFMessage aprilTagPoses(Robot const& robot) // calculate offset of each april tag // we start with offset 1, 1, 1 and switch the z, y, then x as if they were binary digits based off of the april tag // number see the launch file for a more readable offset for each april tag - bool const x_positive = ((i >> 3) & 1) == 0u; - bool const y_positive = ((i >> 2) & 1) == 0u; - bool const z_positive = ((i >> 1) & 1) == 0u; + bool const x_positive = ((i >> 2) & 1) == 0u; + bool const y_positive = ((i >> 1) & 1) == 0u; + bool const z_positive = ((i >> 0) & 1) == 0u; // robot position with offset and noise april_to_base.transform.translation.x = x_positive ? 1. : -1.; @@ -231,9 +231,9 @@ tf2_msgs::msg::TFMessage simulateAprilTag(const Robot& robot) // calculate offset of each april tag // we start with offset 1, 1, 1 and switch the z, y, then x as if they were binary digits based off of the april tag // number see the launch file for a more readable offset for each april tag - bool const x_positive = ((i >> 3) & 1) == 0u; - bool const y_positive = ((i >> 2) & 1) == 0u; - bool const z_positive = ((i >> 1) & 1) == 0u; + bool const x_positive = ((i >> 2) & 1) == 0u; + bool const y_positive = ((i >> 1) & 1) == 0u; + bool const z_positive = ((i >> 0) & 1) == 0u; double const x_offset = x_positive ? 1. : -1.; double const y_offset = y_positive ? 1. : -1.; From 34c5ace3ea11c4c98a6e1724793790ed0194820a Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 8 Nov 2024 23:28:47 +0000 Subject: [PATCH 25/39] resolve TODO --- fuse_optimizers/src/fixed_lag_smoother.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index f8816a299..630ce3d28 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -515,17 +515,12 @@ void FixedLagSmoother::transactionCallback(const std::string& sensor_name, fuse_ // And purge out old transactions // - Either before or exactly at the start time // - Or with a minimum time before the minimum time of this ignition sensor transaction - // - // TODO(efernandez) Do '&min_time = std::as_const(start_ime)' when C++17 is supported and we - // can use std::as_const: - // https://en.cppreference.com/w/cpp/utility/as_const pending_transactions_.erase( std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), - [&sensor_name, max_time, - &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) + [&sensor_name, max_time, &min_time = std::as_const(start_time)](const auto& transaction) { return transaction.sensor_name != sensor_name && (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), // NOLINT(whitespace/braces) + }), pending_transactions_.end()); } else From d06e62219c073a3b6c29c5d2f96f71563b9d3ca6 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 8 Nov 2024 23:29:10 +0000 Subject: [PATCH 26/39] clangd fixes --- fuse_models/include/fuse_models/odometry_3d_publisher.hpp | 4 ++++ .../parameters/odometry_3d_publisher_params.hpp | 2 -- fuse_models/src/odometry_3d_publisher.cpp | 8 +++++--- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp index dc623ad99..729b616ea 100644 --- a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp @@ -118,6 +118,10 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher * @brief Destructor */ virtual ~Odometry3DPublisher() = default; + Odometry3DPublisher(Odometry3DPublisher const&) = delete; + Odometry3DPublisher(Odometry3DPublisher&&) = delete; + Odometry3DPublisher& operator=(Odometry3DPublisher const&) = delete; + Odometry3DPublisher& operator=(Odometry3DPublisher&&) = delete; /** * @brief Shadowing extension to the AsyncPublisher::initialize call diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp index 21cb0a29e..1f0734e3b 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -36,10 +36,8 @@ #include -#include #include #include -#include #include diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index ec2649f91..9693572cc 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -345,7 +345,7 @@ void Odometry3DPublisher::onStart() // TODO(CH3): Add this to a separate callback group for async behavior publish_timer_ = rclcpp::create_timer(interfaces_, clock_, std::chrono::duration(1.0 / params_.publish_frequency), - std::move(std::bind(&Odometry3DPublisher::publishTimerCallback, this)), cb_group_); + std::bind(&Odometry3DPublisher::publishTimerCallback, this), cb_group_); delayed_throttle_filter_.reset(); } @@ -424,7 +424,7 @@ void Odometry3DPublisher::publishTimerCallback() { rclcpp::Time latest_stamp; rclcpp::Time latest_covariance_stamp; - bool latest_covariance_valid; + bool latest_covariance_valid = false; nav_msgs::msg::Odometry odom_output; geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; { @@ -453,7 +453,9 @@ void Odometry3DPublisher::publishTimerCallback() rclcpp::Time timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); // Convert pose in Eigen representation - fuse_core::Vector3d position, velocity_linear, velocity_angular; + fuse_core::Vector3d position; + fuse_core::Vector3d velocity_linear; + fuse_core::Vector3d velocity_angular; Eigen::Quaterniond orientation; position << pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(); orientation.x() = pose.getRotation().x(); From 4653c87288b173d7ea85fb120bb8085258123671 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Sun, 24 Nov 2024 05:06:47 +0000 Subject: [PATCH 27/39] readme update --- README.md | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 3908121b1..de613922d 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,16 @@ # fuse -**NOTE**: The `rolling` branch is a [work in progress](https://github.com/locusrobotics/fuse/issues/276) port of the fuse stack to ROS 2, it is **not** expected to work until said work is done! +Welcome to PickNik Robotics's fork of fuse! -The fuse stack provides a general architecture for performing sensor fusion live on a robot. Some possible applications -include state estimation, localization, mapping, and calibration. +## Getting Started + +Using the Dockerfile is the easiest way to get started. Once inside, simply `rosdep update` and `colcon build` and you're ready to get started! Try `ros2 launch fuse_tutorials fuse_3d_tutorial.launch.py` and look at its pertinent code for a crash course in fuse and its capabilities. ## Overview -fuse is a ROS framework for performing sensor fusion using nonlinear least squares optimization techniques. In -particular, fuse provides: +The fuse stack provides a general architecture for performing sensor fusion live on a robot using nonlinear least squares optimization techniques. +Some possible applications include state estimation, localization, mapping, and calibration. +In particular, fuse provides: * a plugin-based system for modeling sensor measurements * a similar plugin-based system for motion models From 911e5eed1977af71e838cb54c7a9f5de77ebecc5 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Sun, 24 Nov 2024 05:26:55 +0000 Subject: [PATCH 28/39] clang tidy fixes --- fuse_core/include/fuse_core/fuse_macros.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/fuse_core/include/fuse_core/fuse_macros.hpp b/fuse_core/include/fuse_core/fuse_macros.hpp index 6484d12f3..be86909be 100644 --- a/fuse_core/include/fuse_core/fuse_macros.hpp +++ b/fuse_core/include/fuse_core/fuse_macros.hpp @@ -58,10 +58,6 @@ // Required by __MAKE_SHARED_ALIGNED_DEFINITION, that uses Eigen::aligned_allocator(). #include -#include -#include -#include - /** * Creates a custom new() implementation that ensures memory is allocated with proper byte * alignment. This should be added to the public section of classes or structs that contain From 707ebb89cee2a7275f17c1b957917e41cdf66727 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Sun, 24 Nov 2024 05:27:05 +0000 Subject: [PATCH 29/39] remove unused imports --- fuse_tutorials/launch/fuse_3d_tutorial.launch.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/fuse_tutorials/launch/fuse_3d_tutorial.launch.py b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py index f57d66dc1..664d83e50 100755 --- a/fuse_tutorials/launch/fuse_3d_tutorial.launch.py +++ b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py @@ -15,9 +15,8 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import ExecuteProcess from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare From f8fbc2863630bd723a5e11d5222228333bbe3fc3 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Sun, 24 Nov 2024 05:31:27 +0000 Subject: [PATCH 30/39] merge main --- .../parameters/transform_sensor_params.hpp | 2 +- .../include/fuse_models/transform_sensor.hpp | 15 +++++++-------- fuse_optimizers/src/fixed_lag_smoother.cpp | 9 +++++++-- 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp index f2e87cfbb..e6506abe3 100644 --- a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp @@ -48,7 +48,7 @@ namespace fuse_models::parameters { /** - * @brief Defines the set of parameters required by the Odometry3D class + * @brief Defines the set of parameters required by the TransformSensor class */ struct TransformSensorParams : public ParameterBase { diff --git a/fuse_models/include/fuse_models/transform_sensor.hpp b/fuse_models/include/fuse_models/transform_sensor.hpp index 14c9e3577..fa8523dcf 100644 --- a/fuse_models/include/fuse_models/transform_sensor.hpp +++ b/fuse_models/include/fuse_models/transform_sensor.hpp @@ -56,20 +56,19 @@ namespace fuse_models { /** - * @brief An adapter-type sensor that produces pose constraints from AprilTag detections + * @brief An adapter-type sensor that produces pose constraints from published transforms * * This sensor subscribes to a MessageType topic and creates orientation and pose variables and constraints. - * - * This sensor really just separates out the orientation, angular velocity, and linear acceleration - * components of the message, and processes them just like the Pose3D, Twist3D, and Acceleration3D - * classes. + * This sensor can be used for AprilTags or any pose for which the transform to the desired state estimation frame is + * known. For an example, try `ros2 launch fuse_tutorials fuse_apriltag_tutorial.launch.py` and see its relevant files. * * Parameters: * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to * publish * - device_name (string) Used to generate the device/robot ID if the device_id is not provided - * - queue_size (int, default: 10) The subscriber queue size for the pose messages - * - topic (string) The topic to which to subscribe for the pose messages + * - queue_size (int, default: 10) The subscriber queue size for the transform messages + * - topic (string) The topic to which to subscribe for the transform messages + * - target_frame (string) the state estimation frame to transform tfs to * * Subscribes: * - \p topic (MessageType) IMU data at a given timestep @@ -103,7 +102,7 @@ class TransformSensor : public fuse_core::AsyncSensorModel const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** - * @brief Callback for pose messages + * @brief Callback for tf messages * @param[in] msg - The IMU message to process */ void process(const MessageType& msg); diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 630ce3d28..f8816a299 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -515,12 +515,17 @@ void FixedLagSmoother::transactionCallback(const std::string& sensor_name, fuse_ // And purge out old transactions // - Either before or exactly at the start time // - Or with a minimum time before the minimum time of this ignition sensor transaction + // + // TODO(efernandez) Do '&min_time = std::as_const(start_ime)' when C++17 is supported and we + // can use std::as_const: + // https://en.cppreference.com/w/cpp/utility/as_const pending_transactions_.erase( std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), - [&sensor_name, max_time, &min_time = std::as_const(start_time)](const auto& transaction) { + [&sensor_name, max_time, + &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) return transaction.sensor_name != sensor_name && (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), + }), // NOLINT(whitespace/braces) pending_transactions_.end()); } else From 0cfb403f7ace11ee9f017536f511f9a45c9b739d Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Sun, 24 Nov 2024 05:36:32 +0000 Subject: [PATCH 31/39] clang tidy fixes --- fuse_core/include/fuse_core/transaction.hpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/fuse_core/include/fuse_core/transaction.hpp b/fuse_core/include/fuse_core/transaction.hpp index 4449c439d..9651d35bc 100644 --- a/fuse_core/include/fuse_core/transaction.hpp +++ b/fuse_core/include/fuse_core/transaction.hpp @@ -111,7 +111,7 @@ class Transaction /** * @brief Read-only access to this transaction's timestamp */ - const rclcpp::Time& stamp() const + [[nodiscard]] const rclcpp::Time& stamp() const { return stamp_; } @@ -129,7 +129,7 @@ class Transaction * * @return An iterator range containing all involved timestamps, ordered oldest to newest */ - const_stamp_range involvedStamps() const + [[nodiscard]] const_stamp_range involvedStamps() const { return involved_stamps_; } @@ -140,7 +140,7 @@ class Transaction * * @return The minimum (oldest) timestamp. */ - const rclcpp::Time& minStamp() const; + [[nodiscard]] const rclcpp::Time& minStamp() const; /** * @brief Read-only access to the maximum (newest) timestamp among the transaction's stamp and all @@ -148,21 +148,21 @@ class Transaction * * @return The maximum (newest) timestamp. */ - const rclcpp::Time& maxStamp() const; + [[nodiscard]] const rclcpp::Time& maxStamp() const; /** * @brief Read-only access to the added constraints * * @return An iterator range containing all added constraints */ - const_constraint_range addedConstraints() const; + [[nodiscard]] const_constraint_range addedConstraints() const; /** * @brief Read-only access to the removed constraints * * @return An iterator range containing all removed constraint UUIDs */ - const_uuid_range removedConstraints() const + [[nodiscard]] const_uuid_range removedConstraints() const { return removed_constraints_; } @@ -172,14 +172,14 @@ class Transaction * * @return An iterator range containing all added variables */ - const_variable_range addedVariables() const; + [[nodiscard]] const_variable_range addedVariables() const; /** * @brief Read-only access to the removed variables * * @return An iterator range containing all removed variable UUIDs */ - const_uuid_range removedVariables() const + [[nodiscard]] const_uuid_range removedVariables() const { return removed_variables_; } @@ -190,7 +190,7 @@ class Transaction * * @return True if the transaction is empty, false otherwise */ - bool empty() const; + [[nodiscard]] bool empty() const; /** * @brief Add a timestamp to the "involved stamps" collection @@ -275,7 +275,7 @@ class Transaction * * @return A unique pointer to a new instance of the most-derived Variable */ - Transaction::UniquePtr clone() const; + [[nodiscard]] Transaction::UniquePtr clone() const; /** * @brief Serialize this Constraint into the provided binary archive From c3531a7e10e6b3af6d2f6d9649eaf4a9e7aa5fd8 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Sun, 24 Nov 2024 05:41:06 +0000 Subject: [PATCH 32/39] add constructors --- .../include/fuse_optimizers/variable_stamp_index.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp index ab166db75..425ab73a2 100644 --- a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp +++ b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp @@ -69,6 +69,10 @@ class VariableStampIndex * @brief Destructor */ virtual ~VariableStampIndex() = default; + VariableStampIndex(VariableStampIndex const&) = default; + VariableStampIndex(VariableStampIndex&&) = default; + VariableStampIndex& operator=(VariableStampIndex const&) = default; + VariableStampIndex& operator=(VariableStampIndex&&) = default; /** * @brief Return true if no variables exist in the index From ff88f17b4f12c5b195fc94e652734fc405b818fc Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Sun, 24 Nov 2024 05:42:24 +0000 Subject: [PATCH 33/39] clang tidy fixes --- fuse_optimizers/src/variable_stamp_index.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/fuse_optimizers/src/variable_stamp_index.cpp b/fuse_optimizers/src/variable_stamp_index.cpp index a2fbd3a04..032ae1de4 100644 --- a/fuse_optimizers/src/variable_stamp_index.cpp +++ b/fuse_optimizers/src/variable_stamp_index.cpp @@ -33,7 +33,6 @@ */ #include -#include #include #include @@ -54,10 +53,7 @@ rclcpp::Time VariableStampIndex::currentStamp() const { return iter->second; } - else - { - return rclcpp::Time(0, 0, RCL_ROS_TIME); - } + return { 0, 0, RCL_ROS_TIME }; } void VariableStampIndex::addNewTransaction(const fuse_core::Transaction& transaction) @@ -94,8 +90,8 @@ void VariableStampIndex::applyAddedVariables(const fuse_core::Transaction& trans { for (const auto& variable : transaction.addedVariables()) { - auto stamped_variable = dynamic_cast(&variable); - if (stamped_variable) + const auto* stamped_variable = dynamic_cast(&variable); + if (stamped_variable != nullptr) { stamped_index_[variable.uuid()] = stamped_variable->stamp(); } @@ -107,7 +103,7 @@ void VariableStampIndex::applyRemovedConstraints(const fuse_core::Transaction& t { for (const auto& constraint_uuid : transaction.removedConstraints()) { - for (auto& variable_uuid : constraints_[constraint_uuid]) + for (const auto& variable_uuid : constraints_[constraint_uuid]) { variables_[variable_uuid].erase(constraint_uuid); } From 839b79c816bf8a66a77b95fe2e63de590f14a5a8 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Sun, 24 Nov 2024 05:51:12 +0000 Subject: [PATCH 34/39] remove unused includes --- .../include/fuse_constraints/marginalize_variables.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp index 6911376e6..f69c9aad4 100644 --- a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp +++ b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp @@ -36,10 +36,7 @@ #include -#include #include -#include -#include #include #include From 8a78217afc10b39a17ad7c4115d9e6a3e4e80324 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 27 Nov 2024 23:48:50 +0000 Subject: [PATCH 35/39] clang tidy const correctness --- fuse_models/src/odometry_3d_publisher.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 9693572cc..a9b1b36f0 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -124,7 +124,7 @@ void Odometry3DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr if (0u == latest_stamp.nanoseconds()) { { - std::lock_guard lock(mutex_); + std::lock_guard const lock(mutex_); latest_stamp_ = latest_stamp; } @@ -146,7 +146,7 @@ void Odometry3DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr if (!getState(*graph, latest_stamp, device_id_, position_uuid, orientation_uuid, velocity_linear_uuid, velocity_angular_uuid, acceleration_linear_uuid, odom_output, acceleration_output)) { - std::lock_guard lock(mutex_); + std::lock_guard const lock(mutex_); latest_stamp_ = latest_stamp; return; } @@ -324,7 +324,7 @@ void Odometry3DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr } { - std::lock_guard lock(mutex_); + std::lock_guard const lock(mutex_); latest_stamp_ = latest_stamp; latest_covariance_stamp_ = latest_covariance_stamp; @@ -428,7 +428,7 @@ void Odometry3DPublisher::publishTimerCallback() nav_msgs::msg::Odometry odom_output; geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; { - std::lock_guard lock(mutex_); + std::lock_guard const lock(mutex_); latest_stamp = latest_stamp_; latest_covariance_stamp = latest_covariance_stamp_; @@ -450,7 +450,7 @@ void Odometry3DPublisher::publishTimerCallback() // If requested, we need to project our state forward in time using the 3D kinematic model if (params_.predict_to_current_time) { - rclcpp::Time timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); + rclcpp::Time const timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); // Convert pose in Eigen representation fuse_core::Vector3d position; From 48a854854250e0d1252d82c822a83586370f5a83 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 2 Dec 2024 22:57:09 +0000 Subject: [PATCH 36/39] fixes in batch optimizer --- .../include/fuse_optimizers/batch_optimizer.hpp | 10 ++++++---- .../include/fuse_optimizers/batch_optimizer_params.hpp | 2 -- fuse_optimizers/src/batch_optimizer.cpp | 9 +++------ 3 files changed, 9 insertions(+), 12 deletions(-) diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp index d3d905322..969df9624 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp @@ -39,11 +39,9 @@ #include #include #include -#include #include #include #include -#include #include #include @@ -111,13 +109,17 @@ class BatchOptimizer : public Optimizer * @param[in] interfaces The node interfaces for the node driving the optimizer * @param[in] graph The graph used with the optimizer */ - BatchOptimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph = nullptr); + explicit BatchOptimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr); /** * @brief Destructor */ virtual ~BatchOptimizer(); + BatchOptimizer(BatchOptimizer const&) = delete; + BatchOptimizer(BatchOptimizer&&) = delete; + BatchOptimizer& operator=(BatchOptimizer const&) = delete; + BatchOptimizer& operator=(BatchOptimizer&&) = delete; protected: /** diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp index 5d64d5f9f..4977c92d8 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp @@ -37,9 +37,7 @@ #include -#include #include -#include #include #include diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index c87fa4b28..40beb7d9f 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -32,7 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include #include #include @@ -108,11 +107,9 @@ void BatchOptimizer::applyMotionModelsToQueue() pending_transactions_.erase(pending_transactions_.begin()); continue; } - else - { - // Stop processing future transactions. Try again next time. - break; - } + + // Stop processing future transactions. Try again next time. + break; } // Merge the sensor+motion model transactions into a combined transaction that will be applied // directly to the graph From 31e5eb96998a8733fe687f53aaac7117d7fdf062 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 2 Dec 2024 23:44:32 +0000 Subject: [PATCH 37/39] clang tidy fixes --- fuse_optimizers/src/batch_optimizer.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index 40beb7d9f..78b530e99 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -78,7 +78,7 @@ BatchOptimizer::~BatchOptimizer() void BatchOptimizer::applyMotionModelsToQueue() { // We need get the pending transactions from the queue - std::lock_guard pending_transactions_lock(pending_transactions_mutex_); + std::lock_guard const pending_transactions_lock(pending_transactions_mutex_); rclcpp::Time current_time; // Use the most recent transaction time as the current time if (!pending_transactions_.empty()) @@ -114,7 +114,7 @@ void BatchOptimizer::applyMotionModelsToQueue() // Merge the sensor+motion model transactions into a combined transaction that will be applied // directly to the graph { - std::lock_guard combined_transaction_lock(combined_transaction_mutex_); + std::lock_guard const combined_transaction_lock(combined_transaction_mutex_); combined_transaction_->merge(*element.transaction, true); } // We are done with this transaction. Delete it from the queue. @@ -144,7 +144,7 @@ void BatchOptimizer::optimizationLoop() // Copy the combined transaction so it can be shared with all the plugins fuse_core::Transaction::ConstSharedPtr const_transaction; { - std::lock_guard lock(combined_transaction_mutex_); + std::lock_guard const lock(combined_transaction_mutex_); const_transaction = std::move(combined_transaction_); combined_transaction_ = fuse_core::Transaction::make_shared(); } @@ -153,7 +153,7 @@ void BatchOptimizer::optimizationLoop() // Optimize the entire graph graph_->optimize(params_.solver_options); // Make a copy of the graph to share - fuse_core::Graph::ConstSharedPtr const_graph = graph_->clone(); + fuse_core::Graph::ConstSharedPtr const const_graph = graph_->clone(); // Optimization is complete. Notify all the things about the graph changes. notify(const_transaction, const_graph); // Clear the request flag now that this optimization cycle is complete @@ -172,7 +172,7 @@ void BatchOptimizer::optimizerTimerCallback() applyMotionModelsToQueue(); // Check if there is any pending information to be applied to the graph. { - std::lock_guard lock(combined_transaction_mutex_); + std::lock_guard const lock(combined_transaction_mutex_); optimization_request_ = !combined_transaction_->empty(); } // If there is some pending work, trigger the next optimization cycle. @@ -191,11 +191,11 @@ void BatchOptimizer::transactionCallback(const std::string& sensor_name, fuse_co // Or we have "started" already, and the new transaction is after the starting time. auto transaction_clock_type = transaction->stamp().get_clock_type(); - rclcpp::Time transaction_time = transaction->stamp(); + rclcpp::Time const transaction_time = transaction->stamp(); rclcpp::Time last_pending_time(0, 0, transaction_clock_type); // NOTE(CH3): Uninitialized if (!started_ || transaction_time >= start_time_) { - std::lock_guard lock(pending_transactions_mutex_); + std::lock_guard const lock(pending_transactions_mutex_); pending_transactions_.emplace(transaction_time, TransactionQueueElement(sensor_name, std::move(transaction))); last_pending_time = pending_transactions_.rbegin()->first; } @@ -219,7 +219,7 @@ void BatchOptimizer::transactionCallback(const std::string& sensor_name, fuse_co { purge_time = last_pending_time - params_.transaction_timeout; } - std::lock_guard lock(pending_transactions_mutex_); + std::lock_guard const lock(pending_transactions_mutex_); auto purge_iter = pending_transactions_.lower_bound(purge_time); pending_transactions_.erase(pending_transactions_.begin(), purge_iter); } @@ -238,7 +238,7 @@ void BatchOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status.add("Started", started_); { - std::lock_guard lock(pending_transactions_mutex_); + std::lock_guard const lock(pending_transactions_mutex_); status.add("Pending Transactions", pending_transactions_.size()); } } From 6c6f9aa6299e9d840028f0620f7b62203b5fa25b Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 4 Dec 2024 23:48:57 +0000 Subject: [PATCH 38/39] add on-demand prediction --- .../fuse_models/odometry_3d_publisher.hpp | 9 + .../odometry_3d_publisher_params.hpp | 6 + fuse_models/src/odometry_3d_publisher.cpp | 283 ++++++++++-------- .../config/fuse_apriltag_tutorial.yaml | 1 + fuse_tutorials/src/apriltag_simulator.cpp | 6 + 5 files changed, 181 insertions(+), 124 deletions(-) diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp index 729b616ea..4c83af19b 100644 --- a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp @@ -130,6 +130,12 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher const std::string& name) override; protected: + // helper function + void predict(tf2::Transform& pose, nav_msgs::msg::Odometry& odom_output, rclcpp::Time const& to_predict_to, + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output, bool latest_covariance_valid) const; + void publishTF(nav_msgs::msg::Odometry const& odom_output, tf2::Transform& pose); + + void predictTimestampCallback(builtin_interfaces::msg::Time const& time_msg); /** * @brief Perform any required post-construction initialization, such as advertising publishers or * reading from the parameter server. @@ -221,6 +227,9 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Publisher::SharedPtr acceleration_pub_; + rclcpp::Subscription::SharedPtr predict_timestamp_sub_; + rclcpp::Time predict_timestamp_; + std::mutex predict_timestamp_mutex_; std::shared_ptr tf_broadcaster_ = nullptr; std::unique_ptr tf_listener_; diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp index 1f0734e3b..7b52895fa 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -75,6 +75,10 @@ struct Odometry3DPublisherParams : public ParameterBase { publish_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_tf"), publish_tf); invert_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "invert_tf"), invert_tf); + predict_to_future = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "predict_to_future"), predict_to_future); + predict_timestamp_topic = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "predict_timestamp_topic"), predict_timestamp_topic); predict_to_current_time = fuse_core::getParam( interfaces, fuse_core::joinParameterName(ns, "predict_to_current_time"), predict_to_current_time); predict_with_acceleration = fuse_core::getParam( @@ -156,6 +160,8 @@ struct Odometry3DPublisherParams : public ParameterBase std::string base_link_output_frame_id{ base_link_frame_id }; std::string world_frame_id{ odom_frame_id }; std::string topic{ "odometry/filtered" }; + bool predict_to_future{ false }; + std::string predict_timestamp_topic{ "predict_time" }; std::string acceleration_topic{ "acceleration/filtered" }; ceres::Covariance::Options covariance_options; }; diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index a9b1b36f0..aee53ffdd 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -38,6 +38,8 @@ #include #include #include +#include +#include #include #include #include @@ -114,6 +116,16 @@ void Odometry3DPublisher::onInit() rclcpp::create_publisher(interfaces_, params_.topic, params_.queue_size, pub_options); acceleration_pub_ = rclcpp::create_publisher( interfaces_, params_.acceleration_topic, params_.queue_size, pub_options); + + predict_timestamp_sub_ = rclcpp::create_subscription( + interfaces_, params_.predict_timestamp_topic, params_.queue_size, + std::bind(&Odometry3DPublisher::predictTimestampCallback, this, std::placeholders::_1)); +} + +void Odometry3DPublisher::predictTimestampCallback(builtin_interfaces::msg::Time const& time_msg) +{ + std::lock_guard const lock(predict_timestamp_mutex_); + predict_timestamp_ = rclcpp::Time(time_msg); } void Odometry3DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, @@ -420,10 +432,143 @@ bool Odometry3DPublisher::getState(const fuse_core::Graph& graph, const rclcpp:: return true; } +void Odometry3DPublisher::predict(tf2::Transform& pose, nav_msgs::msg::Odometry& odom_output, + rclcpp::Time const& to_predict_to, + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output, + bool latest_covariance_valid) const +{ + const double dt = to_predict_to.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); + // Convert pose in Eigen representation + fuse_core::Vector3d position; + fuse_core::Vector3d velocity_linear; + fuse_core::Vector3d velocity_angular; + Eigen::Quaterniond orientation; + position << pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(); + orientation.x() = pose.getRotation().x(); + orientation.y() = pose.getRotation().y(); + orientation.z() = pose.getRotation().z(); + orientation.w() = pose.getRotation().w(); + velocity_linear << odom_output.twist.twist.linear.x, odom_output.twist.twist.linear.y, + odom_output.twist.twist.linear.z; + velocity_angular << odom_output.twist.twist.angular.x, odom_output.twist.twist.angular.y, + odom_output.twist.twist.angular.z; + + fuse_core::Matrix15d jacobian; + + fuse_core::Vector3d acceleration_linear; + if (params_.predict_with_acceleration) + { + acceleration_linear << acceleration_output.accel.accel.linear.x, acceleration_output.accel.accel.linear.y, + acceleration_output.accel.accel.linear.z; + } + + ::fuse_models::predict(position, orientation, velocity_linear, velocity_angular, acceleration_linear, dt, position, + orientation, velocity_linear, velocity_angular, acceleration_linear, jacobian); + + // Convert back to tf2 representation + pose.setOrigin(tf2::Vector3(position.x(), position.y(), position.z())); + pose.setRotation(tf2::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w())); + + odom_output.pose.pose.position.x = position.x(); + odom_output.pose.pose.position.y = position.y(); + odom_output.pose.pose.position.z = position.z(); + odom_output.pose.pose.orientation.x = orientation.x(); + odom_output.pose.pose.orientation.y = orientation.y(); + odom_output.pose.pose.orientation.z = orientation.z(); + odom_output.pose.pose.orientation.w = orientation.w(); + + odom_output.twist.twist.linear.x = velocity_linear.x(); + odom_output.twist.twist.linear.y = velocity_linear.y(); + odom_output.twist.twist.linear.z = velocity_linear.z(); + odom_output.twist.twist.angular.x = velocity_angular.x(); + odom_output.twist.twist.angular.y = velocity_angular.y(); + odom_output.twist.twist.angular.z = velocity_angular.z(); + + if (params_.predict_with_acceleration) + { + acceleration_output.accel.accel.linear.x = acceleration_linear.x(); + acceleration_output.accel.accel.linear.y = acceleration_linear.y(); + acceleration_output.accel.accel.linear.z = acceleration_linear.z(); + } + + odom_output.header.stamp = to_predict_to; + acceleration_output.header.stamp = to_predict_to; + + // Either the last covariance computation was skipped because there was no subscriber, + // or it failed + if (latest_covariance_valid) + { + fuse_core::Matrix15d covariance; + covariance.setZero(); + Eigen::Map pose_covariance(odom_output.pose.covariance.data()); + Eigen::Map twist_covariance(odom_output.twist.covariance.data()); + Eigen::Map acceleration_covariance(acceleration_output.accel.covariance.data()); + + covariance.block<6, 6>(0, 0) = pose_covariance; + covariance.block<6, 6>(6, 6) = twist_covariance; + covariance.block<3, 3>(12, 12) = acceleration_covariance; + + covariance = jacobian * covariance * jacobian.transpose(); + + auto process_noise_covariance = params_.process_noise_covariance; + if (params_.scale_process_noise) + { + common::scaleProcessNoiseCovariance(process_noise_covariance, velocity_linear, velocity_angular, + params_.velocity_linear_norm_min_, params_.velocity_angular_norm_min_); + } + + covariance.noalias() += dt * process_noise_covariance; + + pose_covariance = covariance.block<6, 6>(0, 0); + twist_covariance = covariance.block<6, 6>(6, 6); + acceleration_covariance = covariance.block<3, 3>(12, 12); + } +} + +void Odometry3DPublisher::publishTF(nav_msgs::msg::Odometry const& odom_output, tf2::Transform& pose) +{ + auto frame_id = odom_output.header.frame_id; + auto child_frame_id = odom_output.child_frame_id; + + if (params_.invert_tf) + { + pose = pose.inverse(); + std::swap(frame_id, child_frame_id); + } + + 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; + trans.transform = tf2::toMsg(pose); + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) + { + try + { + auto base_to_odom = tf_buffer_->lookupTransform(params_.base_link_frame_id, params_.odom_frame_id, + trans.header.stamp, params_.tf_timeout); + + 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; + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Could not lookup the " << params_.base_link_frame_id << "->" << params_.odom_frame_id + << " transform. Error: " << e.what()); + + return; + } + } + + tf_broadcaster_->sendTransform(trans); +} + void Odometry3DPublisher::publishTimerCallback() { rclcpp::Time latest_stamp; - rclcpp::Time latest_covariance_stamp; bool latest_covariance_valid = false; nav_msgs::msg::Odometry odom_output; geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; @@ -431,12 +576,13 @@ void Odometry3DPublisher::publishTimerCallback() std::lock_guard const lock(mutex_); latest_stamp = latest_stamp_; - latest_covariance_stamp = latest_covariance_stamp_; latest_covariance_valid = latest_covariance_valid_; odom_output = odom_output_; acceleration_output = acceleration_output_; } + nav_msgs::msg::Odometry odom_output_predict = odom_output; + if (0u == latest_stamp.nanoseconds()) { RCLCPP_WARN_STREAM_EXPRESSION(logger_, delayed_throttle_filter_.isEnabled(), @@ -447,98 +593,14 @@ void Odometry3DPublisher::publishTimerCallback() tf2::Transform pose; tf2::fromMsg(odom_output.pose.pose, pose); + tf2::Transform pose_predict; + tf2::fromMsg(odom_output_predict.pose.pose, pose); + // If requested, we need to project our state forward in time using the 3D kinematic model if (params_.predict_to_current_time) { rclcpp::Time const timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); - - // Convert pose in Eigen representation - fuse_core::Vector3d position; - fuse_core::Vector3d velocity_linear; - fuse_core::Vector3d velocity_angular; - Eigen::Quaterniond orientation; - position << pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(); - orientation.x() = pose.getRotation().x(); - orientation.y() = pose.getRotation().y(); - orientation.z() = pose.getRotation().z(); - orientation.w() = pose.getRotation().w(); - velocity_linear << odom_output.twist.twist.linear.x, odom_output.twist.twist.linear.y, - odom_output.twist.twist.linear.z; - velocity_angular << odom_output.twist.twist.angular.x, odom_output.twist.twist.angular.y, - odom_output.twist.twist.angular.z; - - const double dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); - - fuse_core::Matrix15d jacobian; - - fuse_core::Vector3d acceleration_linear; - if (params_.predict_with_acceleration) - { - acceleration_linear << acceleration_output.accel.accel.linear.x, acceleration_output.accel.accel.linear.y, - acceleration_output.accel.accel.linear.z; - } - - predict(position, orientation, velocity_linear, velocity_angular, acceleration_linear, dt, position, orientation, - velocity_linear, velocity_angular, acceleration_linear, jacobian); - - // Convert back to tf2 representation - pose.setOrigin(tf2::Vector3(position.x(), position.y(), position.z())); - pose.setRotation(tf2::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w())); - - odom_output.pose.pose.position.x = position.x(); - odom_output.pose.pose.position.y = position.y(); - odom_output.pose.pose.position.z = position.z(); - odom_output.pose.pose.orientation.x = orientation.x(); - odom_output.pose.pose.orientation.y = orientation.y(); - odom_output.pose.pose.orientation.z = orientation.z(); - odom_output.pose.pose.orientation.w = orientation.w(); - - odom_output.twist.twist.linear.x = velocity_linear.x(); - odom_output.twist.twist.linear.y = velocity_linear.y(); - odom_output.twist.twist.linear.z = velocity_linear.z(); - odom_output.twist.twist.angular.x = velocity_angular.x(); - odom_output.twist.twist.angular.y = velocity_angular.y(); - odom_output.twist.twist.angular.z = velocity_angular.z(); - - if (params_.predict_with_acceleration) - { - acceleration_output.accel.accel.linear.x = acceleration_linear.x(); - acceleration_output.accel.accel.linear.y = acceleration_linear.y(); - acceleration_output.accel.accel.linear.z = acceleration_linear.z(); - } - - 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 - if (latest_covariance_valid) - { - fuse_core::Matrix15d covariance; - covariance.setZero(); - Eigen::Map pose_covariance(odom_output.pose.covariance.data()); - Eigen::Map twist_covariance(odom_output.twist.covariance.data()); - Eigen::Map acceleration_covariance(acceleration_output.accel.covariance.data()); - - covariance.block<6, 6>(0, 0) = pose_covariance; - covariance.block<6, 6>(6, 6) = twist_covariance; - covariance.block<3, 3>(12, 12) = acceleration_covariance; - - covariance = jacobian * covariance * jacobian.transpose(); - - auto process_noise_covariance = params_.process_noise_covariance; - if (params_.scale_process_noise) - { - common::scaleProcessNoiseCovariance(process_noise_covariance, velocity_linear, velocity_angular, - params_.velocity_linear_norm_min_, params_.velocity_angular_norm_min_); - } - - covariance.noalias() += dt * process_noise_covariance; - - pose_covariance = covariance.block<6, 6>(0, 0); - twist_covariance = covariance.block<6, 6>(6, 6); - acceleration_covariance = covariance.block<3, 3>(12, 12); - } + predict(pose, odom_output, timer_now, acceleration_output, latest_covariance_valid); } odom_pub_->publish(odom_output); @@ -546,44 +608,17 @@ void Odometry3DPublisher::publishTimerCallback() if (params_.publish_tf) { - auto frame_id = odom_output.header.frame_id; - auto child_frame_id = odom_output.child_frame_id; - - if (params_.invert_tf) + if (params_.predict_to_future) { - pose = pose.inverse(); - std::swap(frame_id, child_frame_id); - } - - 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; - trans.transform = tf2::toMsg(pose); - if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) - { - try - { - auto base_to_odom = tf_buffer_->lookupTransform(params_.base_link_frame_id, params_.odom_frame_id, - trans.header.stamp, params_.tf_timeout); - - 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; - } - catch (const std::exception& e) + rclcpp::Time predict_time; { - RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, - "Could not lookup the " << params_.base_link_frame_id << "->" - << params_.odom_frame_id - << " transform. Error: " << e.what()); - - return; + std::lock_guard const lock(predict_timestamp_mutex_); + predict_time = predict_timestamp_; } + predict(pose_predict, odom_output_predict, predict_time, acceleration_output, latest_covariance_valid); + publishTF(odom_output_predict, pose_predict); } - - tf_broadcaster_->sendTransform(trans); + publishTF(odom_output, pose); } } diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml index 51531cbe9..b514c02b4 100644 --- a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -53,3 +53,4 @@ state_estimator: world_frame_id: 'odom' publish_tf: true publish_frequency: 100.0 + predict_to_future: true diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index c1bce41d1..4af84b331 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -314,6 +314,7 @@ int main(int argc, char** argv) // create the ground truth publisher auto ground_truth_publisher = node->create_publisher("ground_truth", 1); + auto predict_time_publisher = node->create_publisher("predict_time", 1); // Initialize the robot state (state variables are zero-initialized) auto state = Robot(); @@ -336,6 +337,11 @@ int main(int argc, char** argv) // store the first time this runs (since it won't start running exactly at a multiple of `motion_duration`) static auto const firstTime = node->now(); auto const now = node->now(); + builtin_interfaces::msg::Time predict_time(now); + + // predict into the future + predict_time.sec += 10; + predict_time_publisher->publish(predict_time); // compensate for the original time offset double const now_d = (now - firstTime).seconds(); From 45574806f4d93748d68d94abed2832641b78d97f Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 6 Dec 2024 00:23:43 +0000 Subject: [PATCH 39/39] got prediction fully working, added to config --- .../fuse_models/odometry_3d_publisher.hpp | 1 + .../odometry_3d_publisher_params.hpp | 1 + fuse_models/src/odometry_3d_publisher.cpp | 37 +-- .../config/fuse_apriltag_tutorial.rviz | 253 ++++++++++++++++++ .../config/fuse_apriltag_tutorial.yaml | 2 +- .../launch/fuse_apriltag_tutorial.launch.py | 2 +- fuse_tutorials/src/apriltag_simulator.cpp | 3 +- 7 files changed, 281 insertions(+), 18 deletions(-) create mode 100644 fuse_tutorials/config/fuse_apriltag_tutorial.rviz diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp index 4c83af19b..8f4853dc1 100644 --- a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp @@ -226,6 +226,7 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher std::unique_ptr tf_buffer_; rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr odom_pub_predict_; rclcpp::Publisher::SharedPtr acceleration_pub_; rclcpp::Subscription::SharedPtr predict_timestamp_sub_; rclcpp::Time predict_timestamp_; diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp index 7b52895fa..e74fdcb5e 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -162,6 +162,7 @@ struct Odometry3DPublisherParams : public ParameterBase std::string topic{ "odometry/filtered" }; bool predict_to_future{ false }; std::string predict_timestamp_topic{ "predict_time" }; + std::string predict_topic{ "predicted_odometry/filtered" }; std::string acceleration_topic{ "acceleration/filtered" }; ceres::Covariance::Options covariance_options; }; diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index aee53ffdd..202161b46 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -114,12 +114,19 @@ void Odometry3DPublisher::onInit() odom_pub_ = rclcpp::create_publisher(interfaces_, params_.topic, params_.queue_size, pub_options); + + if (params_.predict_to_future) + { + odom_pub_predict_ = rclcpp::create_publisher(interfaces_, params_.predict_topic, + params_.queue_size, pub_options); + + predict_timestamp_sub_ = rclcpp::create_subscription( + interfaces_, params_.predict_timestamp_topic, params_.queue_size, + std::bind(&Odometry3DPublisher::predictTimestampCallback, this, std::placeholders::_1)); + } + acceleration_pub_ = rclcpp::create_publisher( interfaces_, params_.acceleration_topic, params_.queue_size, pub_options); - - predict_timestamp_sub_ = rclcpp::create_subscription( - interfaces_, params_.predict_timestamp_topic, params_.queue_size, - std::bind(&Odometry3DPublisher::predictTimestampCallback, this, std::placeholders::_1)); } void Odometry3DPublisher::predictTimestampCallback(builtin_interfaces::msg::Time const& time_msg) @@ -594,7 +601,7 @@ void Odometry3DPublisher::publishTimerCallback() tf2::fromMsg(odom_output.pose.pose, pose); tf2::Transform pose_predict; - tf2::fromMsg(odom_output_predict.pose.pose, pose); + tf2::fromMsg(odom_output_predict.pose.pose, pose_predict); // If requested, we need to project our state forward in time using the 3D kinematic model if (params_.predict_to_current_time) @@ -605,19 +612,19 @@ void Odometry3DPublisher::publishTimerCallback() odom_pub_->publish(odom_output); acceleration_pub_->publish(acceleration_output); - - if (params_.publish_tf) + if (params_.predict_to_future) { - if (params_.predict_to_future) + rclcpp::Time predict_time; { - rclcpp::Time predict_time; - { - std::lock_guard const lock(predict_timestamp_mutex_); - predict_time = predict_timestamp_; - } - predict(pose_predict, odom_output_predict, predict_time, acceleration_output, latest_covariance_valid); - publishTF(odom_output_predict, pose_predict); + std::lock_guard const lock(predict_timestamp_mutex_); + predict_time = predict_timestamp_; } + predict(pose_predict, odom_output_predict, predict_time, acceleration_output, latest_covariance_valid); + odom_pub_predict_->publish(odom_output_predict); + } + + if (params_.publish_tf) + { publishTF(odom_output, pose); } } diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.rviz b/fuse_tutorials/config/fuse_apriltag_tutorial.rviz new file mode 100644 index 000000000..832ca6afc --- /dev/null +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.rviz @@ -0,0 +1,253 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Odometry1/Shape1 + - /Odometry2/Shape1 + - /Odometry3 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 0; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ground_truth + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 0; 255; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_filtered + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /predicted_odometry/filtered + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10.72309398651123 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 60 + Y: 60 diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml index b514c02b4..5e1605030 100644 --- a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -14,7 +14,7 @@ state_estimator: 3d_motion_model: # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc # use high values for the acceleration process noise because we aren't measuring the applied forces - process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10., 10., 10.] + process_noise_diagonal: [0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 10., 10., 10.] sensor_models: initial_localization_sensor: diff --git a/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py index 6df7100cd..62e4042a5 100644 --- a/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py +++ b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py @@ -59,7 +59,7 @@ def generate_launch_description(): "-d", [ PathJoinSubstitution( - [pkg_dir, "config", "fuse_3d_tutorial.rviz"] + [pkg_dir, "config", "fuse_apriltag_tutorial.rviz"] ) ], ], diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index 4af84b331..43e952834 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -62,6 +62,7 @@ constexpr double aprilTagOrientationSigma = 0.25; //!< the april tag orientatio constexpr size_t numAprilTags = 8; //!< the number of april tags constexpr double detectionProbability = 0.5; //!< the probability that any given april tag is detectable on a given tick of the simulation +constexpr double futurePredictionTimeSeconds = 0.1; } // namespace /** @@ -340,7 +341,7 @@ int main(int argc, char** argv) builtin_interfaces::msg::Time predict_time(now); // predict into the future - predict_time.sec += 10; + predict_time.nanosec += static_cast(futurePredictionTimeSeconds * 1e9); predict_time_publisher->publish(predict_time); // compensate for the original time offset