diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index 14f255ec..d3f69ef0 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -317,9 +317,9 @@ static inline void quaternionToAngleAxis(const double* q, double* angle_axis, do else { jacobian_map.setZero(); - jacobian_map(0, 0) = 2.0; - jacobian_map(1, 1) = 2.0; - jacobian_map(2, 2) = 2.0; + jacobian_map(0, 1) = 2.0; + jacobian_map(1, 2) = 2.0; + jacobian_map(2, 3) = 2.0; } } } diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 095ab2bf..954714a1 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2020, Clearpath Robotics + * Copyright (c) 2024, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -214,25 +215,50 @@ TEST(Util, quaternionProduct) TEST(Util, quaternionToAngleAxis) { - // Test correct quaternion to angle-axis function jacobian - const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); - double angle_axis[3]; - double q[4]{ q_eigen.w(), q_eigen.x(), q_eigen.y(), q_eigen.z() }; + // Test correct quaternion to angle-axis function jacobian, for quaternions representing non-zero rotation + { + const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); + double angle_axis[3]; + double q[4]{ q_eigen.w(), q_eigen.x(), q_eigen.y(), q_eigen.z() }; - double J_analytic[12]; - double J_autodiff[12]; + double J_analytic[12]; + double J_autodiff[12]; - fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); - double* jacobians[1] = { J_autodiff }; - const double* parameters[1] = { q }; + double* jacobians[1] = { J_autodiff }; + const double* parameters[1] = { q }; - ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create(); - double angle_axis_autodiff[3]; - quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); + ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create(); + double angle_axis_autodiff[3]; + quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); - Eigen::Map> J_autodiff_map(jacobians[0]); - Eigen::Map> J_analytic_map(J_analytic); + Eigen::Map> J_autodiff_map(jacobians[0]); + Eigen::Map> J_analytic_map(J_analytic); - EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); + } + + // Test correct quaternion to angle-axis function jacobian, for quaternions representing zero rotation + { + double angle_axis[3]; + double q[4]{ 1.0, 0.0, 0.0, 0.0 }; + + double J_analytic[12]; + double J_autodiff[12]; + + fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + + double* jacobians[1] = { J_autodiff }; + const double* parameters[1] = { q }; + + ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create(); + double angle_axis_autodiff[3]; + quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); + + Eigen::Map> J_autodiff_map(jacobians[0]); + Eigen::Map> J_analytic_map(J_analytic); + + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); + } } diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index 4706d298..e6be1b84 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -55,7 +55,7 @@ namespace fuse_models * @brief A fuse_models 3D kinematic model that generates kinematic constraints between provided * time stamps, and adds those constraints to the fuse graph. * - * This class uses an omnidirectional kinematic model for the robot. It is equivalent to the motion model + * This class uses a omnidirectional kinematic model for the robot. It is equivalent to the motion model * in the robot_localization state estimation nodes. * * Parameters: diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp index 93620c86..1de55c27 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.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__UNICYCLE_3D_IGNITION_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_IGNITION_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_IGNITION_HPP_ #include #include @@ -57,7 +57,7 @@ namespace fuse_models * motion model. * * This class publishes a transaction that contains a prior on each state subvariable used in the - * Omnidirectional 3D motion model (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * omnidirectional 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). When the sensor is first loaded, it publishes a single transaction * with the configured initial state and covariance. * Additionally, whenever a pose is received, either on the set_pose service or the topic, this @@ -205,4 +205,4 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel } // namespace fuse_models -#endif // FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_IGNITION_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp index 16755462..ab64cd35 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_predict.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__UNICYCLE_3D_PREDICT_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_PREDICT_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_PREDICT_HPP_ #include @@ -503,4 +503,4 @@ inline void predict(const fuse_core::Vector3d& position1, const Eigen::Quaternio } // namespace fuse_models -#endif // FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_PREDICT_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp index 0fcaf906..f5ff0fec 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.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__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTION_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTION_HPP_ #include @@ -278,4 +278,4 @@ Omnidirectional3DStateCostFunction::Omnidirectional3DStateCostFunction(const dou } // namespace fuse_models -#endif // FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTION_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp index 0ccafd8f..d0425676 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.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__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTOR_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTOR_HPP_ #include @@ -185,4 +185,4 @@ bool Omnidirectional3DStateCostFunctor::operator()(const T* const position1, con } // namespace fuse_models -#endif // FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTOR_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp index 18273e19..4c7bbfaf 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.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__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ #include #include @@ -189,4 +189,4 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint BOOST_CLASS_EXPORT_KEY(fuse_models::Omnidirectional3DStateKinematicConstraint); -#endif // FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp index b84d1c25..a187989f 100644 --- a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.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__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ -#define FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ +#ifndef FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_ #include #include @@ -175,4 +175,4 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase } // namespace fuse_models -#endif // FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ +#endif // FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_ diff --git a/fuse_models/src/omnidirectional_3d_ignition.cpp b/fuse_models/src/omnidirectional_3d_ignition.cpp index 55c64a60..a70c0831 100644 --- a/fuse_models/src/omnidirectional_3d_ignition.cpp +++ b/fuse_models/src/omnidirectional_3d_ignition.cpp @@ -133,8 +133,7 @@ void Omnidirectional3DIgnition::start() { auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); tf2::Quaternion q; - // q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]); - q.setEuler(params_.initial_state[5], params_.initial_state[4], params_.initial_state[3]); + q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]); pose.header.stamp = clock_->now(); pose.pose.pose.position.x = params_.initial_state[0]; pose.pose.pose.position.y = params_.initial_state[1]; diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt index 53584343..71b7009e 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/fuse_models/test/CMakeLists.txt @@ -8,7 +8,8 @@ set(TEST_TARGETS test_unicycle_2d_ignition test_omnidirectional_3d test_omnidirectional_3d_predict - test_omnidirectional_3d_state_cost_function) + test_omnidirectional_3d_state_cost_function + test_omnidirectional_3d_ignition) foreach(test_name ${TEST_TARGETS}) ament_add_gtest("${test_name}" "${test_name}.cpp") diff --git a/fuse_models/test/test_omnidirectional_3d_ignition.cpp b/fuse_models/test/test_omnidirectional_3d_ignition.cpp new file mode 100644 index 00000000..2dc1e8b3 --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d_ignition.cpp @@ -0,0 +1,463 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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. + */ +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint; +using fuse_constraints::AbsoluteOrientation3DStampedConstraint; +using fuse_constraints::AbsolutePosition3DStampedConstraint; +using fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint; +using fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint; + +/** + * @brief Promise used to communicate between the tests and the callback + */ +std::promise callback_promise; + +/** + * @brief Transaction callback that forwards the transaction into the promise result + */ +void transactionCallback(fuse_core::Transaction::SharedPtr transaction) +{ + callback_promise.set_value(std::move(transaction)); +} + +/** + * @brief Helper function for fetching the desired constraint from a transaction + */ +template +const Derived* getConstraint(const fuse_core::Transaction& transaction) +{ + for (const auto& constraint : transaction.addedConstraints()) + { + auto derived = dynamic_cast(&constraint); + if (derived) + { + return derived; + } + } + return nullptr; +} + +class Omnidirectional3DIgnitionTestFixture : public ::testing::Test +{ +public: + Omnidirectional3DIgnitionTestFixture() + { + } + + void SetUp() override + { + rclcpp::init(0, nullptr); + executor_ = std::make_shared(); + spinner_ = std::thread([&]() { executor_->spin(); }); + } + + void TearDown() override + { + executor_->cancel(); + if (spinner_.joinable()) + { + spinner_.join(); + } + executor_.reset(); + rclcpp::shutdown(); + } + + std::thread spinner_; //!< Internal thread for spinning the executor + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; +}; + +TEST_F(Omnidirectional3DIgnitionTestFixture, InitialTransaction) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]" }); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + // The ignition sensor should publish a transaction immediately. Wait for the callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(status == std::future_status::ready); + + // Check the transaction + auto transaction = callback_future.get(); + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 1.2, 2.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 1.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 9.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 0.2, 0.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 16.0, 0.0, 0.0, 0.0, 25.0, 0.0, 0.0, 0.0, 36.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + fuse_variables::Orientation3DStamped orientation_actual; + orientation_actual.w() = actual->mean()[0]; + orientation_actual.x() = actual->mean()[1]; + orientation_actual.y() = actual->mean()[2]; + orientation_actual.z() = actual->mean()[3]; + EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-9); + EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-9); + EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 6.7, 7.8, 8.9; + fuse_core::Matrix3d expected_cov; + expected_cov << 49.0, 0.0, 0.0, 0.0, 64.0, 0.0, 0.0, 0.0, 81.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 9.1, 10.2, 11.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 100.0, 0.0, 0.0, 0.0, 121.0, 0.0, 0.0, 0.0, 144.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 12.4, 13.5, 14.6; + fuse_core::Matrix3d expected_cov; + expected_cov << 169.0, 0.0, 0.0, 0.0, 196.0, 0.0, 0.0, 0.0, 225.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } +} + +TEST_F(Omnidirectional3DIgnitionTestFixture, SkipInitialTransaction) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]", + "-p", "ignition_sensor.reset_service:=''", "-p", "ignition_sensor.publish_on_startup:=false" }); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + // The ignition sensor should publish a transaction immediately. Wait for the callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_FALSE(status == std::future_status::ready); +} + +TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseService) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]", + "-p", "ignition_sensor.reset_service:=''", "-p", "ignition_sensor.publish_on_startup:=false" }); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()); // roll, pitch, yaw = 0.1 rad + + // Call the SetPose service + auto srv = std::make_shared(); + srv->pose.header.stamp = rclcpp::Time(12, 345678910); + srv->pose.pose.pose.position.x = 1.0; + srv->pose.pose.pose.position.y = 2.0; + srv->pose.pose.pose.position.z = 3.0; + 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.pose.orientation.w = q.w(); + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 2.0; + srv->pose.pose.covariance[14] = 3.0; + srv->pose.pose.covariance[21] = 4.0; + srv->pose.pose.covariance[28] = 5.0; + srv->pose.pose.covariance[35] = 6.0; + auto client = node->create_client("/omnidirectional_3d_ignition_test/set_pose"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto result = client->async_send_request(srv); + ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); + EXPECT_TRUE(result.get()->success); + + // The ignition sensor should publish a transaction in response to the service call. Wait for the + // callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(status == std::future_status::ready); + + // Check the transaction + auto transaction = callback_future.get(); + { + fuse_core::Vector3d expected_mean; + expected_mean << 1.0, 2.0, 3.0; + fuse_core::Matrix3d expected_cov; + expected_cov << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 0.1, 0.1; + fuse_core::Matrix3d expected_cov; + expected_cov << 4.0, 0.0, 0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 6.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + fuse_variables::Orientation3DStamped orientation_actual; + orientation_actual.w() = actual->mean()[0]; + orientation_actual.x() = actual->mean()[1]; + orientation_actual.y() = actual->mean()[2]; + orientation_actual.z() = actual->mean()[3]; + EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-1); + EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-1); + EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-1); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 6.7, 7.8, 8.9; + fuse_core::Matrix3d expected_cov; + expected_cov << 49.0, 0.0, 0.0, 0.0, 64.0, 0.0, 0.0, 0.0, 81.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 9.1, 10.2, 11.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 100.0, 0.0, 0.0, 0.0, 121.0, 0.0, 0.0, 0.0, 144.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 12.4, 13.5, 14.6; + fuse_core::Matrix3d expected_cov; + expected_cov << 169.0, 0.0, 0.0, 0.0, 196.0, 0.0, 0.0, 0.0, 225.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } +} + +TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseDeprecatedService) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]", + "-p", "ignition_sensor.reset_service:=''", "-p", "ignition_sensor.publish_on_startup:=false" }); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()); // roll, pitch, yaw = 0.1 rad + + // Call the SetPose service + auto srv = std::make_shared(); + srv->pose.header.stamp = rclcpp::Time(12, 345678910); + srv->pose.pose.pose.position.x = 1.0; + srv->pose.pose.pose.position.y = 2.0; + srv->pose.pose.pose.position.z = 3.0; + 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.pose.orientation.w = q.w(); + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 2.0; + srv->pose.pose.covariance[14] = 3.0; + srv->pose.pose.covariance[21] = 4.0; + srv->pose.pose.covariance[28] = 5.0; + srv->pose.pose.covariance[35] = 6.0; + auto client = + node->create_client("/omnidirectional_3d_ignition_test/set_pose_deprecated"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto result = client->async_send_request(srv); + ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); + + // The ignition sensor should publish a transaction in response to the service call. Wait for the + // callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(status == std::future_status::ready); + + // Check the transaction + auto transaction = callback_future.get(); + { + fuse_core::Vector3d expected_mean; + expected_mean << 1.0, 2.0, 3.0; + fuse_core::Matrix3d expected_cov; + expected_cov << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 0.1, 0.1; + fuse_core::Matrix3d expected_cov; + expected_cov << 4.0, 0.0, 0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 6.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + fuse_variables::Orientation3DStamped orientation_actual; + orientation_actual.w() = actual->mean()[0]; + orientation_actual.x() = actual->mean()[1]; + orientation_actual.y() = actual->mean()[2]; + orientation_actual.z() = actual->mean()[3]; + EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-1); // not high precision + EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-1); + EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-1); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 6.7, 7.8, 8.9; + fuse_core::Matrix3d expected_cov; + expected_cov << 49.0, 0.0, 0.0, 0.0, 64.0, 0.0, 0.0, 0.0, 81.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 9.1, 10.2, 11.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 100.0, 0.0, 0.0, 0.0, 121.0, 0.0, 0.0, 0.0, 144.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 12.4, 13.5, 14.6; + fuse_core::Matrix3d expected_cov; + expected_cov << 169.0, 0.0, 0.0, 0.0, 196.0, 0.0, 0.0, 0.0, 225.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } +} diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 10c4e819..3fcd33be 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -229,8 +229,6 @@ void FixedLagSmoother::optimizationLoop() // Optimize the entire graph summary_ = graph_->optimize(params_.solver_options); - // std::cout << summary_.FullReport() << std::endl; - // Optimization is complete. Notify all the things about the graph changes. const auto new_transaction_stamp = new_transaction->stamp(); notify(std::move(new_transaction), graph_->clone());