From 1c0abf8a678e3df3f26da434c32410558f5bb501 Mon Sep 17 00:00:00 2001 From: David Murdoch Date: Thu, 30 Jun 2022 08:23:30 -0400 Subject: [PATCH 01/24] Adding singleton and reset method to Optimizer --- fuse_optimizers/include/fuse_optimizers/optimizer.h | 10 ++++++++++ fuse_optimizers/src/optimizer.cpp | 5 +++++ 2 files changed, 15 insertions(+) diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index 20f16f736..d8250a0b1 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -113,6 +113,8 @@ class Optimizer */ virtual ~Optimizer(); + static Optimizer* getOptimizer() { return singleton_; } + protected: // The unique ptrs returned by pluginlib have a custom deleter. This makes specifying the type rather annoying // as it is not equivalent to Class::UniquePtr @@ -163,6 +165,7 @@ class Optimizer diagnostic_updater::Updater diagnostic_updater_; //!< Diagnostic updater ros::Timer diagnostic_updater_timer_; //!< Diagnostic updater timer double diagnostic_updater_timer_period_{ 1.0 }; //!< Diagnostic updater timer period in seconds + static Optimizer* singleton_; // Date: Thu, 30 Jun 2022 13:05:00 -0400 Subject: [PATCH 02/24] Updating fixed lag smoother --- .../fuse_optimizers/fixed_lag_smoother.h | 7 ++++ fuse_optimizers/src/fixed_lag_smoother.cpp | 35 +++++++++++++++++++ 2 files changed, 42 insertions(+) diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index eefacac2a..da64571ff 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -303,6 +303,13 @@ class FixedLagSmoother : public Optimizer const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; + /** + * @brief Method exposed for resetting the optimizer in the event of an error. Replaces + * the need to throw exceptions when things go wrong. + * @param[in] err_msg A specific error message that gives a reason for the reset + */ + void resetOptimizer(const std::string& err_msg) override; + /** * @brief Update and publish diagnotics * @param[in] status The diagnostic status diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index a91850825..29e69def5 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -632,4 +632,39 @@ void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrappe } } +void FixedLagSmoother::resetOptimizer(const std::string& err_msg) +{ + ROS_ERROR(err_msg.c_str()); + // Tell all the plugins to stop + stopPlugins(); + // Reset the optimizer state + { + std::lock_guard lock(optimization_requested_mutex_); + optimization_request_ = false; + } + started_ = false; + ignited_ = false; + setStartTime(ros::Time(0, 0)); + // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the + // pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to + // prevent the possibility of deadlocks. + { + std::lock_guard lock(optimization_mutex_); + // Clear all pending transactions + { + std::lock_guard lock(pending_transactions_mutex_); + pending_transactions_.clear(); + } + // Clear the graph and marginal tracking states + graph_->clear(); + marginal_transaction_ = fuse_core::Transaction(); + timestamp_tracking_.clear(); + lag_expiration_ = ros::Time(0, 0); + } + // Tell all the plugins to start + startPlugins(); + // Test for auto-start + autostart(); +} + } // namespace fuse_optimizers From 2074eb10a8c6697dca799a0cacf9e3714a1e67be Mon Sep 17 00:00:00 2001 From: David Murdoch Date: Fri, 22 Jul 2022 10:18:11 -0400 Subject: [PATCH 03/24] Beginning error handler definitions --- fuse_core/include/fuse_core/error_handler.h | 116 ++++++++++++++++++ fuse_models/CMakeLists.txt | 1 + fuse_models/fuse_plugins.xml | 5 + .../include/fuse_models/basic_error_handler.h | 115 +++++++++++++++++ fuse_models/src/basic_error_handler.cpp | 61 +++++++++ .../include/fuse_optimizers/optimizer.h | 6 +- fuse_optimizers/src/fixed_lag_smoother.cpp | 2 +- fuse_optimizers/src/optimizer.cpp | 11 +- 8 files changed, 309 insertions(+), 8 deletions(-) create mode 100644 fuse_core/include/fuse_core/error_handler.h create mode 100644 fuse_models/include/fuse_models/basic_error_handler.h create mode 100644 fuse_models/src/basic_error_handler.cpp diff --git a/fuse_core/include/fuse_core/error_handler.h b/fuse_core/include/fuse_core/error_handler.h new file mode 100644 index 000000000..c2ffe40f3 --- /dev/null +++ b/fuse_core/include/fuse_core/error_handler.h @@ -0,0 +1,116 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Locus 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. + */ +#ifndef FUSE_CORE_ERROR_HANDLER_H +#define FUSE_CORE_ERROR_HANDLER_H + +#include + +#include + + +namespace fuse_core +{ + +/** + * @brief The interface definition for error handler plugins in the fuse ecosystem. + * + * An error handler plugin is responsible for handling any errors that may come up in execution of the program. + */ +class ErrorHandler +{ +public: + FUSE_SMART_PTR_ALIASES_ONLY(ErrorHandler); + + /** + * @brief Destructor + */ + virtual ~ErrorHandler() = default; + + /** + * @brief Perform any required post-construction initialization, such as subscribing to topics or reading from the + * parameter server. + * + * This will be called on each plugin after construction, and after the ros node has been initialized. Plugins are + * encouraged to subnamespace any of their parameters to prevent conflicts and allow the same plugin to be used + * multiple times with different settings and topics. + * + * @param[in] name A unique name to give this plugin instance + */ + virtual void initialize(const std::string& name) = 0; + + /** + * @brief Handler to be invoked whenever a runtime error occurs + * + * @param[in] info Information provided about what specifically occurred + */ + virtual void runtimeError(const std::string& info) = 0; + + /** + * @brief Handler to be invoked whenever invalid arguments are given to a function + * + * @param[in] info Information provided about what specifically occurred + */ + virtual void invalidArgument(const std::string& info) = 0; + + /** + * @brief Handler to be invoked whenever an out of range error occurs + * + * @param[in] info Information provided about what specifically occurred + */ + virtual void outOfRangeError(const std::string& info) = 0; + + /** + * @brief Handler to be invoked whenever a logic error occurs + * + * @param[in] info Information provided about what specifically occurred + */ + virtual void logicError(const std::string& info) = 0; + + + /** + * @brief Get the unique name of this error handler + */ + virtual const std::string& name() const = 0; + +protected: + /** + * @brief Default Constructor + */ + ErrorHandler() = default; + std::string name_; +}; + +} // namespace fuse_core + +#endif // FUSE_CORE_ERROR_HANDLER_H diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index 597706e0a..445004bc9 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -94,6 +94,7 @@ add_library(${PROJECT_NAME} src/unicycle_2d.cpp src/unicycle_2d_ignition.cpp src/unicycle_2d_state_kinematic_constraint.cpp + src/basic_error_handler.cpp ) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index 6a6da558a..66d89e24d 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -68,4 +68,9 @@ A fuse_models ignition sensor designed to be used in conjunction with the unicycle 2D motion model. + + + A basic error handler that throws exceptions for any error that occurs. + + diff --git a/fuse_models/include/fuse_models/basic_error_handler.h b/fuse_models/include/fuse_models/basic_error_handler.h new file mode 100644 index 000000000..9f2ef6afd --- /dev/null +++ b/fuse_models/include/fuse_models/basic_error_handler.h @@ -0,0 +1,115 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Locus 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. + */ +#ifndef FUSE_MODELS_BASIC_ERROR_HANDLER_H +#define FUSE_MODELS_BASIC_ERROR_HANDLER_H + +#include +#include + +#include + + +namespace fuse_models +{ + +/** + * @brief The interface definition for error handler plugins in the fuse ecosystem. + * + * An error handler plugin is responsible for handling any errors that may come up in execution of the program. + */ +class BasicErrorHandler: public fuse_core::ErrorHandler +{ +public: + FUSE_SMART_PTR_ALIASES_ONLY(BasicErrorHandler); + + /** + * @brief Default Constructor + */ + BasicErrorHandler(); + + /** + * @brief Destructor + */ + ~BasicErrorHandler() = default; + + /** + * @brief Perform any required post-construction initialization, such as subscribing to topics or reading from the + * parameter server. + * + * This will be called on each plugin after construction, and after the ros node has been initialized. Plugins are + * encouraged to subnamespace any of their parameters to prevent conflicts and allow the same plugin to be used + * multiple times with different settings and topics. + * + * @param[in] name A unique name to give this plugin instance + */ + void initialize(const std::string& name) override; + + /** + * @brief Handler to be invoked whenever a runtime error occurs + * + * @param[in] info Information provided about what specifically occurred + */ + void runtimeError(const std::string& info) override; + + /** + * @brief Handler to be invoked whenever invalid arguments are given to a function + * + * @param[in] info Information provided about what specifically occurred + */ + void invalidArgument(const std::string& info) override; + + /** + * @brief Handler to be invoked whenever an out of range error occurs + * + * @param[in] info Information provided about what specifically occurred + */ + void outOfRangeError(const std::string& info) override; + + /** + * @brief Handler to be invoked whenever a logic error occurs + * + * @param[in] info Information provided about what specifically occurred + */ + void logicError(const std::string& info) override; + + + /** + * @brief Get the unique name of this error handler + */ + const std::string& name() const override; +}; + +} // namespace fuse_core + +#endif // FUSE_MODELS_BASIC_ERROR_HANDLER_H diff --git a/fuse_models/src/basic_error_handler.cpp b/fuse_models/src/basic_error_handler.cpp new file mode 100644 index 000000000..6ffbaad7e --- /dev/null +++ b/fuse_models/src/basic_error_handler.cpp @@ -0,0 +1,61 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Locus 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 + +// Register this motion model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::BasicErrorHandler, fuse_core::ErrorHandler) + +namespace fuse_models +{ + + BasicErrorHandler::BasicErrorHandler() {}; + + void BasicErrorHandler::initialize(const std::string& name) + { + name_ = name; + } + + void BasicErrorHandler::runtimeError(const std::string& info) + { + throw std::runtime_error(info); + } + + void BasicErrorHandler::invalidArgument(const std::string& info) + { + throw std::invalid_argument(info); + } + +} \ No newline at end of file diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index d8250a0b1..7e865cf44 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -113,7 +114,7 @@ class Optimizer */ virtual ~Optimizer(); - static Optimizer* getOptimizer() { return singleton_; } + // static Optimizer* getOptimizer() { return singleton_; } protected: // The unique ptrs returned by pluginlib have a custom deleter. This makes specifying the type rather annoying @@ -161,11 +162,12 @@ class Optimizer Publishers publishers_; //!< The set of publishers to execute after every graph optimization pluginlib::ClassLoader sensor_model_loader_; //!< Pluginlib class loader for SensorModels SensorModels sensor_models_; //!< The set of sensor models, addressable by name + pluginlib::ClassLoader error_handler_loader_; //!< Pluginlib class loader for ErrorHandler diagnostic_updater::Updater diagnostic_updater_; //!< Diagnostic updater ros::Timer diagnostic_updater_timer_; //!< Diagnostic updater timer double diagnostic_updater_timer_period_{ 1.0 }; //!< Diagnostic updater timer period in seconds - static Optimizer* singleton_; // Date: Wed, 27 Jul 2022 09:01:23 -0400 Subject: [PATCH 04/24] Integrating the error handler --- fuse_models/src/basic_error_handler.cpp | 9 +++ .../fuse_optimizers/fixed_lag_smoother.h | 2 +- .../include/fuse_optimizers/optimizer.h | 23 ++++--- fuse_optimizers/src/fixed_lag_smoother.cpp | 68 +++++++++---------- fuse_optimizers/src/optimizer.cpp | 26 +++++-- 5 files changed, 80 insertions(+), 48 deletions(-) diff --git a/fuse_models/src/basic_error_handler.cpp b/fuse_models/src/basic_error_handler.cpp index 6ffbaad7e..5f0115049 100644 --- a/fuse_models/src/basic_error_handler.cpp +++ b/fuse_models/src/basic_error_handler.cpp @@ -58,4 +58,13 @@ namespace fuse_models throw std::invalid_argument(info); } + void BasicErrorHandler::outOfRangeError(const std::string& info) + { + throw std::out_of_range(info); + } + + void BasicErrorHandler::logicError(const std::string& info) + { + throw std::logic_error(info); + } } \ No newline at end of file diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index da64571ff..e7fe7b87d 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -308,7 +308,7 @@ class FixedLagSmoother : public Optimizer * the need to throw exceptions when things go wrong. * @param[in] err_msg A specific error message that gives a reason for the reset */ - void resetOptimizer(const std::string& err_msg) override; + // void resetOptimizer(const std::string& err_msg) override; /** * @brief Update and publish diagnotics diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index 7e865cf44..96b0e2c24 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -114,7 +114,6 @@ class Optimizer */ virtual ~Optimizer(); - // static Optimizer* getOptimizer() { return singleton_; } protected: // The unique ptrs returned by pluginlib have a custom deleter. This makes specifying the type rather annoying @@ -124,6 +123,7 @@ class Optimizer using PublisherUniquePtr = class_loader::ClassLoader::UniquePtr; using Publishers = std::unordered_map; using SensorModelUniquePtr = class_loader::ClassLoader::UniquePtr; + using ErrorHandlerUniquePtr = class_loader::ClassLoader::UniquePtr; /** * @brief A struct to hold the sensor model and whether it is an ignition one or not @@ -163,11 +163,11 @@ class Optimizer pluginlib::ClassLoader sensor_model_loader_; //!< Pluginlib class loader for SensorModels SensorModels sensor_models_; //!< The set of sensor models, addressable by name pluginlib::ClassLoader error_handler_loader_; //!< Pluginlib class loader for ErrorHandler + ErrorHandlerUniquePtr error_handler_; // The error handler for a given optimizer diagnostic_updater::Updater diagnostic_updater_; //!< Diagnostic updater ros::Timer diagnostic_updater_timer_; //!< Diagnostic updater timer double diagnostic_updater_timer_period_{ 1.0 }; //!< Diagnostic updater timer period in seconds - // static Optimizer* singleton_; // lock(optimization_requested_mutex_); - optimization_request_ = false; - } - started_ = false; - ignited_ = false; - setStartTime(ros::Time(0, 0)); - // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the - // pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to - // prevent the possibility of deadlocks. - { - std::lock_guard lock(optimization_mutex_); - // Clear all pending transactions - { - std::lock_guard lock(pending_transactions_mutex_); - pending_transactions_.clear(); - } - // Clear the graph and marginal tracking states - graph_->clear(); - marginal_transaction_ = fuse_core::Transaction(); - timestamp_tracking_.clear(); - lag_expiration_ = ros::Time(0, 0); - } - // Tell all the plugins to start - startPlugins(); - // Test for auto-start - autostart(); -} +// void FixedLagSmoother::resetOptimizer(const std::string& err_msg) +// { +// // ROS_ERROR(err_msg.c_str()); +// // Tell all the plugins to stop +// stopPlugins(); +// // Reset the optimizer state +// { +// std::lock_guard lock(optimization_requested_mutex_); +// optimization_request_ = false; +// } +// started_ = false; +// ignited_ = false; +// setStartTime(ros::Time(0, 0)); +// // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the +// // pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to +// // prevent the possibility of deadlocks. +// { +// std::lock_guard lock(optimization_mutex_); +// // Clear all pending transactions +// { +// std::lock_guard lock(pending_transactions_mutex_); +// pending_transactions_.clear(); +// } +// // Clear the graph and marginal tracking states +// graph_->clear(); +// marginal_transaction_ = fuse_core::Transaction(); +// timestamp_tracking_.clear(); +// lag_expiration_ = ros::Time(0, 0); +// } +// // Tell all the plugins to start +// startPlugins(); +// // Test for auto-start +// autostart(); +// } } // namespace fuse_optimizers diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 0fa02cde7..e832225d0 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -92,11 +92,6 @@ Optimizer::Optimizer( error_handler_loader_("fuse_core", "fuse_core::ErrorHandler"), diagnostic_updater_(node_handle_) { - // if (singleton_ != nullptr) { - // singleton_ = this; - // } else { - // throw std::runtime_error("More than one optimizer at the same time!"); - // } // Setup diagnostics updater private_node_handle_.param("diagnostic_updater_timer_period", diagnostic_updater_timer_period_, diagnostic_updater_timer_period_); @@ -112,6 +107,7 @@ Optimizer::Optimizer( ros::Time::waitForValid(); // Load all configured plugins + loadErrorHandler(); loadMotionModels(); loadSensorModels(); loadPublishers(); @@ -352,6 +348,26 @@ void Optimizer::loadPublishers() diagnostic_updater_.force_update(); } +void Optimizer::loadErrorHandler() +{ + std::string type; + if (!private_node_handle_.hasParam("error_handler")) + { + ROS_WARN("No error handler specified, defaulting to BasicErrorHandler."); + type = "fuse_models::BasicErrorHandler"; + } + else + { + //parse error handler type + private_node_handle_.getParam("error_handler", type); + } + + // If the plugin name is not found, this will throw + auto handler = error_handler_loader_.createUniqueInstance(type); + handler->initialize("handler"); + error_handler_ = std::move(handler); +} + bool Optimizer::applyMotionModels( const std::string& sensor_name, fuse_core::Transaction& transaction) const From 7b30254cc6ad3397852c7a8f93e2313c8395f087 Mon Sep 17 00:00:00 2001 From: David Murdoch Date: Thu, 15 Dec 2022 11:42:48 -0500 Subject: [PATCH 05/24] Changing optimizer structure --- fuse_core/include/fuse_core/error_handler.h | 131 ++++++++---------- fuse_models/CMakeLists.txt | 1 - fuse_models/fuse_plugins.xml | 5 - .../include/fuse_models/basic_error_handler.h | 115 --------------- fuse_models/src/basic_error_handler.cpp | 70 ---------- .../fuse_optimizers/fixed_lag_smoother.h | 7 - .../include/fuse_optimizers/optimizer.h | 13 +- fuse_optimizers/src/optimizer.cpp | 47 +++++-- 8 files changed, 96 insertions(+), 293 deletions(-) delete mode 100644 fuse_models/include/fuse_models/basic_error_handler.h delete mode 100644 fuse_models/src/basic_error_handler.cpp diff --git a/fuse_core/include/fuse_core/error_handler.h b/fuse_core/include/fuse_core/error_handler.h index c2ffe40f3..a12d6b35c 100644 --- a/fuse_core/include/fuse_core/error_handler.h +++ b/fuse_core/include/fuse_core/error_handler.h @@ -31,86 +31,65 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ + #ifndef FUSE_CORE_ERROR_HANDLER_H #define FUSE_CORE_ERROR_HANDLER_H -#include +#include "ros/ros.h" -#include +namespace fuse_core +{ + + class ErrorHandler + { + public: + + static void initializeErrorHandler(const std::string& type) + { + // If the plugin name is not found, this will throw + // auto handler = error_handler_loader_.createInstance(type); + // handler->initialize("handler"); + // error_handler_internal_ = handler; + } + static void invalidArgument(const std::string& info) + { + // error_handler_internal_->invalidArgument(info); + } -namespace fuse_core -{ + static void runtimeError(const std::string& info) + { + // error_handler_internal_->runtimeError(info); + } -/** - * @brief The interface definition for error handler plugins in the fuse ecosystem. - * - * An error handler plugin is responsible for handling any errors that may come up in execution of the program. - */ -class ErrorHandler -{ -public: - FUSE_SMART_PTR_ALIASES_ONLY(ErrorHandler); - - /** - * @brief Destructor - */ - virtual ~ErrorHandler() = default; - - /** - * @brief Perform any required post-construction initialization, such as subscribing to topics or reading from the - * parameter server. - * - * This will be called on each plugin after construction, and after the ros node has been initialized. Plugins are - * encouraged to subnamespace any of their parameters to prevent conflicts and allow the same plugin to be used - * multiple times with different settings and topics. - * - * @param[in] name A unique name to give this plugin instance - */ - virtual void initialize(const std::string& name) = 0; - - /** - * @brief Handler to be invoked whenever a runtime error occurs - * - * @param[in] info Information provided about what specifically occurred - */ - virtual void runtimeError(const std::string& info) = 0; - - /** - * @brief Handler to be invoked whenever invalid arguments are given to a function - * - * @param[in] info Information provided about what specifically occurred - */ - virtual void invalidArgument(const std::string& info) = 0; - - /** - * @brief Handler to be invoked whenever an out of range error occurs - * - * @param[in] info Information provided about what specifically occurred - */ - virtual void outOfRangeError(const std::string& info) = 0; - - /** - * @brief Handler to be invoked whenever a logic error occurs - * - * @param[in] info Information provided about what specifically occurred - */ - virtual void logicError(const std::string& info) = 0; - - - /** - * @brief Get the unique name of this error handler - */ - virtual const std::string& name() const = 0; - -protected: - /** - * @brief Default Constructor - */ - ErrorHandler() = default; - std::string name_; -}; - -} // namespace fuse_core - -#endif // FUSE_CORE_ERROR_HANDLER_H + static void outOfRangeError(const std::string& info) + { + // error_handler_internal_->outOfRangeError(info); + } + + static void logicError(const std::string& info) + { + // error_handler_internal_->logicError(info); + } + + static void registerLogicError(std::function cb) + { + logic_error_cb_ = cb; + } + + + static std::function logic_error_cb_; + protected: + ErrorHandler() = default; + + ros::NodeHandle nh_; + + }; + + + // boost::shared_ptr ErrorHandler::error_handler_internal_ = boost::make_shared(); + // pluginlib::ClassLoader ErrorHandler::error_handler_loader_("fuse_core", "fuse_core::BasicErrorHandler"); +} + + +#endif // FUSE_CORE_ERROR_HANDLER_H \ No newline at end of file diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index 445004bc9..597706e0a 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -94,7 +94,6 @@ add_library(${PROJECT_NAME} src/unicycle_2d.cpp src/unicycle_2d_ignition.cpp src/unicycle_2d_state_kinematic_constraint.cpp - src/basic_error_handler.cpp ) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index 66d89e24d..6a6da558a 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -68,9 +68,4 @@ A fuse_models ignition sensor designed to be used in conjunction with the unicycle 2D motion model. - - - A basic error handler that throws exceptions for any error that occurs. - - diff --git a/fuse_models/include/fuse_models/basic_error_handler.h b/fuse_models/include/fuse_models/basic_error_handler.h deleted file mode 100644 index 9f2ef6afd..000000000 --- a/fuse_models/include/fuse_models/basic_error_handler.h +++ /dev/null @@ -1,115 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus 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. - */ -#ifndef FUSE_MODELS_BASIC_ERROR_HANDLER_H -#define FUSE_MODELS_BASIC_ERROR_HANDLER_H - -#include -#include - -#include - - -namespace fuse_models -{ - -/** - * @brief The interface definition for error handler plugins in the fuse ecosystem. - * - * An error handler plugin is responsible for handling any errors that may come up in execution of the program. - */ -class BasicErrorHandler: public fuse_core::ErrorHandler -{ -public: - FUSE_SMART_PTR_ALIASES_ONLY(BasicErrorHandler); - - /** - * @brief Default Constructor - */ - BasicErrorHandler(); - - /** - * @brief Destructor - */ - ~BasicErrorHandler() = default; - - /** - * @brief Perform any required post-construction initialization, such as subscribing to topics or reading from the - * parameter server. - * - * This will be called on each plugin after construction, and after the ros node has been initialized. Plugins are - * encouraged to subnamespace any of their parameters to prevent conflicts and allow the same plugin to be used - * multiple times with different settings and topics. - * - * @param[in] name A unique name to give this plugin instance - */ - void initialize(const std::string& name) override; - - /** - * @brief Handler to be invoked whenever a runtime error occurs - * - * @param[in] info Information provided about what specifically occurred - */ - void runtimeError(const std::string& info) override; - - /** - * @brief Handler to be invoked whenever invalid arguments are given to a function - * - * @param[in] info Information provided about what specifically occurred - */ - void invalidArgument(const std::string& info) override; - - /** - * @brief Handler to be invoked whenever an out of range error occurs - * - * @param[in] info Information provided about what specifically occurred - */ - void outOfRangeError(const std::string& info) override; - - /** - * @brief Handler to be invoked whenever a logic error occurs - * - * @param[in] info Information provided about what specifically occurred - */ - void logicError(const std::string& info) override; - - - /** - * @brief Get the unique name of this error handler - */ - const std::string& name() const override; -}; - -} // namespace fuse_core - -#endif // FUSE_MODELS_BASIC_ERROR_HANDLER_H diff --git a/fuse_models/src/basic_error_handler.cpp b/fuse_models/src/basic_error_handler.cpp deleted file mode 100644 index 5f0115049..000000000 --- a/fuse_models/src/basic_error_handler.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus 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 - -// Register this motion model with ROS as a plugin. -PLUGINLIB_EXPORT_CLASS(fuse_models::BasicErrorHandler, fuse_core::ErrorHandler) - -namespace fuse_models -{ - - BasicErrorHandler::BasicErrorHandler() {}; - - void BasicErrorHandler::initialize(const std::string& name) - { - name_ = name; - } - - void BasicErrorHandler::runtimeError(const std::string& info) - { - throw std::runtime_error(info); - } - - void BasicErrorHandler::invalidArgument(const std::string& info) - { - throw std::invalid_argument(info); - } - - void BasicErrorHandler::outOfRangeError(const std::string& info) - { - throw std::out_of_range(info); - } - - void BasicErrorHandler::logicError(const std::string& info) - { - throw std::logic_error(info); - } -} \ No newline at end of file diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index e7fe7b87d..eefacac2a 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -303,13 +303,6 @@ class FixedLagSmoother : public Optimizer const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; - /** - * @brief Method exposed for resetting the optimizer in the event of an error. Replaces - * the need to throw exceptions when things go wrong. - * @param[in] err_msg A specific error message that gives a reason for the reset - */ - // void resetOptimizer(const std::string& err_msg) override; - /** * @brief Update and publish diagnotics * @param[in] status The diagnostic status diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index 96b0e2c24..98c3e46b4 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -38,7 +38,6 @@ #include #include #include -#include #include #include #include @@ -123,7 +122,6 @@ class Optimizer using PublisherUniquePtr = class_loader::ClassLoader::UniquePtr; using Publishers = std::unordered_map; using SensorModelUniquePtr = class_loader::ClassLoader::UniquePtr; - using ErrorHandlerUniquePtr = class_loader::ClassLoader::UniquePtr; /** * @brief A struct to hold the sensor model and whether it is an ignition one or not @@ -162,8 +160,7 @@ class Optimizer Publishers publishers_; //!< The set of publishers to execute after every graph optimization pluginlib::ClassLoader sensor_model_loader_; //!< Pluginlib class loader for SensorModels SensorModels sensor_models_; //!< The set of sensor models, addressable by name - pluginlib::ClassLoader error_handler_loader_; //!< Pluginlib class loader for ErrorHandler - ErrorHandlerUniquePtr error_handler_; // The error handler for a given optimizer + diagnostic_updater::Updater diagnostic_updater_; //!< Diagnostic updater ros::Timer diagnostic_updater_timer_; //!< Diagnostic updater timer @@ -274,6 +271,14 @@ class Optimizer * @param[in] status The diagnostic status */ virtual void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status); + + void invalidArgumentCallback(const std::string& info); + + void logicErrorCallback(const std::string& info); + + void runtimeErrorCallback(const std::string& info); + + void outOfRangeErrorCallback(const std::string& info); }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index e832225d0..e82f79f29 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -89,7 +90,6 @@ Optimizer::Optimizer( motion_model_loader_("fuse_core", "fuse_core::MotionModel"), publisher_loader_("fuse_core", "fuse_core::Publisher"), sensor_model_loader_("fuse_core", "fuse_core::SensorModel"), - error_handler_loader_("fuse_core", "fuse_core::ErrorHandler"), diagnostic_updater_(node_handle_) { // Setup diagnostics updater @@ -144,7 +144,7 @@ void Optimizer::loadMotionModels() || (!motion_model.hasMember("name")) || (!motion_model.hasMember("type"))) { - throw std::invalid_argument("The 'motion_models' parameter should be a list of the form: " + fuse_core::ErrorHandler::invalidArgument("The 'motion_models' parameter should be a list of the form: " "-{name: string, type: string}"); } @@ -161,7 +161,7 @@ void Optimizer::loadMotionModels() if ( (motion_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!motion_model_config.hasMember("type"))) { - throw std::invalid_argument("The 'motion_models' parameter should be a struct of the form: " + fuse_core::ErrorHandler::invalidArgument("The 'motion_models' parameter should be a struct of the form: " "{string: {type: string}}"); } @@ -171,7 +171,7 @@ void Optimizer::loadMotionModels() } else { - throw std::invalid_argument("The 'motion_models' parameter should be a list of the form: " + fuse_core::ErrorHandler::invalidArgument("The 'motion_models' parameter should be a list of the form: " "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); } @@ -210,7 +210,7 @@ void Optimizer::loadSensorModels() || (!sensor_model.hasMember("name")) || (!sensor_model.hasMember("type"))) { - throw std::invalid_argument("The 'sensor_models' parameter should be a list of the form: " + fuse_core::ErrorHandler::invalidArgument("The 'sensor_models' parameter should be a list of the form: " "-{name: string, type: string, motion_models: [name1, name2, ...]}"); } @@ -228,7 +228,7 @@ void Optimizer::loadSensorModels() if ( (sensor_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!sensor_model_config.hasMember("type"))) { - throw std::invalid_argument("The 'sensor_models' parameter should be a struct of the form: " + fuse_core::ErrorHandler::invalidArgument("The 'sensor_models' parameter should be a struct of the form: " "{string: {type: string, motion_models: [name1, name2, ...]}}"); } @@ -238,7 +238,7 @@ void Optimizer::loadSensorModels() } else { - throw std::invalid_argument("The 'sensor_models' parameter should be a list of the form: " + fuse_core::ErrorHandler::invalidArgument("The 'sensor_models' parameter should be a list of the form: " "-{name: string, type: string, motion_models: [name1, name2, ...]} " "or a struct of the form: " "{string: {type: string, motion_models: [name1, name2, ...]}}"); @@ -303,7 +303,7 @@ void Optimizer::loadPublishers() || (!publisher.hasMember("name")) || (!publisher.hasMember("type"))) { - throw std::invalid_argument("The 'publishers' parameter should be a list of the form: " + fuse_core::ErrorHandler::invalidArgument("The 'publishers' parameter should be a list of the form: " "-{name: string, type: string}"); } @@ -321,7 +321,7 @@ void Optimizer::loadPublishers() if ( (publisher_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!publisher_config.hasMember("type"))) { - throw std::invalid_argument("The 'publishers' parameter should be a struct of the form: " + fuse_core::ErrorHandler::invalidArgument("The 'publishers' parameter should be a struct of the form: " "{string: {type: string}}"); } @@ -331,7 +331,7 @@ void Optimizer::loadPublishers() } else { - throw std::invalid_argument("The 'publishers' parameter should be a list of the form: " + fuse_core::ErrorHandler::invalidArgument("The 'publishers' parameter should be a list of the form: " "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); } @@ -354,7 +354,7 @@ void Optimizer::loadErrorHandler() if (!private_node_handle_.hasParam("error_handler")) { ROS_WARN("No error handler specified, defaulting to BasicErrorHandler."); - type = "fuse_models::BasicErrorHandler"; + type = "fuse_core::BasicErrorHandler"; } else { @@ -362,10 +362,7 @@ void Optimizer::loadErrorHandler() private_node_handle_.getParam("error_handler", type); } - // If the plugin name is not found, this will throw - auto handler = error_handler_loader_.createUniqueInstance(type); - handler->initialize("handler"); - error_handler_ = std::move(handler); + fuse_core::ErrorHandler::initializeErrorHandler(type); } bool Optimizer::applyMotionModels( @@ -513,4 +510,24 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat status.add("Publishers", std::accumulate(publishers_.begin(), publishers_.end(), std::string(), print_key)); } +void Optimizer::invalidArgumentCallback(const std::string& info) +{ + throw std::invalid_argument(info); +} + +void Optimizer::logicErrorCallback(const std::string& info) +{ + throw std::logic_error(info); +} + +void Optimizer::runtimeErrorCallback(const std::string& info) +{ + throw std::runtime_error(info); +} + +void Optimizer::outOfRangeErrorCallback(const std::string& info) +{ + throw std::out_of_range(info); +} + } // namespace fuse_optimizers From 5ad18679e0255e086297c3c2edb68665d82c67ad Mon Sep 17 00:00:00 2001 From: David Murdoch Date: Tue, 12 Sep 2023 13:42:18 -0400 Subject: [PATCH 06/24] OReworking the whole thing --- fuse_core/include/fuse_core/error_handler.h | 137 ++++++++++++------ .../include/fuse_core/message_buffer_impl.h | 11 +- fuse_core/include/fuse_core/parameter.h | 6 +- .../include/fuse_core/timestamp_manager.h | 1 + fuse_core/src/timestamp_manager.cpp | 3 +- .../include/fuse_optimizers/optimizer.h | 15 -- fuse_optimizers/src/optimizer.cpp | 56 ++----- 7 files changed, 118 insertions(+), 111 deletions(-) diff --git a/fuse_core/include/fuse_core/error_handler.h b/fuse_core/include/fuse_core/error_handler.h index a12d6b35c..31a7cfec4 100644 --- a/fuse_core/include/fuse_core/error_handler.h +++ b/fuse_core/include/fuse_core/error_handler.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2022, Locus Robotics + * Copyright (c) 2023, Locus Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,60 +36,113 @@ #define FUSE_CORE_ERROR_HANDLER_H #include "ros/ros.h" +#include + +/** + * @brief A class that handles errors that can occur in fuse. + * + * By default, fuse optimizers handle any errors that come up by throwing a relevant exception. However, + * while this is generically exceptable, in practical systems it's sometimes not sufficient to just + * throw exceptions and hope that the rest of the system goes on while fuse resets and fixes itself. + * + * This class exists to allow for handling generic classes of errors using whatever callback the user sees fit to use. + * I recommend registering new callbacks that are bound to an Optimizer instance for more specific functionality. + * + * A note: By default, the base Optimizer class registers the basic callbacks that throw error specific exceptions. + * basicCallback below should never be used, and is only provided for simplifying initialisation. + */ + +namespace fuse_core +{ -namespace fuse_core +class ErrorHandler { - - class ErrorHandler + public: + static ErrorHandler& getHandler() { - public: - - static void initializeErrorHandler(const std::string& type) - { - // If the plugin name is not found, this will throw - // auto handler = error_handler_loader_.createInstance(type); - // handler->initialize("handler"); - // error_handler_internal_ = handler; - } + static ErrorHandler handler {}; + return handler; + } - static void invalidArgument(const std::string& info) - { - // error_handler_internal_->invalidArgument(info); - } - static void runtimeError(const std::string& info) - { - // error_handler_internal_->runtimeError(info); - } + void invalidArgument(const std::string& info) + { + invalid_argument_cb_(info); + } - static void outOfRangeError(const std::string& info) - { - // error_handler_internal_->outOfRangeError(info); - } + void runtimeError(const std::string& info) + { + runtime_error_cb_(info); + } - static void logicError(const std::string& info) - { - // error_handler_internal_->logicError(info); - } + void outOfRangeError(const std::string& info) + { + out_of_range_cb_(info); + } - static void registerLogicError(std::function cb) - { - logic_error_cb_ = cb; - } + void logicError(const std::string& info) + { + logic_error_cb_(info); + } + void registerinvalidArgumentErrorCb(std::function cb) + { + invalid_argument_cb_ = cb; + } - static std::function logic_error_cb_; - protected: - ErrorHandler() = default; + void registerRuntimeErrorCb(std::function cb) + { + runtime_error_cb_ = cb; + } - ros::NodeHandle nh_; + void registerOutOfRangeErrorCb(std::function cb) + { + out_of_range_cb_ = cb; + } - }; + void registerLogicErrorCb(std::function cb) + { + logic_error_cb_ = cb; + } + private: + ErrorHandler() + { + invalid_argument_cb_ = invalidArgumentCallback; + runtime_error_cb_ = runtimeErrorCallback; + out_of_range_cb_ = outOfRangeErrorCallback; + logic_error_cb_ = logicErrorCallback; + } + + ~ErrorHandler() {} + std::function invalid_argument_cb_; + std::function runtime_error_cb_; + std::function out_of_range_cb_; + std::function logic_error_cb_; + + + /* + * Default error handling behavior + */ + static void invalidArgumentCallback(std::string info) + { + throw std::invalid_argument(info); + } + static void logicErrorCallback(std::string info) + { + throw std::logic_error(info); + } + static void runtimeErrorCallback(std::string info) + { + throw std::runtime_error(info); + } + static void outOfRangeErrorCallback(std::string info) + { + throw std::out_of_range(info); + } +}; - // boost::shared_ptr ErrorHandler::error_handler_internal_ = boost::make_shared(); - // pluginlib::ClassLoader ErrorHandler::error_handler_loader_("fuse_core", "fuse_core::BasicErrorHandler"); -} +} // namespace fuse_core -#endif // FUSE_CORE_ERROR_HANDLER_H \ No newline at end of file +#endif // FUSE_CORE_ERROR_HANDLER_H diff --git a/fuse_core/include/fuse_core/message_buffer_impl.h b/fuse_core/include/fuse_core/message_buffer_impl.h index d83cd668d..30616ffb7 100644 --- a/fuse_core/include/fuse_core/message_buffer_impl.h +++ b/fuse_core/include/fuse_core/message_buffer_impl.h @@ -34,6 +34,7 @@ #ifndef FUSE_CORE_MESSAGE_BUFFER_IMPL_H #define FUSE_CORE_MESSAGE_BUFFER_IMPL_H +#include #include #include @@ -74,8 +75,9 @@ typename MessageBuffer::message_range MessageBuffer::query( beginning_time_ss << beginning_stamp; std::stringstream ending_time_ss; ending_time_ss << ending_stamp; - throw std::invalid_argument("The beginning_stamp (" + beginning_time_ss.str() + ") must be less than or equal to " - "the ending_stamp (" + ending_time_ss.str() + ")."); + ErrorHandler::getHandler().invalidArgument("The beginning_stamp (" + beginning_time_ss.str() + + ") must be less than or equal to " + "the ending_stamp (" + ending_time_ss.str() + ")."); } // Verify the query is within the bounds of the buffer if (buffer_.empty() || (beginning_stamp < buffer_.front().first) || (ending_stamp > buffer_.back().first)) @@ -91,8 +93,9 @@ typename MessageBuffer::message_range MessageBuffer::query( { available_time_range_ss << "(" << buffer_.front().first << ", " << buffer_.back().first << ")"; } - throw std::out_of_range("The requested time range " + requested_time_range_ss.str() + " is outside the available " - "time range " + available_time_range_ss.str() + "."); + ErrorHandler::getHandler().outOfRangeError("The requested time range " + + requested_time_range_ss.str() + " is outside the available " + "time range " + available_time_range_ss.str() + "."); } // Find the entry that is strictly greater than the requested beginning stamp. If the extended range flag is true, // we will then back up one entry. diff --git a/fuse_core/include/fuse_core/parameter.h b/fuse_core/include/fuse_core/parameter.h index dd8d6f9b9..48aac4db6 100644 --- a/fuse_core/include/fuse_core/parameter.h +++ b/fuse_core/include/fuse_core/parameter.h @@ -35,6 +35,7 @@ #define FUSE_CORE_PARAMETER_H #include +#include #include #include @@ -135,14 +136,15 @@ fuse_core::Matrix getCovarianceDiagonalParam(const ros::Node const auto diagonal_size = diagonal.size(); if (diagonal_size != Size) { - throw std::invalid_argument("Invalid size of " + std::to_string(diagonal_size) + ", expected " + + ErrorHandler::getHandler().invalidArgument("Invalid size of " + std::to_string(diagonal_size) + ", expected " + std::to_string(Size)); } if (std::any_of(diagonal.begin(), diagonal.end(), [](const auto& value) { return value < Scalar(0); })) // NOLINT(whitespace/braces) { - throw std::invalid_argument("Invalid negative diagonal values in " + fuse_core::to_string(Vector(diagonal.data()))); + ErrorHandler::getHandler().invalidArgument("Invalid negative diagonal values in " + + fuse_core::to_string(Vector(diagonal.data()))); } return Vector(diagonal.data()).asDiagonal(); diff --git a/fuse_core/include/fuse_core/timestamp_manager.h b/fuse_core/include/fuse_core/timestamp_manager.h index b759e4ece..ee72cd9b0 100644 --- a/fuse_core/include/fuse_core/timestamp_manager.h +++ b/fuse_core/include/fuse_core/timestamp_manager.h @@ -35,6 +35,7 @@ #define FUSE_CORE_TIMESTAMP_MANAGER_H #include +#include #include #include #include diff --git a/fuse_core/src/timestamp_manager.cpp b/fuse_core/src/timestamp_manager.cpp index d8d00aacf..b92f4750b 100644 --- a/fuse_core/src/timestamp_manager.cpp +++ b/fuse_core/src/timestamp_manager.cpp @@ -74,7 +74,8 @@ void TimestampManager::query( && (stamps.front() < motion_model_history_.begin()->first) && (stamps.front() < (motion_model_history_.rbegin()->first - buffer_length_))) { - throw std::invalid_argument("All timestamps must be within the defined buffer length of the motion model"); + ErrorHandler::getHandler().invalidArgument("All timestamps must be within the " \ + "defined buffer length of the motion model"); } // Create a list of all the required timestamps involved in motion model segments that must be created // Add all of the existing timestamps between the first and last input stamp diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index 98c3e46b4..8b2a10714 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -206,13 +206,6 @@ class Optimizer */ void loadSensorModels(); - /** - * @brief Configure the error handler plugin specified on the parameter server - * - * If the parameter server configuration is invalid or missing, it will default to basicErrorHandler. - */ - void loadErrorHandler(); - /** * @brief Given a transaction and some timestamps, augment the transaction with constraints from all associated * motion models. @@ -271,14 +264,6 @@ class Optimizer * @param[in] status The diagnostic status */ virtual void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status); - - void invalidArgumentCallback(const std::string& info); - - void logicErrorCallback(const std::string& info); - - void runtimeErrorCallback(const std::string& info); - - void outOfRangeErrorCallback(const std::string& info); }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index e82f79f29..2edefbccd 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -107,7 +107,6 @@ Optimizer::Optimizer( ros::Time::waitForValid(); // Load all configured plugins - loadErrorHandler(); loadMotionModels(); loadSensorModels(); loadPublishers(); @@ -144,7 +143,7 @@ void Optimizer::loadMotionModels() || (!motion_model.hasMember("name")) || (!motion_model.hasMember("type"))) { - fuse_core::ErrorHandler::invalidArgument("The 'motion_models' parameter should be a list of the form: " + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'motion_models' parameter should be a list of the form: " "-{name: string, type: string}"); } @@ -161,7 +160,7 @@ void Optimizer::loadMotionModels() if ( (motion_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!motion_model_config.hasMember("type"))) { - fuse_core::ErrorHandler::invalidArgument("The 'motion_models' parameter should be a struct of the form: " + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'motion_models' parameter should be a struct of the form: " "{string: {type: string}}"); } @@ -171,7 +170,7 @@ void Optimizer::loadMotionModels() } else { - fuse_core::ErrorHandler::invalidArgument("The 'motion_models' parameter should be a list of the form: " + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'motion_models' parameter should be a list of the form: " "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); } @@ -210,7 +209,7 @@ void Optimizer::loadSensorModels() || (!sensor_model.hasMember("name")) || (!sensor_model.hasMember("type"))) { - fuse_core::ErrorHandler::invalidArgument("The 'sensor_models' parameter should be a list of the form: " + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'sensor_models' parameter should be a list of the form: " "-{name: string, type: string, motion_models: [name1, name2, ...]}"); } @@ -228,7 +227,7 @@ void Optimizer::loadSensorModels() if ( (sensor_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!sensor_model_config.hasMember("type"))) { - fuse_core::ErrorHandler::invalidArgument("The 'sensor_models' parameter should be a struct of the form: " + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'sensor_models' parameter should be a struct of the form: " "{string: {type: string, motion_models: [name1, name2, ...]}}"); } @@ -238,7 +237,7 @@ void Optimizer::loadSensorModels() } else { - fuse_core::ErrorHandler::invalidArgument("The 'sensor_models' parameter should be a list of the form: " + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'sensor_models' parameter should be a list of the form: " "-{name: string, type: string, motion_models: [name1, name2, ...]} " "or a struct of the form: " "{string: {type: string, motion_models: [name1, name2, ...]}}"); @@ -303,7 +302,7 @@ void Optimizer::loadPublishers() || (!publisher.hasMember("name")) || (!publisher.hasMember("type"))) { - fuse_core::ErrorHandler::invalidArgument("The 'publishers' parameter should be a list of the form: " + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be a list of the form: " "-{name: string, type: string}"); } @@ -321,7 +320,7 @@ void Optimizer::loadPublishers() if ( (publisher_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!publisher_config.hasMember("type"))) { - fuse_core::ErrorHandler::invalidArgument("The 'publishers' parameter should be a struct of the form: " + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be a struct of the form: " "{string: {type: string}}"); } @@ -331,7 +330,7 @@ void Optimizer::loadPublishers() } else { - fuse_core::ErrorHandler::invalidArgument("The 'publishers' parameter should be a list of the form: " + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be a list of the form: " "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); } @@ -348,23 +347,6 @@ void Optimizer::loadPublishers() diagnostic_updater_.force_update(); } -void Optimizer::loadErrorHandler() -{ - std::string type; - if (!private_node_handle_.hasParam("error_handler")) - { - ROS_WARN("No error handler specified, defaulting to BasicErrorHandler."); - type = "fuse_core::BasicErrorHandler"; - } - else - { - //parse error handler type - private_node_handle_.getParam("error_handler", type); - } - - fuse_core::ErrorHandler::initializeErrorHandler(type); -} - bool Optimizer::applyMotionModels( const std::string& sensor_name, fuse_core::Transaction& transaction) const @@ -510,24 +492,4 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat status.add("Publishers", std::accumulate(publishers_.begin(), publishers_.end(), std::string(), print_key)); } -void Optimizer::invalidArgumentCallback(const std::string& info) -{ - throw std::invalid_argument(info); -} - -void Optimizer::logicErrorCallback(const std::string& info) -{ - throw std::logic_error(info); -} - -void Optimizer::runtimeErrorCallback(const std::string& info) -{ - throw std::runtime_error(info); -} - -void Optimizer::outOfRangeErrorCallback(const std::string& info) -{ - throw std::out_of_range(info); -} - } // namespace fuse_optimizers From 68759e500f750e958b0f6c60af62b4e949ab87b7 Mon Sep 17 00:00:00 2001 From: locus-services <33065330+locus-services@users.noreply.github.com> Date: Tue, 23 Aug 2022 03:22:47 -0400 Subject: [PATCH 07/24] Tailor: Updating Jenkinsfile --- Jenkinsfile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 23e9f61f6..41e41edad 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -1,5 +1,5 @@ #!/usr/bin/env groovy -@Library('tailor-meta@0.1.18')_ +@Library('tailor-meta@0.1.20')_ tailorTestPipeline( // Name of job that generated this test definition. rosdistro_job: '/ci/rosdistro/master', @@ -12,7 +12,7 @@ tailorTestPipeline( // OS distributions to test. distributions: ['focal'], // Version of tailor_meta to build against - tailor_meta: '0.1.18', + tailor_meta: '0.1.20', // Master or release branch associated with this track source_branch: 'devel', // Docker registry where test image is stored From 234151e089ee67eba4577444af8e6ca7fb8c63c0 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Perdomo Date: Thu, 17 Nov 2022 13:58:55 +0100 Subject: [PATCH 08/24] Fix Ceres 2.0.0 API support (#273) * Pass kNumResiduals to the internal AutoDiff function. Ceres added this argument in https://github.com/ceres-solver/ceres-solver/commit/e7a30359ee754057f9bd7b349c98c291138d91f4 we need to pass it else template substitution fails. * Pass kLocalSize instead of kGlobalSize Upstream commit made me assume kGlobalSize, but that threw at runtime when the tests ran. This seems to work, also put a using statement there to make roslint happy. Co-authored-by: Ivor Wanders --- .../include/fuse_core/autodiff_local_parameterization.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/fuse_core/include/fuse_core/autodiff_local_parameterization.h b/fuse_core/include/fuse_core/autodiff_local_parameterization.h index 1338f1bd3..4cc9657f6 100644 --- a/fuse_core/include/fuse_core/autodiff_local_parameterization.h +++ b/fuse_core/include/fuse_core/autodiff_local_parameterization.h @@ -190,7 +190,7 @@ bool AutoDiffLocalParameterization ::Differentiate(*plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); #else - return ceres::internal::AutoDifferentiate>( + return ceres::internal::AutoDifferentiate>( *plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); #endif } @@ -217,7 +217,8 @@ bool AutoDiffLocalParameterization ::Differentiate(*minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs); #else - return ceres::internal::AutoDifferentiate>( + using StaticParameters = ceres::internal::StaticParameterDims; + return ceres::internal::AutoDifferentiate( *minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs); #endif } From 9f58fa31a9159ae6ff7fb76e83b8eb94028f6edf Mon Sep 17 00:00:00 2001 From: Gary Servin Date: Wed, 23 Feb 2022 13:55:43 -0300 Subject: [PATCH 09/24] Update changelogs --- fuse/CHANGELOG.rst | 3 +++ fuse_constraints/CHANGELOG.rst | 12 ++++++++++++ fuse_core/CHANGELOG.rst | 22 ++++++++++++++++++++++ fuse_doc/CHANGELOG.rst | 5 +++++ fuse_graphs/CHANGELOG.rst | 16 ++++++++++++++++ fuse_loss/CHANGELOG.rst | 5 +++++ fuse_models/CHANGELOG.rst | 9 +++++++++ fuse_msgs/CHANGELOG.rst | 3 +++ fuse_optimizers/CHANGELOG.rst | 10 ++++++++++ fuse_publishers/CHANGELOG.rst | 8 ++++++++ fuse_tutorials/CHANGELOG.rst | 14 ++++++++++++++ fuse_variables/CHANGELOG.rst | 18 ++++++++++++++++++ fuse_viz/CHANGELOG.rst | 5 +++++ 13 files changed, 130 insertions(+) create mode 100644 fuse_tutorials/CHANGELOG.rst diff --git a/fuse/CHANGELOG.rst b/fuse/CHANGELOG.rst index 1566e6d99..ead27d8af 100644 --- a/fuse/CHANGELOG.rst +++ b/fuse/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package fuse ^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_constraints/CHANGELOG.rst b/fuse_constraints/CHANGELOG.rst index 586979119..bee3627dd 100644 --- a/fuse_constraints/CHANGELOG.rst +++ b/fuse_constraints/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package fuse_constraints ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* Adding doxygen to all packages (#241) +* Contributors: Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_core/CHANGELOG.rst b/fuse_core/CHANGELOG.rst index c4d872b58..e0f6bc5f2 100644 --- a/fuse_core/CHANGELOG.rst +++ b/fuse_core/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package fuse_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* [RST-4455] Fix C++17 compile issue and simplify matrix serialization at the same time (#244) +* Adding doxygen to all packages (#241) +* Add unstamped 3D point variable (#233) (#239) + * Add unstamped 3D landmark variable + * Add landmark test and new uuid generator + Co-authored-by: Stephen Williams + Co-authored-by: Jake McLaughlin +* [RST-3474] Created a getConstraintCosts() method + * Created a getConstraintCosts() method for reporting the costs and residuals of individual constraints +* [RST-2831] Support for optimization bounds (#235) +* Added a time-limited optimization option to the Graph class (#234) +* Contributors: Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_doc/CHANGELOG.rst b/fuse_doc/CHANGELOG.rst index 32118b642..b7b6e6b46 100644 --- a/fuse_doc/CHANGELOG.rst +++ b/fuse_doc/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package fuse_doc ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adding doxygen to all packages (#241) +* Contributors: Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_graphs/CHANGELOG.rst b/fuse_graphs/CHANGELOG.rst index 32fd5bc75..cededdc30 100644 --- a/fuse_graphs/CHANGELOG.rst +++ b/fuse_graphs/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package fuse_graphs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* Adding doxygen to all packages (#241) +* [RST-3474] Created a getConstraintCosts() method + * Created a getConstraintCosts() method for reporting the costs and residuals of individual constraints +* [RST-2831] Support for optimization bounds (#235) +* Added a time-limited optimization option to the Graph class (#234) +* Contributors: Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_loss/CHANGELOG.rst b/fuse_loss/CHANGELOG.rst index e2dbab0dd..aa4fcf4d3 100644 --- a/fuse_loss/CHANGELOG.rst +++ b/fuse_loss/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package fuse_loss ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adding doxygen to all packages (#241) +* Contributors: Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_models/CHANGELOG.rst b/fuse_models/CHANGELOG.rst index e692a3b02..23310f8fd 100644 --- a/fuse_models/CHANGELOG.rst +++ b/fuse_models/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package fuse_models ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Adding doxygen to all packages (#241) +* [RST-3451] Delay some transform warnings so startup is less chatty +* Contributors: Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_msgs/CHANGELOG.rst b/fuse_msgs/CHANGELOG.rst index 17ac69352..08b4b9177 100644 --- a/fuse_msgs/CHANGELOG.rst +++ b/fuse_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package fuse_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_optimizers/CHANGELOG.rst b/fuse_optimizers/CHANGELOG.rst index b36b4c1b7..769633c95 100644 --- a/fuse_optimizers/CHANGELOG.rst +++ b/fuse_optimizers/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package fuse_optimizers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Adding doxygen to all packages (#241) +* [RST-3240] Fix how the variables to be marginalized are selected. +* [RST-3451] Cleaned up condition variable usage +* Contributors: Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_publishers/CHANGELOG.rst b/fuse_publishers/CHANGELOG.rst index 32a34f27c..4c3be5b7b 100644 --- a/fuse_publishers/CHANGELOG.rst +++ b/fuse_publishers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package fuse_publishers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Adding doxygen to all packages (#241) +* Contributors: Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_tutorials/CHANGELOG.rst b/fuse_tutorials/CHANGELOG.rst new file mode 100644 index 000000000..de02f3e8f --- /dev/null +++ b/fuse_tutorials/CHANGELOG.rst @@ -0,0 +1,14 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package fuse_tutorials +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Fix install space for fuse_tutorials (#264) +* Added simple tutorial files from the S3 bucket (#253) +* Sensor tutorial (#251) + * Create a new sensor type with a non-trivial measurement function, a new publisher to visualize the results, and a simplistic robot simulator to demonstrate the sensor in action. +* Contributors: Paul Bovbel, Stephen Williams diff --git a/fuse_variables/CHANGELOG.rst b/fuse_variables/CHANGELOG.rst index d259316c6..e0a2e58d7 100644 --- a/fuse_variables/CHANGELOG.rst +++ b/fuse_variables/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package fuse_variables ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Make 2D versions of the landmark variables (#250) +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* Adding doxygen to all packages (#241) +* Add unstamped 3D point variable (#233) (#239) + * Add unstamped 3D landmark variable + * Add landmark test and new uuid generator + Co-authored-by: Stephen Williams + Co-authored-by: Jake McLaughlin +* Contributors: Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_viz/CHANGELOG.rst b/fuse_viz/CHANGELOG.rst index b137fccc0..c2f3b8379 100644 --- a/fuse_viz/CHANGELOG.rst +++ b/fuse_viz/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package fuse_viz ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adding doxygen to all packages (#241) +* Contributors: Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) From 5acee2025234c3be181134452df014719b1a6b6c Mon Sep 17 00:00:00 2001 From: Gary Servin Date: Wed, 23 Feb 2022 13:55:44 -0300 Subject: [PATCH 10/24] 0.5.0 --- fuse/CHANGELOG.rst | 4 ++-- fuse/package.xml | 2 +- fuse_constraints/CHANGELOG.rst | 4 ++-- fuse_constraints/package.xml | 2 +- fuse_core/CHANGELOG.rst | 4 ++-- fuse_core/package.xml | 2 +- fuse_doc/CHANGELOG.rst | 4 ++-- fuse_doc/package.xml | 2 +- fuse_graphs/CHANGELOG.rst | 4 ++-- fuse_graphs/package.xml | 2 +- fuse_loss/CHANGELOG.rst | 4 ++-- fuse_loss/package.xml | 2 +- fuse_models/CHANGELOG.rst | 4 ++-- fuse_models/package.xml | 2 +- fuse_msgs/CHANGELOG.rst | 4 ++-- fuse_msgs/package.xml | 2 +- fuse_optimizers/CHANGELOG.rst | 4 ++-- fuse_optimizers/package.xml | 2 +- fuse_publishers/CHANGELOG.rst | 4 ++-- fuse_publishers/package.xml | 2 +- fuse_tutorials/CHANGELOG.rst | 4 ++-- fuse_tutorials/package.xml | 2 +- fuse_variables/CHANGELOG.rst | 4 ++-- fuse_variables/package.xml | 2 +- fuse_viz/CHANGELOG.rst | 4 ++-- fuse_viz/package.xml | 2 +- 26 files changed, 39 insertions(+), 39 deletions(-) diff --git a/fuse/CHANGELOG.rst b/fuse/CHANGELOG.rst index ead27d8af..76e655514 100644 --- a/fuse/CHANGELOG.rst +++ b/fuse/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse ^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ 0.4.2 (2021-07-20) ------------------ diff --git a/fuse/package.xml b/fuse/package.xml index fe44deaca..4e9abfffb 100644 --- a/fuse/package.xml +++ b/fuse/package.xml @@ -1,7 +1,7 @@ fuse - 0.4.2 + 0.5.0 The fuse metapackage diff --git a/fuse_constraints/CHANGELOG.rst b/fuse_constraints/CHANGELOG.rst index bee3627dd..29989cefe 100644 --- a/fuse_constraints/CHANGELOG.rst +++ b/fuse_constraints/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_constraints ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ * [RST-4186] Fix fuse macro names (#263) * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. * Update all fuse objects to use the new macro names diff --git a/fuse_constraints/package.xml b/fuse_constraints/package.xml index df9bab00e..aa73be7ce 100644 --- a/fuse_constraints/package.xml +++ b/fuse_constraints/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_constraints - 0.4.2 + 0.5.0 The fuse_constraints package provides a set of commonly used constraint types, such as direct measurements on state variables (absolute constraints) or measurements of the state changes (relative constraints). diff --git a/fuse_core/CHANGELOG.rst b/fuse_core/CHANGELOG.rst index e0f6bc5f2..f466a341e 100644 --- a/fuse_core/CHANGELOG.rst +++ b/fuse_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ * [RST-4186] Fix fuse macro names (#263) * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. * Update all fuse objects to use the new macro names diff --git a/fuse_core/package.xml b/fuse_core/package.xml index a42e0a591..96db4573f 100644 --- a/fuse_core/package.xml +++ b/fuse_core/package.xml @@ -1,7 +1,7 @@ fuse_core - 0.4.2 + 0.5.0 The fuse_core package provides the base class interfaces for the various fuse components. Concrete implementations of these interfaces are provided in other packages. diff --git a/fuse_doc/CHANGELOG.rst b/fuse_doc/CHANGELOG.rst index b7b6e6b46..6f32f8704 100644 --- a/fuse_doc/CHANGELOG.rst +++ b/fuse_doc/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_doc ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ * Adding doxygen to all packages (#241) * Contributors: Tom Moore diff --git a/fuse_doc/package.xml b/fuse_doc/package.xml index 9d1822cbc..131418eed 100644 --- a/fuse_doc/package.xml +++ b/fuse_doc/package.xml @@ -1,7 +1,7 @@ fuse_doc - 0.4.2 + 0.5.0 The fuse_doc package provides documentation and examples for the fuse package. diff --git a/fuse_graphs/CHANGELOG.rst b/fuse_graphs/CHANGELOG.rst index cededdc30..48ccf43f2 100644 --- a/fuse_graphs/CHANGELOG.rst +++ b/fuse_graphs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_graphs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ * [RST-4186] Fix fuse macro names (#263) * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. * Update all fuse objects to use the new macro names diff --git a/fuse_graphs/package.xml b/fuse_graphs/package.xml index 77f391f1a..d82d1715f 100644 --- a/fuse_graphs/package.xml +++ b/fuse_graphs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_graphs - 0.4.2 + 0.5.0 The fuse_graphs package provides some concrete implementations of the fuse_core::Graph interface. diff --git a/fuse_loss/CHANGELOG.rst b/fuse_loss/CHANGELOG.rst index aa4fcf4d3..93e1bab58 100644 --- a/fuse_loss/CHANGELOG.rst +++ b/fuse_loss/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_loss ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ * Adding doxygen to all packages (#241) * Contributors: Tom Moore diff --git a/fuse_loss/package.xml b/fuse_loss/package.xml index fb89aad3f..8d24594b0 100644 --- a/fuse_loss/package.xml +++ b/fuse_loss/package.xml @@ -1,7 +1,7 @@ fuse_loss - 0.4.2 + 0.5.0 The fuse_loss package provides a set of commonly used loss functions, such as the basic ones provided by Ceres. diff --git a/fuse_models/CHANGELOG.rst b/fuse_models/CHANGELOG.rst index 23310f8fd..230f26251 100644 --- a/fuse_models/CHANGELOG.rst +++ b/fuse_models/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_models ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ * [RST-4186] Fix fuse macro names (#263) * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. * Update all fuse objects to use the new macro names diff --git a/fuse_models/package.xml b/fuse_models/package.xml index 9b29265f2..b723e6762 100644 --- a/fuse_models/package.xml +++ b/fuse_models/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_models - 0.4.2 + 0.5.0 fuse plugins that implement various kinematic and sensor models Tom Moore diff --git a/fuse_msgs/CHANGELOG.rst b/fuse_msgs/CHANGELOG.rst index 08b4b9177..647dc16bb 100644 --- a/fuse_msgs/CHANGELOG.rst +++ b/fuse_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ 0.4.2 (2021-07-20) ------------------ diff --git a/fuse_msgs/package.xml b/fuse_msgs/package.xml index 4ed960fc6..20671f8bc 100644 --- a/fuse_msgs/package.xml +++ b/fuse_msgs/package.xml @@ -1,7 +1,7 @@ fuse_msgs - 0.4.2 + 0.5.0 The fuse_msgs package contains messages capable of holding serialized fuse objects diff --git a/fuse_optimizers/CHANGELOG.rst b/fuse_optimizers/CHANGELOG.rst index 769633c95..237660d1c 100644 --- a/fuse_optimizers/CHANGELOG.rst +++ b/fuse_optimizers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_optimizers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ * [RST-4186] Fix fuse macro names (#263) * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. * Update all fuse objects to use the new macro names diff --git a/fuse_optimizers/package.xml b/fuse_optimizers/package.xml index 19faf4220..41d1ca4b2 100644 --- a/fuse_optimizers/package.xml +++ b/fuse_optimizers/package.xml @@ -1,7 +1,7 @@ fuse_optimizers - 0.4.2 + 0.5.0 The fuse_optimizers package provides a set of optimizer implementations. An optimizer is the object responsible for coordinating the sensors and motion model inputs, computing the optimal state values, and providing access to diff --git a/fuse_publishers/CHANGELOG.rst b/fuse_publishers/CHANGELOG.rst index 4c3be5b7b..1be4f2d14 100644 --- a/fuse_publishers/CHANGELOG.rst +++ b/fuse_publishers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_publishers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ * [RST-4186] Fix fuse macro names (#263) * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. * Update all fuse objects to use the new macro names diff --git a/fuse_publishers/package.xml b/fuse_publishers/package.xml index ed9d998d2..7ec975faf 100644 --- a/fuse_publishers/package.xml +++ b/fuse_publishers/package.xml @@ -1,7 +1,7 @@ fuse_publishers - 0.4.2 + 0.5.0 The fuse_publishers package provides a set of common publisher plugins. diff --git a/fuse_tutorials/CHANGELOG.rst b/fuse_tutorials/CHANGELOG.rst index de02f3e8f..bfd60d22f 100644 --- a/fuse_tutorials/CHANGELOG.rst +++ b/fuse_tutorials/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_tutorials ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ * [RST-4186] Fix fuse macro names (#263) * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. * Update all fuse objects to use the new macro names diff --git a/fuse_tutorials/package.xml b/fuse_tutorials/package.xml index bf430348d..a4cd52b84 100644 --- a/fuse_tutorials/package.xml +++ b/fuse_tutorials/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_tutorials - 0.4.2 + 0.5.0 Package containing source code for the fuse tutorials diff --git a/fuse_variables/CHANGELOG.rst b/fuse_variables/CHANGELOG.rst index e0a2e58d7..76a882663 100644 --- a/fuse_variables/CHANGELOG.rst +++ b/fuse_variables/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_variables ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ * [RST-4186] Fix fuse macro names (#263) * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. * Update all fuse objects to use the new macro names diff --git a/fuse_variables/package.xml b/fuse_variables/package.xml index ae0a12f93..7034e1cd0 100644 --- a/fuse_variables/package.xml +++ b/fuse_variables/package.xml @@ -1,7 +1,7 @@ fuse_variables - 0.4.2 + 0.5.0 The fuse_variables package provides a set of commonly used variable types, such as 2D and 3D positions, orientations, velocities, and accelerations. diff --git a/fuse_viz/CHANGELOG.rst b/fuse_viz/CHANGELOG.rst index c2f3b8379..1795a32eb 100644 --- a/fuse_viz/CHANGELOG.rst +++ b/fuse_viz/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fuse_viz ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2022-02-23) +------------------ * Adding doxygen to all packages (#241) * Contributors: Tom Moore diff --git a/fuse_viz/package.xml b/fuse_viz/package.xml index e66ab9252..9ead3f987 100644 --- a/fuse_viz/package.xml +++ b/fuse_viz/package.xml @@ -1,7 +1,7 @@ fuse_viz - 0.4.2 + 0.5.0 The fuse_viz package provides visualization tools for fuse. From 29c19c5bfeed49279ebf71ff9174d2d172950643 Mon Sep 17 00:00:00 2001 From: Gary Servin Date: Sat, 7 Jan 2023 01:14:50 -0300 Subject: [PATCH 11/24] Fix changelog --- fuse_tutorials/CHANGELOG.rst | 3 +++ 1 file changed, 3 insertions(+) diff --git a/fuse_tutorials/CHANGELOG.rst b/fuse_tutorials/CHANGELOG.rst index bfd60d22f..5521dd886 100644 --- a/fuse_tutorials/CHANGELOG.rst +++ b/fuse_tutorials/CHANGELOG.rst @@ -12,3 +12,6 @@ Changelog for package fuse_tutorials * Sensor tutorial (#251) * Create a new sensor type with a non-trivial measurement function, a new publisher to visualize the results, and a simplistic robot simulator to demonstrate the sensor in action. * Contributors: Paul Bovbel, Stephen Williams + +0.0.1 (2018-07-05) +------------------ From 4c2846e4198f400927102e9c98045b8a839dc12a Mon Sep 17 00:00:00 2001 From: Gary Servin Date: Wed, 22 Feb 2023 11:32:27 -0300 Subject: [PATCH 12/24] Update changelogs --- fuse/CHANGELOG.rst | 6 ++++++ fuse_constraints/CHANGELOG.rst | 14 ++++++++++++++ fuse_core/CHANGELOG.rst | 31 +++++++++++++++++++++++++++++++ fuse_doc/CHANGELOG.rst | 7 +++++++ fuse_graphs/CHANGELOG.rst | 18 ++++++++++++++++++ fuse_loss/CHANGELOG.rst | 7 +++++++ fuse_models/CHANGELOG.rst | 11 +++++++++++ fuse_msgs/CHANGELOG.rst | 6 ++++++ fuse_optimizers/CHANGELOG.rst | 12 ++++++++++++ fuse_publishers/CHANGELOG.rst | 10 ++++++++++ fuse_tutorials/CHANGELOG.rst | 14 ++++++++++++++ fuse_variables/CHANGELOG.rst | 20 ++++++++++++++++++++ fuse_viz/CHANGELOG.rst | 7 +++++++ 13 files changed, 163 insertions(+) diff --git a/fuse/CHANGELOG.rst b/fuse/CHANGELOG.rst index 76e655514..a4d06a4b6 100644 --- a/fuse/CHANGELOG.rst +++ b/fuse/CHANGELOG.rst @@ -5,6 +5,12 @@ Changelog for package fuse 0.5.0 (2022-02-23) ------------------ +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* Contributors: Gary Servin + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_constraints/CHANGELOG.rst b/fuse_constraints/CHANGELOG.rst index 29989cefe..49ee0c074 100644 --- a/fuse_constraints/CHANGELOG.rst +++ b/fuse_constraints/CHANGELOG.rst @@ -14,6 +14,20 @@ Changelog for package fuse_constraints * Adding doxygen to all packages (#241) * Contributors: Stephen Williams, Tom Moore +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* Adding doxygen to all packages (#241) +* Contributors: Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_core/CHANGELOG.rst b/fuse_core/CHANGELOG.rst index f466a341e..d97ccf703 100644 --- a/fuse_core/CHANGELOG.rst +++ b/fuse_core/CHANGELOG.rst @@ -24,6 +24,37 @@ Changelog for package fuse_core * Added a time-limited optimization option to the Graph class (#234) * Contributors: Stephen Williams, Tom Moore +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* Fix Ceres 2.0.0 API support (#273) + * Pass kNumResiduals to the internal AutoDiff function. + Ceres added this argument in https://github.com/ceres-solver/ceres-solver/commit/e7a30359ee754057f9bd7b349c98c291138d91f4 we need to pass it else template substitution fails. + * Pass kLocalSize instead of kGlobalSize + Upstream commit made me assume kGlobalSize, but that threw at runtime when the tests ran. + This seems to work, also put a using statement there to make roslint happy. + Co-authored-by: Ivor Wanders +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* [RST-4455] Fix C++17 compile issue and simplify matrix serialization at the same time (#244) +* Adding doxygen to all packages (#241) +* Add unstamped 3D point variable (#233) (#239) + * Add unstamped 3D landmark variable + * Add landmark test and new uuid generator + Co-authored-by: Stephen Williams + Co-authored-by: Jake McLaughlin +* [RST-3474] Created a getConstraintCosts() method + * Created a getConstraintCosts() method for reporting the costs and residuals of individual constraints +* [RST-2831] Support for optimization bounds (#235) +* Added a time-limited optimization option to the Graph class (#234) +* Contributors: Enrique Fernandez Perdomo, Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_doc/CHANGELOG.rst b/fuse_doc/CHANGELOG.rst index 6f32f8704..50a2eced1 100644 --- a/fuse_doc/CHANGELOG.rst +++ b/fuse_doc/CHANGELOG.rst @@ -7,6 +7,13 @@ Changelog for package fuse_doc * Adding doxygen to all packages (#241) * Contributors: Tom Moore +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* Adding doxygen to all packages (#241) +* Contributors: Gary Servin, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_graphs/CHANGELOG.rst b/fuse_graphs/CHANGELOG.rst index 48ccf43f2..2b7e5d468 100644 --- a/fuse_graphs/CHANGELOG.rst +++ b/fuse_graphs/CHANGELOG.rst @@ -18,6 +18,24 @@ Changelog for package fuse_graphs * Added a time-limited optimization option to the Graph class (#234) * Contributors: Stephen Williams, Tom Moore +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* Adding doxygen to all packages (#241) +* [RST-3474] Created a getConstraintCosts() method + * Created a getConstraintCosts() method for reporting the costs and residuals of individual constraints +* [RST-2831] Support for optimization bounds (#235) +* Added a time-limited optimization option to the Graph class (#234) +* Contributors: Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_loss/CHANGELOG.rst b/fuse_loss/CHANGELOG.rst index 93e1bab58..74cf4302b 100644 --- a/fuse_loss/CHANGELOG.rst +++ b/fuse_loss/CHANGELOG.rst @@ -7,6 +7,13 @@ Changelog for package fuse_loss * Adding doxygen to all packages (#241) * Contributors: Tom Moore +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* Adding doxygen to all packages (#241) +* Contributors: Gary Servin, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_models/CHANGELOG.rst b/fuse_models/CHANGELOG.rst index 230f26251..9f2911893 100644 --- a/fuse_models/CHANGELOG.rst +++ b/fuse_models/CHANGELOG.rst @@ -11,6 +11,17 @@ Changelog for package fuse_models * [RST-3451] Delay some transform warnings so startup is less chatty * Contributors: Stephen Williams, Tom Moore +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Adding doxygen to all packages (#241) +* [RST-3451] Delay some transform warnings so startup is less chatty +* Contributors: Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_msgs/CHANGELOG.rst b/fuse_msgs/CHANGELOG.rst index 647dc16bb..961a0922a 100644 --- a/fuse_msgs/CHANGELOG.rst +++ b/fuse_msgs/CHANGELOG.rst @@ -5,6 +5,12 @@ Changelog for package fuse_msgs 0.5.0 (2022-02-23) ------------------ +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* Contributors: Gary Servin + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_optimizers/CHANGELOG.rst b/fuse_optimizers/CHANGELOG.rst index 237660d1c..338c77622 100644 --- a/fuse_optimizers/CHANGELOG.rst +++ b/fuse_optimizers/CHANGELOG.rst @@ -12,6 +12,18 @@ Changelog for package fuse_optimizers * [RST-3451] Cleaned up condition variable usage * Contributors: Stephen Williams, Tom Moore +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Adding doxygen to all packages (#241) +* [RST-3240] Fix how the variables to be marginalized are selected. +* [RST-3451] Cleaned up condition variable usage +* Contributors: Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_publishers/CHANGELOG.rst b/fuse_publishers/CHANGELOG.rst index 1be4f2d14..8ee30e81a 100644 --- a/fuse_publishers/CHANGELOG.rst +++ b/fuse_publishers/CHANGELOG.rst @@ -10,6 +10,16 @@ Changelog for package fuse_publishers * Adding doxygen to all packages (#241) * Contributors: Stephen Williams, Tom Moore +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Adding doxygen to all packages (#241) +* Contributors: Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_tutorials/CHANGELOG.rst b/fuse_tutorials/CHANGELOG.rst index 5521dd886..0edc26f92 100644 --- a/fuse_tutorials/CHANGELOG.rst +++ b/fuse_tutorials/CHANGELOG.rst @@ -13,5 +13,19 @@ Changelog for package fuse_tutorials * Create a new sensor type with a non-trivial measurement function, a new publisher to visualize the results, and a simplistic robot simulator to demonstrate the sensor in action. * Contributors: Paul Bovbel, Stephen Williams +Forthcoming +----------- +* Fix changelog +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Fix install space for fuse_tutorials (#264) +* Added simple tutorial files from the S3 bucket (#253) +* Sensor tutorial (#251) + * Create a new sensor type with a non-trivial measurement function, a new publisher to visualize the results, and a simplistic robot simulator to demonstrate the sensor in action. +* Contributors: Gary Servin, Paul Bovbel, Stephen Williams + 0.0.1 (2018-07-05) ------------------ diff --git a/fuse_variables/CHANGELOG.rst b/fuse_variables/CHANGELOG.rst index 76a882663..bdaba23f9 100644 --- a/fuse_variables/CHANGELOG.rst +++ b/fuse_variables/CHANGELOG.rst @@ -20,6 +20,26 @@ Changelog for package fuse_variables Co-authored-by: Jake McLaughlin * Contributors: Stephen Williams, Tom Moore +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Make 2D versions of the landmark variables (#250) +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* Adding doxygen to all packages (#241) +* Add unstamped 3D point variable (#233) (#239) + * Add unstamped 3D landmark variable + * Add landmark test and new uuid generator + Co-authored-by: Stephen Williams + Co-authored-by: Jake McLaughlin +* Contributors: Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_viz/CHANGELOG.rst b/fuse_viz/CHANGELOG.rst index 1795a32eb..e6e244811 100644 --- a/fuse_viz/CHANGELOG.rst +++ b/fuse_viz/CHANGELOG.rst @@ -7,6 +7,13 @@ Changelog for package fuse_viz * Adding doxygen to all packages (#241) * Contributors: Tom Moore +Forthcoming +----------- +* 0.5.0 +* Update changelogs +* Adding doxygen to all packages (#241) +* Contributors: Gary Servin, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) From 99643f58ff793a192e6109e005ea763fec446240 Mon Sep 17 00:00:00 2001 From: Gary Servin Date: Wed, 22 Feb 2023 11:32:27 -0300 Subject: [PATCH 13/24] 0.6.0 --- fuse/CHANGELOG.rst | 4 ++-- fuse/package.xml | 2 +- fuse_constraints/CHANGELOG.rst | 4 ++-- fuse_constraints/package.xml | 2 +- fuse_core/CHANGELOG.rst | 4 ++-- fuse_core/package.xml | 2 +- fuse_doc/CHANGELOG.rst | 4 ++-- fuse_doc/package.xml | 2 +- fuse_graphs/CHANGELOG.rst | 4 ++-- fuse_graphs/package.xml | 2 +- fuse_loss/CHANGELOG.rst | 4 ++-- fuse_loss/package.xml | 2 +- fuse_models/CHANGELOG.rst | 4 ++-- fuse_models/package.xml | 2 +- fuse_msgs/CHANGELOG.rst | 4 ++-- fuse_msgs/package.xml | 2 +- fuse_optimizers/CHANGELOG.rst | 4 ++-- fuse_optimizers/package.xml | 2 +- fuse_publishers/CHANGELOG.rst | 4 ++-- fuse_publishers/package.xml | 2 +- fuse_tutorials/CHANGELOG.rst | 4 ++-- fuse_tutorials/package.xml | 2 +- fuse_variables/CHANGELOG.rst | 4 ++-- fuse_variables/package.xml | 2 +- fuse_viz/CHANGELOG.rst | 4 ++-- fuse_viz/package.xml | 2 +- 26 files changed, 39 insertions(+), 39 deletions(-) diff --git a/fuse/CHANGELOG.rst b/fuse/CHANGELOG.rst index a4d06a4b6..0ad10ea44 100644 --- a/fuse/CHANGELOG.rst +++ b/fuse/CHANGELOG.rst @@ -5,8 +5,8 @@ Changelog for package fuse 0.5.0 (2022-02-23) ------------------ -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * Contributors: Gary Servin diff --git a/fuse/package.xml b/fuse/package.xml index 4e9abfffb..3db99de91 100644 --- a/fuse/package.xml +++ b/fuse/package.xml @@ -1,7 +1,7 @@ fuse - 0.5.0 + 0.6.0 The fuse metapackage diff --git a/fuse_constraints/CHANGELOG.rst b/fuse_constraints/CHANGELOG.rst index 49ee0c074..0e7a591bf 100644 --- a/fuse_constraints/CHANGELOG.rst +++ b/fuse_constraints/CHANGELOG.rst @@ -14,8 +14,8 @@ Changelog for package fuse_constraints * Adding doxygen to all packages (#241) * Contributors: Stephen Williams, Tom Moore -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * [RST-4186] Fix fuse macro names (#263) diff --git a/fuse_constraints/package.xml b/fuse_constraints/package.xml index aa73be7ce..ee7cf7764 100644 --- a/fuse_constraints/package.xml +++ b/fuse_constraints/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_constraints - 0.5.0 + 0.6.0 The fuse_constraints package provides a set of commonly used constraint types, such as direct measurements on state variables (absolute constraints) or measurements of the state changes (relative constraints). diff --git a/fuse_core/CHANGELOG.rst b/fuse_core/CHANGELOG.rst index d97ccf703..b0e152fa3 100644 --- a/fuse_core/CHANGELOG.rst +++ b/fuse_core/CHANGELOG.rst @@ -24,8 +24,8 @@ Changelog for package fuse_core * Added a time-limited optimization option to the Graph class (#234) * Contributors: Stephen Williams, Tom Moore -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * Fix Ceres 2.0.0 API support (#273) diff --git a/fuse_core/package.xml b/fuse_core/package.xml index 96db4573f..239733db4 100644 --- a/fuse_core/package.xml +++ b/fuse_core/package.xml @@ -1,7 +1,7 @@ fuse_core - 0.5.0 + 0.6.0 The fuse_core package provides the base class interfaces for the various fuse components. Concrete implementations of these interfaces are provided in other packages. diff --git a/fuse_doc/CHANGELOG.rst b/fuse_doc/CHANGELOG.rst index 50a2eced1..dcc5a286d 100644 --- a/fuse_doc/CHANGELOG.rst +++ b/fuse_doc/CHANGELOG.rst @@ -7,8 +7,8 @@ Changelog for package fuse_doc * Adding doxygen to all packages (#241) * Contributors: Tom Moore -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * Adding doxygen to all packages (#241) diff --git a/fuse_doc/package.xml b/fuse_doc/package.xml index 131418eed..49eedf614 100644 --- a/fuse_doc/package.xml +++ b/fuse_doc/package.xml @@ -1,7 +1,7 @@ fuse_doc - 0.5.0 + 0.6.0 The fuse_doc package provides documentation and examples for the fuse package. diff --git a/fuse_graphs/CHANGELOG.rst b/fuse_graphs/CHANGELOG.rst index 2b7e5d468..8a1f93197 100644 --- a/fuse_graphs/CHANGELOG.rst +++ b/fuse_graphs/CHANGELOG.rst @@ -18,8 +18,8 @@ Changelog for package fuse_graphs * Added a time-limited optimization option to the Graph class (#234) * Contributors: Stephen Williams, Tom Moore -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * [RST-4186] Fix fuse macro names (#263) diff --git a/fuse_graphs/package.xml b/fuse_graphs/package.xml index d82d1715f..cc8d3231c 100644 --- a/fuse_graphs/package.xml +++ b/fuse_graphs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_graphs - 0.5.0 + 0.6.0 The fuse_graphs package provides some concrete implementations of the fuse_core::Graph interface. diff --git a/fuse_loss/CHANGELOG.rst b/fuse_loss/CHANGELOG.rst index 74cf4302b..531d473e7 100644 --- a/fuse_loss/CHANGELOG.rst +++ b/fuse_loss/CHANGELOG.rst @@ -7,8 +7,8 @@ Changelog for package fuse_loss * Adding doxygen to all packages (#241) * Contributors: Tom Moore -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * Adding doxygen to all packages (#241) diff --git a/fuse_loss/package.xml b/fuse_loss/package.xml index 8d24594b0..e83d34e88 100644 --- a/fuse_loss/package.xml +++ b/fuse_loss/package.xml @@ -1,7 +1,7 @@ fuse_loss - 0.5.0 + 0.6.0 The fuse_loss package provides a set of commonly used loss functions, such as the basic ones provided by Ceres. diff --git a/fuse_models/CHANGELOG.rst b/fuse_models/CHANGELOG.rst index 9f2911893..fbe149c8b 100644 --- a/fuse_models/CHANGELOG.rst +++ b/fuse_models/CHANGELOG.rst @@ -11,8 +11,8 @@ Changelog for package fuse_models * [RST-3451] Delay some transform warnings so startup is less chatty * Contributors: Stephen Williams, Tom Moore -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * [RST-4186] Fix fuse macro names (#263) diff --git a/fuse_models/package.xml b/fuse_models/package.xml index b723e6762..98a3b842e 100644 --- a/fuse_models/package.xml +++ b/fuse_models/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_models - 0.5.0 + 0.6.0 fuse plugins that implement various kinematic and sensor models Tom Moore diff --git a/fuse_msgs/CHANGELOG.rst b/fuse_msgs/CHANGELOG.rst index 961a0922a..cb1ade376 100644 --- a/fuse_msgs/CHANGELOG.rst +++ b/fuse_msgs/CHANGELOG.rst @@ -5,8 +5,8 @@ Changelog for package fuse_msgs 0.5.0 (2022-02-23) ------------------ -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * Contributors: Gary Servin diff --git a/fuse_msgs/package.xml b/fuse_msgs/package.xml index 20671f8bc..a7ef19dba 100644 --- a/fuse_msgs/package.xml +++ b/fuse_msgs/package.xml @@ -1,7 +1,7 @@ fuse_msgs - 0.5.0 + 0.6.0 The fuse_msgs package contains messages capable of holding serialized fuse objects diff --git a/fuse_optimizers/CHANGELOG.rst b/fuse_optimizers/CHANGELOG.rst index 338c77622..54562a3c7 100644 --- a/fuse_optimizers/CHANGELOG.rst +++ b/fuse_optimizers/CHANGELOG.rst @@ -12,8 +12,8 @@ Changelog for package fuse_optimizers * [RST-3451] Cleaned up condition variable usage * Contributors: Stephen Williams, Tom Moore -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * [RST-4186] Fix fuse macro names (#263) diff --git a/fuse_optimizers/package.xml b/fuse_optimizers/package.xml index 41d1ca4b2..e2cadf6ab 100644 --- a/fuse_optimizers/package.xml +++ b/fuse_optimizers/package.xml @@ -1,7 +1,7 @@ fuse_optimizers - 0.5.0 + 0.6.0 The fuse_optimizers package provides a set of optimizer implementations. An optimizer is the object responsible for coordinating the sensors and motion model inputs, computing the optimal state values, and providing access to diff --git a/fuse_publishers/CHANGELOG.rst b/fuse_publishers/CHANGELOG.rst index 8ee30e81a..2daef5d34 100644 --- a/fuse_publishers/CHANGELOG.rst +++ b/fuse_publishers/CHANGELOG.rst @@ -10,8 +10,8 @@ Changelog for package fuse_publishers * Adding doxygen to all packages (#241) * Contributors: Stephen Williams, Tom Moore -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * [RST-4186] Fix fuse macro names (#263) diff --git a/fuse_publishers/package.xml b/fuse_publishers/package.xml index 7ec975faf..caccb0fbc 100644 --- a/fuse_publishers/package.xml +++ b/fuse_publishers/package.xml @@ -1,7 +1,7 @@ fuse_publishers - 0.5.0 + 0.6.0 The fuse_publishers package provides a set of common publisher plugins. diff --git a/fuse_tutorials/CHANGELOG.rst b/fuse_tutorials/CHANGELOG.rst index 0edc26f92..ed2a0787b 100644 --- a/fuse_tutorials/CHANGELOG.rst +++ b/fuse_tutorials/CHANGELOG.rst @@ -13,8 +13,8 @@ Changelog for package fuse_tutorials * Create a new sensor type with a non-trivial measurement function, a new publisher to visualize the results, and a simplistic robot simulator to demonstrate the sensor in action. * Contributors: Paul Bovbel, Stephen Williams -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * Fix changelog * 0.5.0 * Update changelogs diff --git a/fuse_tutorials/package.xml b/fuse_tutorials/package.xml index a4cd52b84..ce2cbed30 100644 --- a/fuse_tutorials/package.xml +++ b/fuse_tutorials/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_tutorials - 0.5.0 + 0.6.0 Package containing source code for the fuse tutorials diff --git a/fuse_variables/CHANGELOG.rst b/fuse_variables/CHANGELOG.rst index bdaba23f9..a78544f43 100644 --- a/fuse_variables/CHANGELOG.rst +++ b/fuse_variables/CHANGELOG.rst @@ -20,8 +20,8 @@ Changelog for package fuse_variables Co-authored-by: Jake McLaughlin * Contributors: Stephen Williams, Tom Moore -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * [RST-4186] Fix fuse macro names (#263) diff --git a/fuse_variables/package.xml b/fuse_variables/package.xml index 7034e1cd0..967bccd75 100644 --- a/fuse_variables/package.xml +++ b/fuse_variables/package.xml @@ -1,7 +1,7 @@ fuse_variables - 0.5.0 + 0.6.0 The fuse_variables package provides a set of commonly used variable types, such as 2D and 3D positions, orientations, velocities, and accelerations. diff --git a/fuse_viz/CHANGELOG.rst b/fuse_viz/CHANGELOG.rst index e6e244811..5e9bb4ac0 100644 --- a/fuse_viz/CHANGELOG.rst +++ b/fuse_viz/CHANGELOG.rst @@ -7,8 +7,8 @@ Changelog for package fuse_viz * Adding doxygen to all packages (#241) * Contributors: Tom Moore -Forthcoming ------------ +0.6.0 (2023-02-22) +------------------ * 0.5.0 * Update changelogs * Adding doxygen to all packages (#241) diff --git a/fuse_viz/package.xml b/fuse_viz/package.xml index 9ead3f987..77c2e2e7e 100644 --- a/fuse_viz/package.xml +++ b/fuse_viz/package.xml @@ -1,7 +1,7 @@ fuse_viz - 0.5.0 + 0.6.0 The fuse_viz package provides visualization tools for fuse. From a2191c2fb23e0ec8e3e8a58e1b1a04eb7f4c7c76 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Fri, 12 May 2023 15:26:21 -0700 Subject: [PATCH 14/24] Update devel to build on Ubuntu Jammy (22.04) (#326) * Update to C++17 for use with Ubuntu Jammy * Include Rviz and Eigen as system includes, which supresses warnings within the included libraries * use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter --- fuse_constraints/CMakeLists.txt | 34 ++++++++--------- fuse_constraints/src/absolute_constraint.cpp | 2 +- ...lute_orientation_3d_stamped_constraint.cpp | 2 +- ...rientation_3d_stamped_euler_constraint.cpp | 2 +- .../absolute_pose_2d_stamped_constraint.cpp | 2 +- .../absolute_pose_3d_stamped_constraint.cpp | 2 +- fuse_constraints/src/marginal_constraint.cpp | 2 +- fuse_constraints/src/relative_constraint.cpp | 2 +- ...tive_orientation_3d_stamped_constraint.cpp | 2 +- .../relative_pose_2d_stamped_constraint.cpp | 2 +- .../relative_pose_3d_stamped_constraint.cpp | 2 +- fuse_core/CMakeLists.txt | 36 +++++++++--------- .../include/fuse_core/graph_deserializer.h | 2 +- fuse_core/include/fuse_core/loss_loader.h | 2 +- .../fuse_core/transaction_deserializer.h | 2 +- fuse_graphs/CMakeLists.txt | 6 +-- fuse_graphs/src/hash_graph.cpp | 2 +- fuse_loss/CMakeLists.txt | 14 +++---- fuse_loss/src/arctan_loss.cpp | 2 +- fuse_loss/src/cauchy_loss.cpp | 2 +- fuse_loss/src/composed_loss.cpp | 2 +- fuse_loss/src/dcs_loss.cpp | 2 +- fuse_loss/src/fair_loss.cpp | 2 +- fuse_loss/src/geman_mcclure_loss.cpp | 2 +- fuse_loss/src/huber_loss.cpp | 2 +- fuse_loss/src/scaled_loss.cpp | 2 +- fuse_loss/src/softlone_loss.cpp | 2 +- fuse_loss/src/tolerant_loss.cpp | 2 +- fuse_loss/src/trivial_loss.cpp | 2 +- fuse_loss/src/tukey_loss.cpp | 2 +- fuse_loss/src/welsch_loss.cpp | 2 +- fuse_models/CMakeLists.txt | 16 ++++---- fuse_models/src/acceleration_2d.cpp | 2 +- fuse_models/src/graph_ignition.cpp | 2 +- fuse_models/src/imu_2d.cpp | 2 +- fuse_models/src/odometry_2d.cpp | 2 +- fuse_models/src/odometry_2d_publisher.cpp | 2 +- fuse_models/src/pose_2d.cpp | 2 +- fuse_models/src/transaction.cpp | 2 +- fuse_models/src/twist_2d.cpp | 2 +- fuse_models/src/unicycle_2d.cpp | 2 +- fuse_models/src/unicycle_2d_ignition.cpp | 2 +- ...unicycle_2d_state_kinematic_constraint.cpp | 2 +- fuse_optimizers/CMakeLists.txt | 12 +++--- .../include/fuse_optimizers/optimizer.h | 2 +- fuse_publishers/CMakeLists.txt | 8 ++-- fuse_publishers/src/path_2d_publisher.cpp | 2 +- fuse_publishers/src/pose_2d_publisher.cpp | 2 +- fuse_publishers/src/serialized_publisher.cpp | 2 +- fuse_tutorials/CMakeLists.txt | 4 +- fuse_tutorials/src/beacon_publisher.cpp | 2 +- fuse_tutorials/src/range_constraint.cpp | 2 +- fuse_tutorials/src/range_sensor_model.cpp | 2 +- fuse_variables/CMakeLists.txt | 38 +++++++++---------- .../src/acceleration_angular_2d_stamped.cpp | 2 +- .../src/acceleration_angular_3d_stamped.cpp | 2 +- .../src/acceleration_linear_2d_stamped.cpp | 2 +- .../src/acceleration_linear_3d_stamped.cpp | 2 +- fuse_variables/src/orientation_2d_stamped.cpp | 2 +- fuse_variables/src/orientation_3d_stamped.cpp | 2 +- .../src/point_2d_fixed_landmark.cpp | 2 +- fuse_variables/src/point_2d_landmark.cpp | 2 +- .../src/point_3d_fixed_landmark.cpp | 2 +- fuse_variables/src/point_3d_landmark.cpp | 2 +- fuse_variables/src/position_2d_stamped.cpp | 2 +- fuse_variables/src/position_3d_stamped.cpp | 2 +- .../src/velocity_angular_2d_stamped.cpp | 2 +- .../src/velocity_angular_3d_stamped.cpp | 2 +- .../src/velocity_linear_2d_stamped.cpp | 2 +- .../src/velocity_linear_3d_stamped.cpp | 2 +- fuse_viz/CMakeLists.txt | 5 ++- fuse_viz/src/serialized_graph_display.cpp | 2 +- 72 files changed, 150 insertions(+), 147 deletions(-) diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 75be81486..670b6337e 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -78,7 +78,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -135,7 +135,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_absolute_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -161,7 +161,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_absolute_orientation_3d_stamped_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -187,7 +187,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_absolute_orientation_3d_stamped_euler_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -211,7 +211,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_absolute_pose_2d_stamped_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -235,7 +235,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_absolute_pose_3d_stamped_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -257,7 +257,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_marginal_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -280,7 +280,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_marginalize_variables PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -304,7 +304,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_normal_delta_pose_2d PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -328,7 +328,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_normal_prior_pose_2d PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -352,7 +352,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_relative_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -376,7 +376,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_relative_pose_2d_stamped_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -400,7 +400,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_relative_pose_3d_stamped_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -419,7 +419,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_uuid_ordering PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -436,7 +436,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_variable_constraints PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -458,7 +458,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(benchmark_normal_delta_pose_2d PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() @@ -477,7 +477,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(benchmark_normal_prior_pose_2d PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_constraints/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp index fb767654c..e5496d362 100644 --- a/fuse_constraints/src/absolute_constraint.cpp +++ b/fuse_constraints/src/absolute_constraint.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp index 34765b2d5..97386d106 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp index 6758c831c..bf052e80d 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp index 31c2044bd..1f3168579 100644 --- a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index ffabb271e..74c1acf88 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/marginal_constraint.cpp b/fuse_constraints/src/marginal_constraint.cpp index 3e85498d3..c08e855b4 100644 --- a/fuse_constraints/src/marginal_constraint.cpp +++ b/fuse_constraints/src/marginal_constraint.cpp @@ -35,7 +35,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/relative_constraint.cpp b/fuse_constraints/src/relative_constraint.cpp index 1b9e63a92..7b3a890b3 100644 --- a/fuse_constraints/src/relative_constraint.cpp +++ b/fuse_constraints/src/relative_constraint.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include diff --git a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp index 73f522e3c..2efd3bf87 100644 --- a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp index af9ddc156..9b91bc535 100644 --- a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp index 1e688e1d5..7ff80f319 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt index 484a6ac99..85bee662f 100644 --- a/fuse_core/CMakeLists.txt +++ b/fuse_core/CMakeLists.txt @@ -72,7 +72,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -94,7 +94,7 @@ target_link_libraries(fuse_echo ) set_target_properties(fuse_echo PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -154,7 +154,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_async_motion_model PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -180,7 +180,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_async_publisher PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -206,7 +206,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_async_sensor_model PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -231,7 +231,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_callback_wrapper PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -256,7 +256,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -281,7 +281,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_eigen PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -298,7 +298,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_local_parameterization PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -323,7 +323,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_loss PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -347,7 +347,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_message_buffer PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -372,7 +372,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_timestamp_manager PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -398,7 +398,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_transaction PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -423,7 +423,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_parameter PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -439,7 +439,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_throttled_callback PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -463,7 +463,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_util PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -488,7 +488,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_uuid PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -513,7 +513,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_variable PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_core/include/fuse_core/graph_deserializer.h b/fuse_core/include/fuse_core/graph_deserializer.h index d4ef1969f..04d0a6073 100644 --- a/fuse_core/include/fuse_core/graph_deserializer.h +++ b/fuse_core/include/fuse_core/graph_deserializer.h @@ -38,7 +38,7 @@ #include #include #include -#include +#include namespace fuse_core diff --git a/fuse_core/include/fuse_core/loss_loader.h b/fuse_core/include/fuse_core/loss_loader.h index 69899d6db..b1d0dc326 100644 --- a/fuse_core/include/fuse_core/loss_loader.h +++ b/fuse_core/include/fuse_core/loss_loader.h @@ -35,7 +35,7 @@ #define FUSE_CORE_LOSS_LOADER_H #include -#include +#include #include diff --git a/fuse_core/include/fuse_core/transaction_deserializer.h b/fuse_core/include/fuse_core/transaction_deserializer.h index c478f0c3b..ec105e029 100644 --- a/fuse_core/include/fuse_core/transaction_deserializer.h +++ b/fuse_core/include/fuse_core/transaction_deserializer.h @@ -38,7 +38,7 @@ #include #include #include -#include +#include namespace fuse_core diff --git a/fuse_graphs/CMakeLists.txt b/fuse_graphs/CMakeLists.txt index 7bcbea337..b81f46d5f 100644 --- a/fuse_graphs/CMakeLists.txt +++ b/fuse_graphs/CMakeLists.txt @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -109,7 +109,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_hash_graph PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -135,7 +135,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(benchmark_create_problem PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index 810ca9652..b5d421cbc 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/CMakeLists.txt b/fuse_loss/CMakeLists.txt index f71d4c341..edbb52ef2 100644 --- a/fuse_loss/CMakeLists.txt +++ b/fuse_loss/CMakeLists.txt @@ -58,7 +58,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -123,7 +123,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_loss_function PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -147,7 +147,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_composed_loss PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -171,7 +171,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_huber_loss PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -195,7 +195,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_tukey_loss PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -227,7 +227,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_qwt_loss_plot PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -257,7 +257,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_scaled_loss PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_loss/src/arctan_loss.cpp b/fuse_loss/src/arctan_loss.cpp index 47520d783..9a275439c 100644 --- a/fuse_loss/src/arctan_loss.cpp +++ b/fuse_loss/src/arctan_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/cauchy_loss.cpp b/fuse_loss/src/cauchy_loss.cpp index e27261b6a..3b2d027c8 100644 --- a/fuse_loss/src/cauchy_loss.cpp +++ b/fuse_loss/src/cauchy_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/composed_loss.cpp b/fuse_loss/src/composed_loss.cpp index 28a69a5bb..554503a21 100644 --- a/fuse_loss/src/composed_loss.cpp +++ b/fuse_loss/src/composed_loss.cpp @@ -35,7 +35,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/src/dcs_loss.cpp b/fuse_loss/src/dcs_loss.cpp index 8d24cd672..3bd51c2fa 100644 --- a/fuse_loss/src/dcs_loss.cpp +++ b/fuse_loss/src/dcs_loss.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/src/fair_loss.cpp b/fuse_loss/src/fair_loss.cpp index bebfe95b7..0ab13738c 100644 --- a/fuse_loss/src/fair_loss.cpp +++ b/fuse_loss/src/fair_loss.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/src/geman_mcclure_loss.cpp b/fuse_loss/src/geman_mcclure_loss.cpp index f34d8ef3c..6924ad3ea 100644 --- a/fuse_loss/src/geman_mcclure_loss.cpp +++ b/fuse_loss/src/geman_mcclure_loss.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/src/huber_loss.cpp b/fuse_loss/src/huber_loss.cpp index db68d3c7a..b2d3a7877 100644 --- a/fuse_loss/src/huber_loss.cpp +++ b/fuse_loss/src/huber_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/scaled_loss.cpp b/fuse_loss/src/scaled_loss.cpp index f38d5d3ac..f70d33baf 100644 --- a/fuse_loss/src/scaled_loss.cpp +++ b/fuse_loss/src/scaled_loss.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/src/softlone_loss.cpp b/fuse_loss/src/softlone_loss.cpp index 8a4fcbf22..fdf7f2909 100644 --- a/fuse_loss/src/softlone_loss.cpp +++ b/fuse_loss/src/softlone_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/tolerant_loss.cpp b/fuse_loss/src/tolerant_loss.cpp index d57277e82..426a2fc4a 100644 --- a/fuse_loss/src/tolerant_loss.cpp +++ b/fuse_loss/src/tolerant_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/trivial_loss.cpp b/fuse_loss/src/trivial_loss.cpp index b21defaa6..63877209f 100644 --- a/fuse_loss/src/trivial_loss.cpp +++ b/fuse_loss/src/trivial_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/tukey_loss.cpp b/fuse_loss/src/tukey_loss.cpp index b9f74fc8d..d12753615 100644 --- a/fuse_loss/src/tukey_loss.cpp +++ b/fuse_loss/src/tukey_loss.cpp @@ -35,7 +35,7 @@ #include -#include +#include #include #include diff --git a/fuse_loss/src/welsch_loss.cpp b/fuse_loss/src/welsch_loss.cpp index fb5c72105..6780c30b9 100644 --- a/fuse_loss/src/welsch_loss.cpp +++ b/fuse_loss/src/welsch_loss.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index 597706e0a..9b861f5b9 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -113,7 +113,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -164,7 +164,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_unicycle_2d PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -180,7 +180,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_unicycle_2d_predict PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -197,7 +197,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_unicycle_2d_state_cost_function PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -223,7 +223,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_graph_ignition PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -239,7 +239,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_unicycle_2d_ignition PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -254,7 +254,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_sensor_proc PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -275,7 +275,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(benchmark_unicycle_2d_state_cost_function PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_models/src/acceleration_2d.cpp b/fuse_models/src/acceleration_2d.cpp index 46019e89b..63e314325 100644 --- a/fuse_models/src/acceleration_2d.cpp +++ b/fuse_models/src/acceleration_2d.cpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index 5977b34ae..80d5b56f1 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -36,7 +36,7 @@ #include -#include +#include #include #include diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp index 5c3bd12fc..0874faccc 100644 --- a/fuse_models/src/imu_2d.cpp +++ b/fuse_models/src/imu_2d.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_models/src/odometry_2d.cpp b/fuse_models/src/odometry_2d.cpp index 4298089ac..3ffb04b28 100644 --- a/fuse_models/src/odometry_2d.cpp +++ b/fuse_models/src/odometry_2d.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp index 28d744f09..448443cff 100644 --- a/fuse_models/src/odometry_2d_publisher.cpp +++ b/fuse_models/src/odometry_2d_publisher.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include #include diff --git a/fuse_models/src/pose_2d.cpp b/fuse_models/src/pose_2d.cpp index 026a79741..248e1ded1 100644 --- a/fuse_models/src/pose_2d.cpp +++ b/fuse_models/src/pose_2d.cpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_models/src/transaction.cpp b/fuse_models/src/transaction.cpp index fa4b80d16..e81d55fb2 100644 --- a/fuse_models/src/transaction.cpp +++ b/fuse_models/src/transaction.cpp @@ -34,7 +34,7 @@ #include -#include +#include #include // Register this sensor model with ROS as a plugin. diff --git a/fuse_models/src/twist_2d.cpp b/fuse_models/src/twist_2d.cpp index 964165a4f..5acf674db 100644 --- a/fuse_models/src/twist_2d.cpp +++ b/fuse_models/src/twist_2d.cpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 103459fc9..1da323519 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 914518196..692be13c4 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -50,7 +50,7 @@ #include #include -#include +#include #include #include #include diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp index 6d4d30e6e..1f9505a46 100644 --- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 798787394..3bb31f6f7 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -72,7 +72,7 @@ target_link_libraries(batch_optimizer_node ) set_target_properties(batch_optimizer_node PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -94,7 +94,7 @@ target_link_libraries(fixed_lag_smoother_node ) set_target_properties(fixed_lag_smoother_node PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -153,7 +153,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_variable_stamp_index PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -177,7 +177,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_optimizer PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -209,7 +209,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_fixed_lag_ignition PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index 8b2a10714..7a51c0ed5 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_publishers/CMakeLists.txt b/fuse_publishers/CMakeLists.txt index 68885d310..a2f78f010 100644 --- a/fuse_publishers/CMakeLists.txt +++ b/fuse_publishers/CMakeLists.txt @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -114,7 +114,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_path_2d_publisher PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -137,7 +137,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_pose_2d_publisher PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -158,7 +158,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_stamped_variable_synchronizer PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_publishers/src/path_2d_publisher.cpp b/fuse_publishers/src/path_2d_publisher.cpp index f1903285f..96ca9a1b1 100644 --- a/fuse_publishers/src/path_2d_publisher.cpp +++ b/fuse_publishers/src/path_2d_publisher.cpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp index 6c99b228d..506423a04 100644 --- a/fuse_publishers/src/pose_2d_publisher.cpp +++ b/fuse_publishers/src/pose_2d_publisher.cpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/fuse_publishers/src/serialized_publisher.cpp b/fuse_publishers/src/serialized_publisher.cpp index 85ecb9f44..db1e20bc8 100644 --- a/fuse_publishers/src/serialized_publisher.cpp +++ b/fuse_publishers/src/serialized_publisher.cpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include diff --git a/fuse_tutorials/CMakeLists.txt b/fuse_tutorials/CMakeLists.txt index 269c8f32f..57da1db9d 100644 --- a/fuse_tutorials/CMakeLists.txt +++ b/fuse_tutorials/CMakeLists.txt @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -72,7 +72,7 @@ target_link_libraries(range_sensor_simulator ) set_target_properties(range_sensor_simulator PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) diff --git a/fuse_tutorials/src/beacon_publisher.cpp b/fuse_tutorials/src/beacon_publisher.cpp index a42e6a3b0..cbd800403 100644 --- a/fuse_tutorials/src/beacon_publisher.cpp +++ b/fuse_tutorials/src/beacon_publisher.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/fuse_tutorials/src/range_constraint.cpp b/fuse_tutorials/src/range_constraint.cpp index 885c8e0cd..42971c993 100644 --- a/fuse_tutorials/src/range_constraint.cpp +++ b/fuse_tutorials/src/range_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_tutorials/src/range_sensor_model.cpp b/fuse_tutorials/src/range_sensor_model.cpp index 9e9e32030..1b0595412 100644 --- a/fuse_tutorials/src/range_sensor_model.cpp +++ b/fuse_tutorials/src/range_sensor_model.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/CMakeLists.txt b/fuse_variables/CMakeLists.txt index fe74f100e..ca25103e7 100644 --- a/fuse_variables/CMakeLists.txt +++ b/fuse_variables/CMakeLists.txt @@ -63,7 +63,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -120,7 +120,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_acceleration_angular_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -144,7 +144,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_acceleration_angular_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -168,7 +168,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_acceleration_linear_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -192,7 +192,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_acceleration_linear_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -215,7 +215,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_fixed_size_variable PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -239,7 +239,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_load_device_id PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -263,7 +263,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_orientation_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -287,7 +287,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_orientation_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -311,7 +311,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_point_2d_fixed_landmark PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -335,7 +335,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_point_2d_landmark PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -359,7 +359,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_point_3d_fixed_landmark PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -383,7 +383,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_point_3d_landmark PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -407,7 +407,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_position_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -431,7 +431,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_position_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -455,7 +455,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_velocity_angular_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -479,7 +479,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_velocity_angular_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -503,7 +503,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_velocity_linear_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -527,7 +527,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_velocity_linear_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_variables/src/acceleration_angular_2d_stamped.cpp b/fuse_variables/src/acceleration_angular_2d_stamped.cpp index 5d4bd545d..c65f1144b 100644 --- a/fuse_variables/src/acceleration_angular_2d_stamped.cpp +++ b/fuse_variables/src/acceleration_angular_2d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/acceleration_angular_3d_stamped.cpp b/fuse_variables/src/acceleration_angular_3d_stamped.cpp index 710be3098..37e80468d 100644 --- a/fuse_variables/src/acceleration_angular_3d_stamped.cpp +++ b/fuse_variables/src/acceleration_angular_3d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/acceleration_linear_2d_stamped.cpp b/fuse_variables/src/acceleration_linear_2d_stamped.cpp index 089dcbdaa..037e562ef 100644 --- a/fuse_variables/src/acceleration_linear_2d_stamped.cpp +++ b/fuse_variables/src/acceleration_linear_2d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/acceleration_linear_3d_stamped.cpp b/fuse_variables/src/acceleration_linear_3d_stamped.cpp index 31297d1c7..e86a0f17a 100644 --- a/fuse_variables/src/acceleration_linear_3d_stamped.cpp +++ b/fuse_variables/src/acceleration_linear_3d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp index 6abb54314..f5848c48f 100644 --- a/fuse_variables/src/orientation_2d_stamped.cpp +++ b/fuse_variables/src/orientation_2d_stamped.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/orientation_3d_stamped.cpp b/fuse_variables/src/orientation_3d_stamped.cpp index 462eea00e..f5dea4a83 100644 --- a/fuse_variables/src/orientation_3d_stamped.cpp +++ b/fuse_variables/src/orientation_3d_stamped.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/point_2d_fixed_landmark.cpp b/fuse_variables/src/point_2d_fixed_landmark.cpp index ab00f94d3..b8dbc31ff 100644 --- a/fuse_variables/src/point_2d_fixed_landmark.cpp +++ b/fuse_variables/src/point_2d_fixed_landmark.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include diff --git a/fuse_variables/src/point_2d_landmark.cpp b/fuse_variables/src/point_2d_landmark.cpp index 603b46ed2..b971a5a20 100644 --- a/fuse_variables/src/point_2d_landmark.cpp +++ b/fuse_variables/src/point_2d_landmark.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include diff --git a/fuse_variables/src/point_3d_fixed_landmark.cpp b/fuse_variables/src/point_3d_fixed_landmark.cpp index 7b92113f5..254a9f0c2 100644 --- a/fuse_variables/src/point_3d_fixed_landmark.cpp +++ b/fuse_variables/src/point_3d_fixed_landmark.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include diff --git a/fuse_variables/src/point_3d_landmark.cpp b/fuse_variables/src/point_3d_landmark.cpp index 7743b13d4..184c0d5ed 100644 --- a/fuse_variables/src/point_3d_landmark.cpp +++ b/fuse_variables/src/point_3d_landmark.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include diff --git a/fuse_variables/src/position_2d_stamped.cpp b/fuse_variables/src/position_2d_stamped.cpp index 0f7411f45..e653937f7 100644 --- a/fuse_variables/src/position_2d_stamped.cpp +++ b/fuse_variables/src/position_2d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/position_3d_stamped.cpp b/fuse_variables/src/position_3d_stamped.cpp index 66efcccbb..65dd23a0e 100644 --- a/fuse_variables/src/position_3d_stamped.cpp +++ b/fuse_variables/src/position_3d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/velocity_angular_2d_stamped.cpp b/fuse_variables/src/velocity_angular_2d_stamped.cpp index 5da8cf624..176808256 100644 --- a/fuse_variables/src/velocity_angular_2d_stamped.cpp +++ b/fuse_variables/src/velocity_angular_2d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/velocity_angular_3d_stamped.cpp b/fuse_variables/src/velocity_angular_3d_stamped.cpp index 6756716ff..5c565589e 100644 --- a/fuse_variables/src/velocity_angular_3d_stamped.cpp +++ b/fuse_variables/src/velocity_angular_3d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/velocity_linear_2d_stamped.cpp b/fuse_variables/src/velocity_linear_2d_stamped.cpp index 8dae97c31..505368111 100644 --- a/fuse_variables/src/velocity_linear_2d_stamped.cpp +++ b/fuse_variables/src/velocity_linear_2d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/velocity_linear_3d_stamped.cpp b/fuse_variables/src/velocity_linear_3d_stamped.cpp index 901aaa0a4..8661c7ec6 100644 --- a/fuse_variables/src/velocity_linear_3d_stamped.cpp +++ b/fuse_variables/src/velocity_linear_3d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_viz/CMakeLists.txt b/fuse_viz/CMakeLists.txt index aa9a0ec44..bde535755 100644 --- a/fuse_viz/CMakeLists.txt +++ b/fuse_viz/CMakeLists.txt @@ -73,6 +73,9 @@ add_dependencies(${PROJECT_NAME} target_include_directories(${PROJECT_NAME} PUBLIC include +) +target_include_directories(${PROJECT_NAME} + SYSTEM PUBLIC ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} @@ -85,7 +88,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) diff --git a/fuse_viz/src/serialized_graph_display.cpp b/fuse_viz/src/serialized_graph_display.cpp index f95bbc8c6..90679f5e2 100644 --- a/fuse_viz/src/serialized_graph_display.cpp +++ b/fuse_viz/src/serialized_graph_display.cpp @@ -322,5 +322,5 @@ void SerializedGraphDisplay::processMessage(const fuse_msgs::SerializedGraph::Co } // namespace rviz -#include +#include PLUGINLIB_EXPORT_CLASS(rviz::SerializedGraphDisplay, rviz::Display) From c5b462a4e9057aae8e97e40d798a476ed185166d Mon Sep 17 00:00:00 2001 From: locus-services <33065330+locus-services@users.noreply.github.com> Date: Sat, 3 Jun 2023 19:59:33 -0400 Subject: [PATCH 15/24] Tailor: Updating Jenkinsfile --- Jenkinsfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Jenkinsfile b/Jenkinsfile index 41e41edad..51bc58c1b 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -10,7 +10,7 @@ tailorTestPipeline( // Release label to pull test images from. release_label: 'hotdog', // OS distributions to test. - distributions: ['focal'], + distributions: ['focal', 'jammy'], // Version of tailor_meta to build against tailor_meta: '0.1.20', // Master or release branch associated with this track From 5a2138fe5d971ce7bbbece7354759d0f837ef8fe Mon Sep 17 00:00:00 2001 From: locus-services <33065330+locus-services@users.noreply.github.com> Date: Wed, 7 Jun 2023 07:39:23 -0400 Subject: [PATCH 16/24] Tailor: Updating Jenkinsfile --- Jenkinsfile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 51bc58c1b..ed91169f1 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -1,5 +1,5 @@ #!/usr/bin/env groovy -@Library('tailor-meta@0.1.20')_ +@Library('tailor-meta@0.1.21')_ tailorTestPipeline( // Name of job that generated this test definition. rosdistro_job: '/ci/rosdistro/master', @@ -12,7 +12,7 @@ tailorTestPipeline( // OS distributions to test. distributions: ['focal', 'jammy'], // Version of tailor_meta to build against - tailor_meta: '0.1.20', + tailor_meta: '0.1.21', // Master or release branch associated with this track source_branch: 'devel', // Docker registry where test image is stored From 34c5847b31a40dadc2e94e0752040923fe110a65 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 4 Jul 2023 19:03:25 -0700 Subject: [PATCH 17/24] Derive the fixed landmarks from the standard landmarks (#259) --- .../fuse_variables/point_2d_fixed_landmark.h | 61 +++------------- .../fuse_variables/point_2d_landmark.h | 10 +++ .../fuse_variables/point_3d_fixed_landmark.h | 72 +++---------------- .../fuse_variables/point_3d_landmark.h | 9 +++ .../src/point_2d_fixed_landmark.cpp | 23 +----- fuse_variables/src/point_2d_landmark.cpp | 9 ++- .../src/point_3d_fixed_landmark.cpp | 24 +------ fuse_variables/src/point_3d_landmark.cpp | 9 ++- 8 files changed, 53 insertions(+), 164 deletions(-) diff --git a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h index 1a5967e16..4d5007512 100644 --- a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h +++ b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h @@ -36,37 +36,27 @@ #include #include -#include +#include #include #include #include -#include - namespace fuse_variables { /** * @brief Variable representing a 2D point landmark that exists across time. * - * This is commonly used to represent locations of visual features. The UUID of this class is constant after - * construction and dependent on a user input database id. As such, the database id cannot be altered after - * construction. + * This is commonly used to represent locations of visual features. This class differs from the Point2DLandmark in that + * the value of the landmark is held constant during optimization. This is appropriate if the landmark positions are + * known or were previously estimated to sufficient accuracy. The UUID of this class is constant after construction and + * dependent on a user input database id. As such, the database id cannot be altered after construction. */ -class Point2DFixedLandmark : public FixedSizeVariable<2> +class Point2DFixedLandmark : public Point2DLandmark { public: FUSE_VARIABLE_DEFINITIONS(Point2DFixedLandmark); - /** - * @brief Can be used to directly index variables in the data array - */ - enum : size_t - { - X = 0, - Y = 1 - }; - /** * @brief Default constructor */ @@ -79,48 +69,14 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> */ explicit Point2DFixedLandmark(const uint64_t& landmark_id); - /** - * @brief Read-write access to the X-axis position. - */ - double& x() { return data_[X]; } - - /** - * @brief Read-only access to the X-axis position. - */ - const double& x() const { return data_[X]; } - - /** - * @brief Read-write access to the Y-axis position. - */ - double& y() { return data_[Y]; } - - /** - * @brief Read-only access to the Y-axis position. - */ - const double& y() const { return data_[Y]; } - - /** - * @brief Read-only access to the id - */ - const uint64_t& id() const { return id_; } - - /** - * @brief Print a human-readable description of the variable to the provided - * stream. - * - * @param[out] stream The stream to write to. Defaults to stdout. - */ - void print(std::ostream& stream = std::cout) const override; - /** * @brief Specifies if the value of the variable should not be changed during optimization */ - bool holdConstant() const override; + bool holdConstant() const override { return true; } private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ { 0 }; /** * @brief The Boost Serialize method that serializes all of the data members @@ -134,8 +90,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); - archive& id_; + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/point_2d_landmark.h b/fuse_variables/include/fuse_variables/point_2d_landmark.h index e6c7a6582..1ebd80d50 100644 --- a/fuse_variables/include/fuse_variables/point_2d_landmark.h +++ b/fuse_variables/include/fuse_variables/point_2d_landmark.h @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -112,6 +113,15 @@ class Point2DLandmark : public FixedSizeVariable<2> */ void print(std::ostream& stream = std::cout) const override; +protected: + /** + * @brief Construct a point 2D variable given a UUID and a landmarks id + * + * @param[in] uuid The UUID for this variable + * @param[in] landmark_id The id associated to a landmark + */ + Point2DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id); + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h index 89c8590ae..d690bf9ed 100644 --- a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h +++ b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h @@ -36,38 +36,27 @@ #include #include -#include +#include #include #include #include -#include - namespace fuse_variables { /** * @brief Variable representing a 3D point landmark that exists across time. * - * This is commonly used to represent locations of visual features. The UUID of this class is constant after - * construction and dependent on a user input database id. As such, the database id cannot be altered after - * construction. + * This is commonly used to represent locations of visual features. This class differs from the Point3DLandmark in that + * the value of the landmark is held constant during optimization. This is appropriate if the landmark positions are + * known or were previously estimated to sufficient accuracy. The UUID of this class is constant after construction and + * dependent on a user input database id. As such, the database id cannot be altered after construction. */ -class Point3DFixedLandmark : public FixedSizeVariable<3> +class Point3DFixedLandmark : public Point3DLandmark { public: FUSE_VARIABLE_DEFINITIONS(Point3DFixedLandmark); - /** - * @brief Can be used to directly index variables in the data array - */ - enum : size_t - { - X = 0, - Y = 1, - Z = 2 - }; - /** * @brief Default constructor */ @@ -80,58 +69,14 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> */ explicit Point3DFixedLandmark(const uint64_t& landmark_id); - /** - * @brief Read-write access to the X-axis position. - */ - double& x() { return data_[X]; } - - /** - * @brief Read-only access to the X-axis position. - */ - const double& x() const { return data_[X]; } - - /** - * @brief Read-write access to the Y-axis position. - */ - double& y() { return data_[Y]; } - - /** - * @brief Read-only access to the Y-axis position. - */ - const double& y() const { return data_[Y]; } - - /** - * @brief Read-write access to the Z-axis position. - */ - double& z() { return data_[Z]; } - - /** - * @brief Read-only access to the Z-axis position. - */ - const double& z() const { return data_[Z]; } - - /** - * @brief Read-only access to the id - */ - const uint64_t& id() const { return id_; } - - /** - * @brief Print a human-readable description of the variable to the provided - * stream. - * - * @param[out] stream The stream to write to. Defaults to stdout. - */ - void print(std::ostream& stream = std::cout) const override; - /** * @brief Specifies if the value of the variable should not be changed during optimization */ - bool holdConstant() const override; + bool holdConstant() const override { return true; } private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ { 0 }; /** * @brief The Boost Serialize method that serializes all of the data members @@ -145,8 +90,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); - archive& id_; + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.h b/fuse_variables/include/fuse_variables/point_3d_landmark.h index 06cf2458b..c165a68d3 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.h +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.h @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -126,6 +127,14 @@ class Point3DLandmark : public FixedSizeVariable<3> */ void print(std::ostream& stream = std::cout) const override; +protected: + /** + * @brief Construct a point 3D variable given a landmarks id + * + * @param[in] landmark_id The id associated to a landmark + */ + Point3DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id); + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/src/point_2d_fixed_landmark.cpp b/fuse_variables/src/point_2d_fixed_landmark.cpp index b8dbc31ff..05e102621 100644 --- a/fuse_variables/src/point_2d_fixed_landmark.cpp +++ b/fuse_variables/src/point_2d_fixed_landmark.cpp @@ -35,35 +35,16 @@ #include #include -#include +#include #include #include -#include - namespace fuse_variables { Point2DFixedLandmark::Point2DFixedLandmark(const uint64_t& landmark_id) : - FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) -{ -} - -void Point2DFixedLandmark::print(std::ostream& stream) const -{ - stream << type() << ":\n" - << " uuid: " << uuid() << "\n" - << " size: " << size() << "\n" - << " landmark id: " << id() << "\n" - << " data:\n" - << " - x: " << x() << "\n" - << " - y: " << y() << "\n"; -} - -bool Point2DFixedLandmark::holdConstant() const + Point2DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) { - return true; } } // namespace fuse_variables diff --git a/fuse_variables/src/point_2d_landmark.cpp b/fuse_variables/src/point_2d_landmark.cpp index b971a5a20..9d8b9fe2e 100644 --- a/fuse_variables/src/point_2d_landmark.cpp +++ b/fuse_variables/src/point_2d_landmark.cpp @@ -44,12 +44,17 @@ namespace fuse_variables { -Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) : - FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), +Point2DLandmark::Point2DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id) : + FixedSizeVariable(uuid), id_(landmark_id) { } +Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) : + Point2DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) +{ +} + void Point2DLandmark::print(std::ostream& stream) const { stream << type() << ":\n" diff --git a/fuse_variables/src/point_3d_fixed_landmark.cpp b/fuse_variables/src/point_3d_fixed_landmark.cpp index 254a9f0c2..f3288bcb7 100644 --- a/fuse_variables/src/point_3d_fixed_landmark.cpp +++ b/fuse_variables/src/point_3d_fixed_landmark.cpp @@ -35,36 +35,16 @@ #include #include -#include +#include #include #include -#include - namespace fuse_variables { Point3DFixedLandmark::Point3DFixedLandmark(const uint64_t& landmark_id) : - FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) -{ -} - -void Point3DFixedLandmark::print(std::ostream& stream) const -{ - stream << type() << ":\n" - << " uuid: " << uuid() << "\n" - << " size: " << size() << "\n" - << " landmark id: " << id() << "\n" - << " data:\n" - << " - x: " << x() << "\n" - << " - y: " << y() << "\n" - << " - z: " << z() << "\n"; -} - -bool Point3DFixedLandmark::holdConstant() const + Point3DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) { - return true; } } // namespace fuse_variables diff --git a/fuse_variables/src/point_3d_landmark.cpp b/fuse_variables/src/point_3d_landmark.cpp index 184c0d5ed..4010f01db 100644 --- a/fuse_variables/src/point_3d_landmark.cpp +++ b/fuse_variables/src/point_3d_landmark.cpp @@ -44,12 +44,17 @@ namespace fuse_variables { -Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) : - FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), +Point3DLandmark::Point3DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id) : + FixedSizeVariable(uuid), id_(landmark_id) { } +Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) : + Point3DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) +{ +} + void Point3DLandmark::print(std::ostream& stream) const { stream << type() << ":\n" From ad14fb803747474c1dce770367eb3c231acee680 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Perdomo Date: Wed, 5 Jul 2023 04:04:03 +0200 Subject: [PATCH 18/24] Minor header fixes (#266) * Use fuse_macros.h instead of deprecated macros.h * Add missed header * Sort headers --- fuse_constraints/test/test_marginalize_variables.cpp | 2 +- fuse_models/include/fuse_models/imu_2d.h | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/fuse_constraints/test/test_marginalize_variables.cpp b/fuse_constraints/test/test_marginalize_variables.cpp index 42f40b212..f2e26be5c 100644 --- a/fuse_constraints/test/test_marginalize_variables.cpp +++ b/fuse_constraints/test/test_marginalize_variables.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/fuse_models/include/fuse_models/imu_2d.h b/fuse_models/include/fuse_models/imu_2d.h index d9257df91..7a3671b19 100644 --- a/fuse_models/include/fuse_models/imu_2d.h +++ b/fuse_models/include/fuse_models/imu_2d.h @@ -34,13 +34,14 @@ #ifndef FUSE_MODELS_IMU_2D_H #define FUSE_MODELS_IMU_2D_H -#include #include +#include #include #include #include +#include #include #include #include From 2eb4b9a906ca050c22e15342d6026363f9460a76 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Perdomo Date: Wed, 5 Jul 2023 04:22:26 +0200 Subject: [PATCH 19/24] Add missed geometry_msgs package (#272) * Add geometry_msgs as a test_depend. * Make geometry_msgs includable. --------- Co-authored-by: Ivor Wanders --- fuse_core/CMakeLists.txt | 7 +++++++ fuse_core/package.xml | 1 + 2 files changed, 8 insertions(+) diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt index 85bee662f..ee0d6d863 100644 --- a/fuse_core/CMakeLists.txt +++ b/fuse_core/CMakeLists.txt @@ -126,6 +126,7 @@ install( if(CATKIN_ENABLE_TESTING) find_package(roslint REQUIRED) find_package(rostest REQUIRED) + find_package(geometry_msgs REQUIRED) # Lint tests set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references") @@ -437,6 +438,12 @@ if(CATKIN_ENABLE_TESTING) ${PROJECT_NAME} ${catkin_LIBRARIES} ) + target_include_directories(test_throttled_callback + PRIVATE + include + ${catkin_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ) set_target_properties(test_throttled_callback PROPERTIES CXX_STANDARD 17 diff --git a/fuse_core/package.xml b/fuse_core/package.xml index 239733db4..1a8421947 100644 --- a/fuse_core/package.xml +++ b/fuse_core/package.xml @@ -20,6 +20,7 @@ rosconsole roslint rostest + geometry_msgs From 4bac7e42034ca4eda91b6d641b1a9ffe523bc805 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Perdomo Date: Wed, 5 Jul 2023 05:04:04 +0200 Subject: [PATCH 20/24] Print graph & transaction on optimization failure (#321) --- fuse_optimizers/src/fixed_lag_smoother.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 2bd516e25..2dc5d7976 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -223,20 +223,26 @@ void FixedLagSmoother::optimizationLoop() // Optimize the entire graph summary_ = graph_->optimize(params_.solver_options); - // 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()); - // Abort if optimization failed. Not converging is not a failure because the solution found is usable. if (!summary_.IsSolutionUsable()) { + std::ostringstream oss; + oss << "Graph:\n"; + graph_->print(oss); + oss << "\nTransaction:\n"; + new_transaction->print(oss); + ROS_FATAL_STREAM("Optimization failed after updating the graph with the transaction with timestamp " - << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); + << new_transaction->stamp() << ". Leaving optimization loop and requesting node shutdown...\n" + << oss.str()); ROS_INFO_STREAM(summary_.FullReport()); ros::requestShutdown(); break; } + // Optimization is complete and succeeded. Notify all the things about the graph changes. + notify(std::move(new_transaction), graph_->clone()); + // Compute a transaction that marginalizes out those variables. lag_expiration_ = computeLagExpirationTime(); marginal_transaction_ = fuse_constraints::marginalizeVariables( From 8021ad726d86fc35277f6fdfbae72e710efeca49 Mon Sep 17 00:00:00 2001 From: fabianhirmann <117293434+fabianhirmann@users.noreply.github.com> Date: Mon, 10 Jul 2023 15:33:40 +0200 Subject: [PATCH 21/24] add missing tf timeout at differential mode of IMU, odometry, and pose sensor model (#322) --- fuse_models/src/imu_2d.cpp | 4 ++-- fuse_models/src/odometry_2d.cpp | 4 ++-- fuse_models/src/pose_2d.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp index 0874faccc..24a4ae0e1 100644 --- a/fuse_models/src/imu_2d.cpp +++ b/fuse_models/src/imu_2d.cpp @@ -222,7 +222,7 @@ void Imu2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped& transformed_pose->header.frame_id = params_.orientation_target_frame.empty() ? pose.header.frame_id : params_.orientation_target_frame; - if (!common::transformMessage(tf_buffer_, pose, *transformed_pose)) + if (!common::transformMessage(tf_buffer_, pose, *transformed_pose, params_.tf_timeout)) { ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform pose message with stamp " << pose.header.stamp << " to orientation target frame " @@ -242,7 +242,7 @@ void Imu2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped& transformed_twist.header.frame_id = params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - if (!common::transformMessage(tf_buffer_, twist, transformed_twist)) + if (!common::transformMessage(tf_buffer_, twist, transformed_twist, params_.tf_timeout)) { ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform twist message with stamp " << twist.header.stamp << " to twist target frame " diff --git a/fuse_models/src/odometry_2d.cpp b/fuse_models/src/odometry_2d.cpp index 3ffb04b28..73bc06d18 100644 --- a/fuse_models/src/odometry_2d.cpp +++ b/fuse_models/src/odometry_2d.cpp @@ -165,7 +165,7 @@ void Odometry2D::processDifferential(const geometry_msgs::PoseWithCovarianceStam transformed_pose->header.frame_id = params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; - if (!common::transformMessage(tf_buffer_, pose, *transformed_pose)) + if (!common::transformMessage(tf_buffer_, pose, *transformed_pose, params_.tf_timeout)) { ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform pose message with stamp " << pose.header.stamp << " to pose target frame " << params_.pose_target_frame); @@ -184,7 +184,7 @@ void Odometry2D::processDifferential(const geometry_msgs::PoseWithCovarianceStam transformed_twist.header.frame_id = params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - if (!common::transformMessage(tf_buffer_, twist, transformed_twist)) + if (!common::transformMessage(tf_buffer_, twist, transformed_twist, params_.tf_timeout)) { ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform twist message with stamp " << twist.header.stamp << " to twist target frame " diff --git a/fuse_models/src/pose_2d.cpp b/fuse_models/src/pose_2d.cpp index 248e1ded1..66fb2e1a1 100644 --- a/fuse_models/src/pose_2d.cpp +++ b/fuse_models/src/pose_2d.cpp @@ -131,7 +131,7 @@ void Pose2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped& auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = params_.target_frame.empty() ? pose.header.frame_id : params_.target_frame; - if (!common::transformMessage(tf_buffer_, pose, *transformed_pose)) + if (!common::transformMessage(tf_buffer_, pose, *transformed_pose, params_.tf_timeout)) { ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform pose message with stamp " << pose.header.stamp << " to target frame " << params_.target_frame); From 7af1e706cf617dbfc1bec82bf38d4736b8fb5977 Mon Sep 17 00:00:00 2001 From: David Murdoch Date: Thu, 21 Sep 2023 10:53:15 -0400 Subject: [PATCH 22/24] Adding error handling everywhere --- ..._prior_orientation_3d_euler_cost_functor.h | 5 ++-- .../src/marginalize_variables.cpp | 5 ++-- fuse_core/include/fuse_core/error_handler.h | 16 +++++++++++++ fuse_core/include/fuse_core/parameter.h | 2 +- fuse_core/src/ceres_options.cpp | 3 ++- fuse_graphs/src/hash_graph.cpp | 24 +++++++++++-------- .../fuse_models/common/sensor_config.h | 3 ++- .../include/fuse_models/common/sensor_proc.h | 7 +++--- .../parameters/unicycle_2d_ignition_params.h | 9 +++---- fuse_models/src/graph_ignition.cpp | 9 +++---- fuse_models/src/unicycle_2d.cpp | 23 +++++++++--------- fuse_models/src/unicycle_2d_ignition.cpp | 15 ++++++------ fuse_viz/include/fuse_viz/conversions.h | 5 ++-- fuse_viz/src/serialized_graph_display.cpp | 3 ++- 14 files changed, 80 insertions(+), 49 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h index 1d949fed3..6f9638629 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h @@ -35,6 +35,7 @@ #define FUSE_CONSTRAINTS_NORMAL_PRIOR_ORIENTATION_3D_EULER_COST_FUNCTOR_H #include +#include #include #include @@ -105,7 +106,7 @@ class NormalPriorOrientation3DEulerCostFunctor for (size_t i = 0; i < axes_.size(); ++i) { - T angle; + T angle = T(0); switch (axes_[i]) { case Euler::ROLL: @@ -125,7 +126,7 @@ class NormalPriorOrientation3DEulerCostFunctor } default: { - throw std::runtime_error("The provided axis specified is unknown. " + fuse_core::ErrorHandler::getHandler().runtimeError("The provided axis specified is unknown. " "I should probably be more informative here"); } } diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index 22d9a8950..df61efb6f 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include @@ -147,7 +148,7 @@ UuidOrdering computeEliminationOrder( variable_groups.data()); if (!success) { - throw std::runtime_error("Failed to call CCOLAMD to generate the elimination order."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to call CCOLAMD to generate the elimination order."); } // Extract the elimination order from CCOLAMD. @@ -321,7 +322,7 @@ LinearTerm linearize( } if (!success) { - throw std::runtime_error("Error in evaluating the cost function. There are two possible reasons. " + fuse_core::ErrorHandler::getHandler().runtimeError("Error in evaluating the cost function. There are two possible reasons. " "Either the CostFunction did not evaluate and fill all residual and jacobians " "that were requested or there was a non-finite value (nan/infinite) generated " "during the jacobian computation."); diff --git a/fuse_core/include/fuse_core/error_handler.h b/fuse_core/include/fuse_core/error_handler.h index 31a7cfec4..a8c27754a 100644 --- a/fuse_core/include/fuse_core/error_handler.h +++ b/fuse_core/include/fuse_core/error_handler.h @@ -55,6 +55,22 @@ namespace fuse_core { +/** + * @brief Used to provide different error handlers for a given context + */ +enum class Context: int +{ + CORE = 0, + CONSTRAINT = 1, + GRAPH = 2, + LOSS = 3, + MODEL = 4, + OPTIMIZER = 5, + PUBLISHER = 6, + VARIABLE = 7, + VIZ = 8 +}; + class ErrorHandler { public: diff --git a/fuse_core/include/fuse_core/parameter.h b/fuse_core/include/fuse_core/parameter.h index 48aac4db6..ba627d61b 100644 --- a/fuse_core/include/fuse_core/parameter.h +++ b/fuse_core/include/fuse_core/parameter.h @@ -62,7 +62,7 @@ void getParamRequired(const ros::NodeHandle& nh, const std::string& key, T& valu { const std::string error = "Could not find required parameter " + key + " in namespace " + nh.getNamespace(); ROS_FATAL_STREAM(error); - throw std::runtime_error(error); + fuse_core::ErrorHandler::getHandler().runtimeError(error); } } diff --git a/fuse_core/src/ceres_options.cpp b/fuse_core/src/ceres_options.cpp index 99acbcc50..b78e9e008 100644 --- a/fuse_core/src/ceres_options.cpp +++ b/fuse_core/src/ceres_options.cpp @@ -32,6 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include @@ -195,7 +196,7 @@ void loadSolverOptionsFromROS(const ros::NodeHandle& nh, ceres::Solver::Options& std::string error; if (!solver_options.IsValid(&error)) { - throw std::invalid_argument("Invalid solver options in parameter " + nh.getNamespace() + ". Error: " + error); + ErrorHandler::getHandler().invalidArgument("Invalid solver options in parameter " + nh.getNamespace() + ". Error: " + error); } } diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index b5d421cbc..e7df7a011 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -33,6 +33,7 @@ */ #include +#include #include #include @@ -127,7 +128,7 @@ bool HashGraph::addConstraint(fuse_core::Constraint::SharedPtr constraint) { if (!variableExists(variable_uuid)) { - throw std::logic_error("Attempting to add a constraint (" + fuse_core::uuid::to_string(constraint->uuid()) + + fuse_core::ErrorHandler::getHandler().logicError("Attempting to add a constraint (" + fuse_core::uuid::to_string(constraint->uuid()) + ") that uses an unknown variable (" + fuse_core::uuid::to_string(variable_uuid) + ")."); } } @@ -165,7 +166,7 @@ const fuse_core::Constraint& HashGraph::getConstraint(const fuse_core::UUID& con auto constraints_iter = constraints_.find(constraint_uuid); if (constraints_iter == constraints_.end()) { - throw std::out_of_range("The constraint UUID " + fuse_core::uuid::to_string(constraint_uuid) + " does not exist."); + fuse_core::ErrorHandler::getHandler().outOfRangeError("The constraint UUID " + fuse_core::uuid::to_string(constraint_uuid) + " does not exist."); } return *constraints_iter->second; } @@ -207,8 +208,11 @@ fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(cons 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::ErrorHandler::getHandler().logicError("Attempting to access constraints connected to variable (" + fuse_core::uuid::to_string(variable_uuid) + "), but that variable does not exist in this graph."); + // Only here to satisfy compiler warnings. Be warned that if your ErrorHandler doesn't throw an exception of some kind, + // this may exhibit undesirable behavior. + return fuse_core::Graph::const_constraint_range(); } } @@ -245,7 +249,7 @@ bool HashGraph::removeVariable(const fuse_core::UUID& variable_uuid) auto cross_reference_iter = constraints_by_variable_uuid_.find(variable_uuid); if (cross_reference_iter != constraints_by_variable_uuid_.end() && !cross_reference_iter->second.empty()) { - throw std::logic_error("Attempting to remove a variable (" + fuse_core::uuid::to_string(variable_uuid) + fuse_core::ErrorHandler::getHandler().logicError("Attempting to remove a variable (" + fuse_core::uuid::to_string(variable_uuid) + ") that is used by existing constraints (" + fuse_core::uuid::to_string(cross_reference_iter->second.front()) + " plus " + std::to_string(cross_reference_iter->second.size() - 1) + " others)."); } @@ -264,7 +268,7 @@ const fuse_core::Variable& HashGraph::getVariable(const fuse_core::UUID& variabl auto variables_iter = variables_.find(variable_uuid); if (variables_iter == variables_.end()) { - throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(variable_uuid) + " does not exist."); + fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + fuse_core::uuid::to_string(variable_uuid) + " does not exist."); } return *variables_iter->second; } @@ -337,13 +341,13 @@ void HashGraph::getCovariance( auto variable1_iter = variables_.find(request.first); if (variable1_iter == variables_.end()) { - throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(request.first) + fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + fuse_core::uuid::to_string(request.first) + " does not exist."); } auto variable2_iter = variables_.find(request.second); if (variable2_iter == variables_.end()) { - throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(request.second) + fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + fuse_core::uuid::to_string(request.second) + " does not exist."); } // Both variables exist. Continue processing. @@ -374,7 +378,7 @@ void HashGraph::getCovariance( ceres::Covariance covariance(options); if (!covariance.Compute(unique_covariance_blocks, &problem)) { - throw std::runtime_error("Could not compute requested covariance blocks."); + fuse_core::ErrorHandler::getHandler().runtimeError("Could not compute requested covariance blocks."); } // Populate the computed covariance blocks into the output variable. if (use_tangent_space) @@ -388,7 +392,7 @@ void HashGraph::getCovariance( output_matrix.data())) { const auto& request = covariance_requests.at(i); - throw std::runtime_error("Could not get covariance block for variable UUIDs " + + fuse_core::ErrorHandler::getHandler().runtimeError("Could not get covariance block for variable UUIDs " + fuse_core::uuid::to_string(request.first) + " and " + fuse_core::uuid::to_string(request.second) + "."); } @@ -405,7 +409,7 @@ void HashGraph::getCovariance( output_matrix.data())) { const auto& request = covariance_requests.at(i); - throw std::runtime_error("Could not get covariance block for variable UUIDs " + + fuse_core::ErrorHandler::getHandler().runtimeError("Could not get covariance block for variable UUIDs " + fuse_core::uuid::to_string(request.first) + " and " + fuse_core::uuid::to_string(request.second) + "."); } diff --git a/fuse_models/include/fuse_models/common/sensor_config.h b/fuse_models/include/fuse_models/common/sensor_config.h index ad6968c73..96ffa68f0 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.h +++ b/fuse_models/include/fuse_models/common/sensor_config.h @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -66,7 +67,7 @@ inline void throwDimensionError(const std::string& dimension) { std::string error = "Dimension " + dimension + " is not valid for this type."; ROS_ERROR_STREAM(error); - throw std::runtime_error(error); + fuse_core::ErrorHandler::getHandler().runtimeError(error); } /** diff --git a/fuse_models/include/fuse_models/common/sensor_proc.h b/fuse_models/include/fuse_models/common/sensor_proc.h index 8629b4b9a..ce20c098b 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.h +++ b/fuse_models/include/fuse_models/common/sensor_proc.h @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -196,18 +197,18 @@ inline void validatePartialMeasurement( { if (!mean_partial.allFinite()) { - throw std::runtime_error("Invalid partial mean " + fuse_core::to_string(mean_partial)); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid partial mean " + fuse_core::to_string(mean_partial)); } if (!fuse_core::isSymmetric(covariance_partial, precision)) { - throw std::runtime_error("Non-symmetric partial covariance matrix\n" + + fuse_core::ErrorHandler::getHandler().runtimeError("Non-symmetric partial covariance matrix\n" + fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); } if (!fuse_core::isPositiveDefinite(covariance_partial)) { - throw std::runtime_error("Non-positive-definite partial covariance matrix\n" + + fuse_core::ErrorHandler::getHandler().runtimeError("Non-positive-definite partial covariance matrix\n" + fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); } } diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h index 08ea2a71a..1ba9c51f6 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h +++ b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -77,7 +78,7 @@ struct Unicycle2DIgnitionParams : public ParameterBase { if (sigma_vector.size() != 8) { - throw std::invalid_argument("The supplied initial_sigma parameter must be length 8, but is actually length " + + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_sigma parameter must be length 8, but is actually length " + std::to_string(sigma_vector.size())); } auto is_sigma_valid = [](const double sigma) @@ -86,7 +87,7 @@ struct Unicycle2DIgnitionParams : public ParameterBase }; if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { - throw std::invalid_argument("The supplied initial_sigma parameter must contain valid floating point values. " + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_sigma parameter must contain valid floating point values. " "NaN, Inf, and values <= 0 are not acceptable."); } initial_sigma.swap(sigma_vector); @@ -97,7 +98,7 @@ struct Unicycle2DIgnitionParams : public ParameterBase { if (state_vector.size() != 8) { - throw std::invalid_argument("The supplied initial_state parameter must be length 8, but is actually length " + + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_state parameter must be length 8, but is actually length " + std::to_string(state_vector.size())); } auto is_state_valid = [](const double state) @@ -106,7 +107,7 @@ struct Unicycle2DIgnitionParams : public ParameterBase }; if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { - throw std::invalid_argument("The supplied initial_state parameter must contain valid floating point values. " + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_state parameter must contain valid floating point values. " "NaN, Inf, etc are not acceptable."); } initial_state.swap(state_vector); diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index 80d5b56f1..f54854113 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -33,6 +33,7 @@ */ #include +#include #include @@ -113,7 +114,7 @@ void GraphIgnition::process(const fuse_msgs::SerializedGraph& msg) // Verify we are in the correct state to process set graph requests if (!started_) { - throw std::runtime_error("Attempting to set the graph while the sensor is stopped."); + fuse_core::ErrorHandler::getHandler().runtimeError("Attempting to set the graph while the sensor is stopped."); } // Deserialize the graph message @@ -122,12 +123,12 @@ void GraphIgnition::process(const fuse_msgs::SerializedGraph& msg) // Validate the requested graph before we do anything if (boost::empty(graph->getConstraints())) { - throw std::runtime_error("Attempting to set a graph with no constraints."); + fuse_core::ErrorHandler::getHandler().runtimeError("Attempting to set a graph with no constraints."); } if (boost::empty(graph->getVariables())) { - throw std::runtime_error("Attempting to set a graph with no variables."); + fuse_core::ErrorHandler::getHandler().runtimeError("Attempting to set a graph with no variables."); } // Tell the optimizer to reset before providing the initial state @@ -143,7 +144,7 @@ void GraphIgnition::process(const fuse_msgs::SerializedGraph& msg) if (!reset_client_.call(srv)) { // The reset() service failed. Propagate that failure to the caller of this service. - throw std::runtime_error("Failed to call the '" + reset_client_.getService() + "' service."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to call the '" + reset_client_.getService() + "' service."); } } diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 1da323519..54c799eee 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -99,13 +100,13 @@ inline void validateCovariance(const Eigen::DenseBase& covariance, { if (!fuse_core::isSymmetric(covariance, precision)) { - throw std::runtime_error("Non-symmetric partial covariance matrix\n" + + fuse_core::ErrorHandler::getHandler().runtimeError("Non-symmetric partial covariance matrix\n" + fuse_core::to_string(covariance, Eigen::FullPrecision)); } if (!fuse_core::isPositiveDefinite(covariance)) { - throw std::runtime_error("Non-positive-definite partial covariance matrix\n" + + fuse_core::ErrorHandler::getHandler().runtimeError("Non-positive-definite partial covariance matrix\n" + fuse_core::to_string(covariance, Eigen::FullPrecision)); } } @@ -150,22 +151,22 @@ void Unicycle2D::StateHistoryElement::validate() const { if (!std::isfinite(pose)) { - throw std::runtime_error("Invalid pose " + std::to_string(pose)); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid pose " + std::to_string(pose)); } if (!std::isfinite(velocity_linear)) { - throw std::runtime_error("Invalid linear velocity " + std::to_string(velocity_linear)); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid linear velocity " + std::to_string(velocity_linear)); } if (!std::isfinite(velocity_yaw)) { - throw std::runtime_error("Invalid yaw velocity " + std::to_string(velocity_yaw)); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid yaw velocity " + std::to_string(velocity_yaw)); } if (!std::isfinite(acceleration_linear)) { - throw std::runtime_error("Invalid linear acceleration " + std::to_string(acceleration_linear)); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid linear acceleration " + std::to_string(acceleration_linear)); } } @@ -198,7 +199,7 @@ void Unicycle2D::onInit() if (process_noise_diagonal.size() != 8) { - throw std::runtime_error("Process noise diagonal must be of length 8!"); + fuse_core::ErrorHandler::getHandler().runtimeError("Process noise diagonal must be of length 8!"); } process_noise_covariance_ = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal(); @@ -213,7 +214,7 @@ void Unicycle2D::onInit() if (buffer_length < 0.0) { - throw std::runtime_error("Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); } buffer_length_ = (buffer_length == 0.0) ? ros::DURATION_MAX : ros::Duration(buffer_length); @@ -488,7 +489,7 @@ void Unicycle2D::validateMotionModel(const StateHistoryElement& state1, const St } catch (const std::runtime_error& ex) { - throw std::runtime_error("Invalid state #1: " + std::string(ex.what())); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid state #1: " + std::string(ex.what())); } try @@ -497,7 +498,7 @@ void Unicycle2D::validateMotionModel(const StateHistoryElement& state1, const St } catch (const std::runtime_error& ex) { - throw std::runtime_error("Invalid state #2: " + std::string(ex.what())); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid state #2: " + std::string(ex.what())); } try @@ -506,7 +507,7 @@ void Unicycle2D::validateMotionModel(const StateHistoryElement& state1, const St } catch (const std::runtime_error& ex) { - throw std::runtime_error("Invalid process noise covariance: " + std::string(ex.what())); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid process noise covariance: " + std::string(ex.what())); } } diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 5f162ab40..6b4d856fc 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -181,12 +182,12 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& // Verify we are in the correct state to process set pose requests if (!started_) { - throw std::runtime_error("Attempting to set the pose while the sensor is stopped."); + fuse_core::ErrorHandler::getHandler().runtimeError("Attempting to set the pose while the sensor is stopped."); } // Validate the requested pose and covariance before we do anything if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y)) { - throw std::invalid_argument("Attempting to set the pose to an invalid position (" + + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose to an invalid position (" + std::to_string(pose.pose.pose.position.x) + ", " + std::to_string(pose.pose.pose.position.y) + ")."); } @@ -196,7 +197,7 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); if (std::abs(orientation_norm - 1.0) > 1.0e-3) { - throw std::invalid_argument("Attempting to set the pose to an invalid orientation (" + + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose to an invalid orientation (" + std::to_string(pose.pose.pose.orientation.x) + ", " + std::to_string(pose.pose.pose.orientation.y) + ", " + std::to_string(pose.pose.pose.orientation.z) + ", " + @@ -207,19 +208,19 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& pose.pose.covariance[6], pose.pose.covariance[7]; if (!fuse_core::isSymmetric(position_cov)) { - throw std::invalid_argument("Attempting to set the pose with a non-symmetric position covariance matri\n " + + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a non-symmetric position covariance matri\n " + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } if (!fuse_core::isPositiveDefinite(position_cov)) { - throw std::invalid_argument("Attempting to set the pose with a non-positive-definite position covariance matrix\n" + + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a non-positive-definite position covariance matrix\n" + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } auto orientation_cov = fuse_core::Matrix1d(); orientation_cov << pose.pose.covariance[35]; if (orientation_cov(0) <= 0.0) { - throw std::invalid_argument("Attempting to set the pose with a non-positive-definite orientation covariance " + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a non-positive-definite orientation covariance " "matrix " + fuse_core::to_string(orientation_cov) + "."); } @@ -236,7 +237,7 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& if (!reset_client_.call(srv)) { // The reset() service failed. Propagate that failure to the caller of this service. - throw std::runtime_error("Failed to call the '" + reset_client_.getService() + "' service."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to call the '" + reset_client_.getService() + "' service."); } } diff --git a/fuse_viz/include/fuse_viz/conversions.h b/fuse_viz/include/fuse_viz/conversions.h index 8f8d16b41..8e36ef8e6 100644 --- a/fuse_viz/include/fuse_viz/conversions.h +++ b/fuse_viz/include/fuse_viz/conversions.h @@ -35,6 +35,7 @@ #ifndef FUSE_VIZ_CONVERSIONS_H #define FUSE_VIZ_CONVERSIONS_H +#include #include #include #include @@ -137,7 +138,7 @@ inline tf2::Transform getPose(const fuse_core::Graph& graph, const fuse_core::UU const auto position = dynamic_cast(&graph.getVariable(position_uuid)); if (!position) { - throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(position_uuid) + + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to get variable " + fuse_core::uuid::to_string(position_uuid) + " from graph as fuse_variables::Position2DStamped."); } @@ -145,7 +146,7 @@ inline tf2::Transform getPose(const fuse_core::Graph& graph, const fuse_core::UU dynamic_cast(&graph.getVariable(orientation_uuid)); if (!orientation) { - throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(orientation_uuid) + + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to get variable " + fuse_core::uuid::to_string(orientation_uuid) + " from graph as fuse_variables::Orientation2DStamped."); } diff --git a/fuse_viz/src/serialized_graph_display.cpp b/fuse_viz/src/serialized_graph_display.cpp index 90679f5e2..2d548c288 100644 --- a/fuse_viz/src/serialized_graph_display.cpp +++ b/fuse_viz/src/serialized_graph_display.cpp @@ -51,6 +51,7 @@ #include #include +#include #include #include #include @@ -249,7 +250,7 @@ void SerializedGraphDisplay::processMessage(const fuse_msgs::SerializedGraph::Co { delete constraint_source_property; - throw std::runtime_error("Failed to insert " + description); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to insert " + description); } show_constraints_property_->addChild(constraint_source_property, From 40d229f070e3f1a4277286be78318d4dec2faa78 Mon Sep 17 00:00:00 2001 From: David Murdoch Date: Wed, 27 Sep 2023 21:51:34 -0400 Subject: [PATCH 23/24] Reworking it to add contexts in a better way, linting errors, unit tests --- fuse_core/CMakeLists.txt | 22 +++ fuse_core/include/fuse_core/error_handler.h | 187 ++++++++++++++++---- fuse_core/src/ceres_options.cpp | 3 +- fuse_core/test/test_error_handler.cpp | 136 ++++++++++++++ 4 files changed, 308 insertions(+), 40 deletions(-) create mode 100644 fuse_core/test/test_error_handler.cpp diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt index ee0d6d863..17c4cf966 100644 --- a/fuse_core/CMakeLists.txt +++ b/fuse_core/CMakeLists.txt @@ -286,6 +286,28 @@ if(CATKIN_ENABLE_TESTING) CXX_STANDARD_REQUIRED YES ) + # Error handler tests + catkin_add_gtest(test_error_handler + test/test_error_handler.cpp + ) + add_dependencies(test_error_handler + ${catkin_EXPORTED_TARGETS} + ) + target_include_directories(test_error_handler + PRIVATE + include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ) + target_link_libraries(test_error_handler + ${catkin_LIBRARIES} + ) + set_target_properties(test_error_handler + PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + # Local Parameterization tests catkin_add_gtest(test_local_parameterization test/test_local_parameterization.cpp diff --git a/fuse_core/include/fuse_core/error_handler.h b/fuse_core/include/fuse_core/error_handler.h index a8c27754a..5a8e8e79b 100644 --- a/fuse_core/include/fuse_core/error_handler.h +++ b/fuse_core/include/fuse_core/error_handler.h @@ -37,6 +37,8 @@ #include "ros/ros.h" #include +#include +#include /** * @brief A class that handles errors that can occur in fuse. @@ -46,96 +48,203 @@ * throw exceptions and hope that the rest of the system goes on while fuse resets and fixes itself. * * This class exists to allow for handling generic classes of errors using whatever callback the user sees fit to use. - * I recommend registering new callbacks that are bound to an Optimizer instance for more specific functionality. + * The default behavior is to throw the relevant exception. Any function that has void return and accepts a string + * can be used for this, however. * - * A note: By default, the base Optimizer class registers the basic callbacks that throw error specific exceptions. - * basicCallback below should never be used, and is only provided for simplifying initialisation. + * The error handler also allows for different error handlers to be used in different contexts. It assumes that unless + * otherwise specified, all error handling is done generically. However, errors in particular contexts can be handled + * with unique callbacks as needed. + * + * It's recommended that you register any needed error non-default callbacks during Optimizer startup. */ namespace fuse_core { /** - * @brief Used to provide different error handlers for a given context + * @brief Used to provide different error handlers for a given context. */ -enum class Context: int +enum class ErrorContext: int { - CORE = 0, - CONSTRAINT = 1, - GRAPH = 2, - LOSS = 3, - MODEL = 4, - OPTIMIZER = 5, - PUBLISHER = 6, - VARIABLE = 7, - VIZ = 8 + GENERIC, + CORE, + CONSTRAINT, + GRAPH, + LOSS, + MODEL, + OPTIMIZER, + PUBLISHER, + VARIABLE, + VIZ }; class ErrorHandler { + using ErrorCb = std::function; public: + /** + * @brief Gets a reference to the error handler. + * + * @param[out] handler - reference to the error handler + */ static ErrorHandler& getHandler() { static ErrorHandler handler {}; return handler; } - - void invalidArgument(const std::string& info) + /** + * @brief Use whatever callback is registered to handle an invalid argument error for a given context. + * + * @param[in] info A string containing information about the error. + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + * @throws std::out_of_range if no callback has been registered for the provided context + */ + void invalidArgument(const std::string& info, ErrorContext context = ErrorContext::GENERIC) { - invalid_argument_cb_(info); + try + { + auto cb = invalid_argument_cbs_.at(context); + cb(info); + } + catch(const std::out_of_range& e) + { + throw std::out_of_range("Failed to call invalidArgument Error handling, no callback found for error " + info); + } + } + /** + * @brief Use whatever callback is registered to handle a runtime error for a given context. + * + * @param[in] info A string containing information about the error. + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + * @throws std::out_of_range if no callback has been registered for the provided context + */ + void runtimeError(const std::string& info, ErrorContext context = ErrorContext::GENERIC) + { + try + { + auto cb = runtime_error_cbs_.at(context); + cb(info); + } + catch(const std::out_of_range& e) + { + throw std::out_of_range("Failed to call runtimeError Error handling, no callback found for error " + info); + } } - void runtimeError(const std::string& info) + /** + * @brief Use whatever callback is registered to handle an out of range error for a given context. + * + * @param[in] info A string containing information about the error. + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + * @throws std::out_of_range if no callback has been registered for the provided context + */ + void outOfRangeError(const std::string& info, ErrorContext context = ErrorContext::GENERIC) { - runtime_error_cb_(info); + try + { + auto cb = out_of_range_cbs_.at(context); + cb(info); + } + catch(const std::out_of_range& e) + { + throw std::out_of_range("Failed to call outOfRangeError handling, no callback found for error " + info); + } } - void outOfRangeError(const std::string& info) + /** + * @brief Use whatever callback is registered to handle a logic error for a given context. + * + * @param[in] info A string containing information about the error. + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + * @throws std::out_of_range if no callback has been registered for the provided context + */ + void logicError(const std::string& info, ErrorContext context = ErrorContext::GENERIC) { - out_of_range_cb_(info); + try + { + auto cb = logic_error_cbs_.at(context); + cb(info); + } + catch(const std::out_of_range& e) + { + throw std::out_of_range("Failed to call logicError handling, no callback found for error " + info); + } } - void logicError(const std::string& info) + /** + * @brief Register a callback to be used for invalid argument errors in a particular context + * + * @param[in] cb The callback to be registered + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + */ + void registerinvalidArgumentErrorCb(ErrorCb cb, ErrorContext context = ErrorContext::GENERIC) { - logic_error_cb_(info); + invalid_argument_cbs_[context] = cb; } - void registerinvalidArgumentErrorCb(std::function cb) + /** + * @brief Register a callback to be used for runtime errors in a particular context + * + * @param[in] cb The callback to be registered + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + */ + void registerRuntimeErrorCb(ErrorCb cb, ErrorContext context = ErrorContext::GENERIC) { - invalid_argument_cb_ = cb; + runtime_error_cbs_[context] = cb; } - void registerRuntimeErrorCb(std::function cb) + /** + * @brief Register a callback to be used for out of range errors in a particular context + * + * @param[in] cb The callback to be registered + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + */ + void registerOutOfRangeErrorCb(ErrorCb cb, ErrorContext context = ErrorContext::GENERIC) { - runtime_error_cb_ = cb; + out_of_range_cbs_[context] = cb; } - void registerOutOfRangeErrorCb(std::function cb) + /** + * @brief Register a callback to be used for logic errors in a particular context + * + * @param[in] cb The callback to be registered + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + */ + void registerLogicErrorCb(ErrorCb cb, ErrorContext context = ErrorContext::GENERIC) { - out_of_range_cb_ = cb; + logic_error_cbs_[context] = cb; } - void registerLogicErrorCb(std::function cb) + // Should be used only for teardown and setup during unit tests + void reset() { - logic_error_cb_ = cb; + invalid_argument_cbs_.clear(); + runtime_error_cbs_.clear(); + out_of_range_cbs_.clear(); + logic_error_cbs_.clear(); + + invalid_argument_cbs_.insert(std::make_pair(ErrorContext::GENERIC, invalidArgumentCallback)); + runtime_error_cbs_.insert(std::make_pair(ErrorContext::GENERIC, runtimeErrorCallback)); + out_of_range_cbs_.insert(std::make_pair(ErrorContext::GENERIC, outOfRangeErrorCallback)); + logic_error_cbs_.insert(std::make_pair(ErrorContext::GENERIC, logicErrorCallback)); } private: ErrorHandler() { - invalid_argument_cb_ = invalidArgumentCallback; - runtime_error_cb_ = runtimeErrorCallback; - out_of_range_cb_ = outOfRangeErrorCallback; - logic_error_cb_ = logicErrorCallback; + invalid_argument_cbs_[ErrorContext::GENERIC] = invalidArgumentCallback; + runtime_error_cbs_[ErrorContext::GENERIC] = runtimeErrorCallback; + out_of_range_cbs_[ErrorContext::GENERIC] = outOfRangeErrorCallback; + logic_error_cbs_[ErrorContext::GENERIC] = logicErrorCallback; } ~ErrorHandler() {} - std::function invalid_argument_cb_; - std::function runtime_error_cb_; - std::function out_of_range_cb_; - std::function logic_error_cb_; + std::unordered_map invalid_argument_cbs_; + std::unordered_map runtime_error_cbs_; + std::unordered_map out_of_range_cbs_; + std::unordered_map logic_error_cbs_; /* * Default error handling behavior diff --git a/fuse_core/src/ceres_options.cpp b/fuse_core/src/ceres_options.cpp index b78e9e008..550a5d3f2 100644 --- a/fuse_core/src/ceres_options.cpp +++ b/fuse_core/src/ceres_options.cpp @@ -196,7 +196,8 @@ void loadSolverOptionsFromROS(const ros::NodeHandle& nh, ceres::Solver::Options& std::string error; if (!solver_options.IsValid(&error)) { - ErrorHandler::getHandler().invalidArgument("Invalid solver options in parameter " + nh.getNamespace() + ". Error: " + error); + ErrorHandler::getHandler().invalidArgument( + "Invalid solver options in parameter " + nh.getNamespace() + ". Error: " + error); } } diff --git a/fuse_core/test/test_error_handler.cpp b/fuse_core/test/test_error_handler.cpp new file mode 100644 index 000000000..b45fbe48a --- /dev/null +++ b/fuse_core/test/test_error_handler.cpp @@ -0,0 +1,136 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Clearpath 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 + + +// Simple toy class for use in testing callback registration +class ErrorTester +{ + public: + ErrorTester() + { + errors_ = 0; + } + + void testFunction(std::string unused) + { + ++errors_; + } + + int errors_; +}; + +TEST(ErrorHandler, DefaultErrorHandler) +{ + // Tests are not guaranteed to be called in order. Ensure cleanup + fuse_core::ErrorHandler::getHandler().reset(); + + // Verify that default behavior for global throws exceptions + // Many other unit tests use expected exceptions for their unit tests. This test is to see if they've been broken + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().invalidArgument("Expected invalid"), std::invalid_argument); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().outOfRangeError("Expected out of range"), std::out_of_range); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().logicError("Expected logic error"), std::logic_error); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().runtimeError("Expected runtime error"), std::runtime_error); + + // Verify that callbacks can be registered as expected + { + ErrorTester test; + + fuse_core::ErrorHandler::getHandler().registerinvalidArgumentErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1)); + fuse_core::ErrorHandler::getHandler().registerRuntimeErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1)); + fuse_core::ErrorHandler::getHandler().registerOutOfRangeErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1)); + fuse_core::ErrorHandler::getHandler().registerLogicErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1)); + + fuse_core::ErrorHandler::getHandler().invalidArgument("Expected invalid argument"); + fuse_core::ErrorHandler::getHandler().outOfRangeError("Expected out of range"); + fuse_core::ErrorHandler::getHandler().logicError("Expected logic error"); + fuse_core::ErrorHandler::getHandler().runtimeError("Expected runtime error"); + + EXPECT_EQ(test.errors_, 4); + } +} + +TEST(ErrorHandler, ContextErrorHandler) +{ + // Tests are not guaranteed to be called in order. Ensure cleanup + fuse_core::ErrorHandler::getHandler().reset(); + + // By default, no context except for the generic one has a defined error callback + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().invalidArgument("Expected invalid", + fuse_core::ErrorContext::CORE), std::out_of_range); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().outOfRangeError("Expected out of range", + fuse_core::ErrorContext::CONSTRAINT), std::out_of_range); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().logicError("Expected logic error", + fuse_core::ErrorContext::GRAPH), std::out_of_range); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().runtimeError("Expected runtime error", + fuse_core::ErrorContext::LOSS), std::out_of_range); + + // Verify that callbacks can be registered as expected for different contexts + { + ErrorTester test; + + fuse_core::ErrorHandler::getHandler().registerinvalidArgumentErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1), fuse_core::ErrorContext::CORE); + fuse_core::ErrorHandler::getHandler().registerRuntimeErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1), fuse_core::ErrorContext::CONSTRAINT); + fuse_core::ErrorHandler::getHandler().registerOutOfRangeErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1), fuse_core::ErrorContext::GRAPH); + fuse_core::ErrorHandler::getHandler().registerLogicErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1), fuse_core::ErrorContext::LOSS); + + fuse_core::ErrorHandler::getHandler().invalidArgument("Expected invalid Argument", fuse_core::ErrorContext::CORE); + fuse_core::ErrorHandler::getHandler().outOfRangeError("Expected out of Range", fuse_core::ErrorContext::GRAPH); + fuse_core::ErrorHandler::getHandler().logicError("Expected logic error", fuse_core::ErrorContext::LOSS); + fuse_core::ErrorHandler::getHandler().runtimeError("Expected runtime error", fuse_core::ErrorContext::CONSTRAINT); + + EXPECT_EQ(test.errors_, 4); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} From 7a0b3d2d509a19e4cb7bb187dfe4078e65ce37e3 Mon Sep 17 00:00:00 2001 From: David Murdoch Date: Wed, 27 Sep 2023 23:01:33 -0400 Subject: [PATCH 24/24] Linting errors --- .../src/marginalize_variables.cpp | 3 +- fuse_graphs/src/hash_graph.cpp | 30 +++++++++----- .../parameters/unicycle_2d_ignition_params.h | 20 ++++++---- fuse_models/src/graph_ignition.cpp | 3 +- fuse_models/src/unicycle_2d.cpp | 6 ++- fuse_models/src/unicycle_2d_ignition.cpp | 21 ++++++---- .../include/fuse_optimizers/optimizer.h | 1 - fuse_optimizers/src/fixed_lag_smoother.cpp | 35 ----------------- fuse_optimizers/src/optimizer.cpp | 39 ++++++++++++------- fuse_viz/include/fuse_viz/conversions.h | 10 +++-- 10 files changed, 85 insertions(+), 83 deletions(-) diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index df61efb6f..ff346f5aa 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -322,7 +322,8 @@ LinearTerm linearize( } if (!success) { - fuse_core::ErrorHandler::getHandler().runtimeError("Error in evaluating the cost function. There are two possible reasons. " + fuse_core::ErrorHandler::getHandler().runtimeError("Error in evaluating the cost function. " + "There are two possible reasons. " "Either the CostFunction did not evaluate and fill all residual and jacobians " "that were requested or there was a non-finite value (nan/infinite) generated " "during the jacobian computation."); diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index e7df7a011..e2bbebdd2 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -128,8 +128,10 @@ bool HashGraph::addConstraint(fuse_core::Constraint::SharedPtr constraint) { if (!variableExists(variable_uuid)) { - fuse_core::ErrorHandler::getHandler().logicError("Attempting to add a constraint (" + fuse_core::uuid::to_string(constraint->uuid()) + - ") that uses an unknown variable (" + fuse_core::uuid::to_string(variable_uuid) + ")."); + fuse_core::ErrorHandler::getHandler().logicError("Attempting to add a constraint (" + + fuse_core::uuid::to_string(constraint->uuid()) + + ") that uses an unknown variable (" + + fuse_core::uuid::to_string(variable_uuid) + ")."); } } // Add the constraint to the list of known constraints @@ -166,7 +168,9 @@ const fuse_core::Constraint& HashGraph::getConstraint(const fuse_core::UUID& con auto constraints_iter = constraints_.find(constraint_uuid); if (constraints_iter == constraints_.end()) { - fuse_core::ErrorHandler::getHandler().outOfRangeError("The constraint UUID " + fuse_core::uuid::to_string(constraint_uuid) + " does not exist."); + fuse_core::ErrorHandler::getHandler().outOfRangeError("The constraint UUID " + + fuse_core::uuid::to_string(constraint_uuid) + + " does not exist."); } return *constraints_iter->second; } @@ -210,7 +214,8 @@ fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(cons // We only want to throw if the requested variable does not exist. fuse_core::ErrorHandler::getHandler().logicError("Attempting to access constraints connected to variable (" + fuse_core::uuid::to_string(variable_uuid) + "), but that variable does not exist in this graph."); - // Only here to satisfy compiler warnings. Be warned that if your ErrorHandler doesn't throw an exception of some kind, + // Only here to satisfy compiler warnings. Be warned that if your ErrorHandler + // doesn't throw an exception of some kind, // this may exhibit undesirable behavior. return fuse_core::Graph::const_constraint_range(); } @@ -249,7 +254,8 @@ bool HashGraph::removeVariable(const fuse_core::UUID& variable_uuid) auto cross_reference_iter = constraints_by_variable_uuid_.find(variable_uuid); if (cross_reference_iter != constraints_by_variable_uuid_.end() && !cross_reference_iter->second.empty()) { - fuse_core::ErrorHandler::getHandler().logicError("Attempting to remove a variable (" + fuse_core::uuid::to_string(variable_uuid) + fuse_core::ErrorHandler::getHandler().logicError("Attempting to remove a variable (" + + fuse_core::uuid::to_string(variable_uuid) + ") that is used by existing constraints (" + fuse_core::uuid::to_string(cross_reference_iter->second.front()) + " plus " + std::to_string(cross_reference_iter->second.size() - 1) + " others)."); } @@ -268,7 +274,9 @@ const fuse_core::Variable& HashGraph::getVariable(const fuse_core::UUID& variabl auto variables_iter = variables_.find(variable_uuid); if (variables_iter == variables_.end()) { - fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + fuse_core::uuid::to_string(variable_uuid) + " does not exist."); + fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + + fuse_core::uuid::to_string(variable_uuid) + + " does not exist."); } return *variables_iter->second; } @@ -341,14 +349,16 @@ void HashGraph::getCovariance( auto variable1_iter = variables_.find(request.first); if (variable1_iter == variables_.end()) { - fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + fuse_core::uuid::to_string(request.first) - + " does not exist."); + fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + + fuse_core::uuid::to_string(request.first) + + " does not exist."); } auto variable2_iter = variables_.find(request.second); if (variable2_iter == variables_.end()) { - fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + fuse_core::uuid::to_string(request.second) - + " does not exist."); + fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + + fuse_core::uuid::to_string(request.second) + + " does not exist."); } // Both variables exist. Continue processing. // Create the output covariance matrix diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h index 1ba9c51f6..327965631 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h +++ b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h @@ -78,8 +78,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase { if (sigma_vector.size() != 8) { - fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_sigma parameter must be length 8, but is actually length " + - std::to_string(sigma_vector.size())); + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_sigma parameter must " + "be length 8, but is actually length " + + std::to_string(sigma_vector.size())); } auto is_sigma_valid = [](const double sigma) { @@ -87,8 +88,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase }; if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { - fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_sigma parameter must contain valid floating point values. " - "NaN, Inf, and values <= 0 are not acceptable."); + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_sigma parameter must " + "contain valid floating point values. " + "NaN, Inf, and values <= 0 are not acceptable."); } initial_sigma.swap(sigma_vector); } @@ -98,8 +100,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase { if (state_vector.size() != 8) { - fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_state parameter must be length 8, but is actually length " + - std::to_string(state_vector.size())); + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_state parameter " + "must be length 8, but is actually length " + + std::to_string(state_vector.size())); } auto is_state_valid = [](const double state) { @@ -107,8 +110,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase }; if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { - fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_state parameter must contain valid floating point values. " - "NaN, Inf, etc are not acceptable."); + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_state parameter must " + "contain valid floating point values. " + "NaN, Inf, etc are not acceptable."); } initial_state.swap(state_vector); } diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index f54854113..b0d949a63 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -144,7 +144,8 @@ void GraphIgnition::process(const fuse_msgs::SerializedGraph& msg) if (!reset_client_.call(srv)) { // The reset() service failed. Propagate that failure to the caller of this service. - fuse_core::ErrorHandler::getHandler().runtimeError("Failed to call the '" + reset_client_.getService() + "' service."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to call the '" + + reset_client_.getService() + "' service."); } } diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 54c799eee..537c4a610 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -166,7 +166,8 @@ void Unicycle2D::StateHistoryElement::validate() const if (!std::isfinite(acceleration_linear)) { - fuse_core::ErrorHandler::getHandler().runtimeError("Invalid linear acceleration " + std::to_string(acceleration_linear)); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid linear acceleration " + + std::to_string(acceleration_linear)); } } @@ -214,7 +215,8 @@ void Unicycle2D::onInit() if (buffer_length < 0.0) { - fuse_core::ErrorHandler::getHandler().runtimeError("Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid negative buffer length of " + + std::to_string(buffer_length) + " specified."); } buffer_length_ = (buffer_length == 0.0) ? ros::DURATION_MAX : ros::Duration(buffer_length); diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 6b4d856fc..79c2646a1 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -208,20 +208,25 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& pose.pose.covariance[6], pose.pose.covariance[7]; if (!fuse_core::isSymmetric(position_cov)) { - fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a non-symmetric position covariance matri\n " + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a " + "non-symmetric position covariance matri\n " + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + + "."); } if (!fuse_core::isPositiveDefinite(position_cov)) { - fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a non-positive-definite position covariance matrix\n" + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a " + "non-positive-definite position covariance matrix\n" + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + + "."); } auto orientation_cov = fuse_core::Matrix1d(); orientation_cov << pose.pose.covariance[35]; if (orientation_cov(0) <= 0.0) { - fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a non-positive-definite orientation covariance " - "matrix " + fuse_core::to_string(orientation_cov) + "."); + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a " + "non-positive-definite orientation covariance " + "matrix " + fuse_core::to_string(orientation_cov) + "."); } // Tell the optimizer to reset before providing the initial state @@ -237,7 +242,9 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& if (!reset_client_.call(srv)) { // The reset() service failed. Propagate that failure to the caller of this service. - fuse_core::ErrorHandler::getHandler().runtimeError("Failed to call the '" + reset_client_.getService() + "' service."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to call the '" + + reset_client_.getService() + + "' service."); } } diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index 7a51c0ed5..22e8ce7f0 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -160,7 +160,6 @@ class Optimizer Publishers publishers_; //!< The set of publishers to execute after every graph optimization pluginlib::ClassLoader sensor_model_loader_; //!< Pluginlib class loader for SensorModels SensorModels sensor_models_; //!< The set of sensor models, addressable by name - diagnostic_updater::Updater diagnostic_updater_; //!< Diagnostic updater ros::Timer diagnostic_updater_timer_; //!< Diagnostic updater timer diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 2dc5d7976..dad9058c6 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -638,39 +638,4 @@ void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrappe } } -// void FixedLagSmoother::resetOptimizer(const std::string& err_msg) -// { -// // ROS_ERROR(err_msg.c_str()); -// // Tell all the plugins to stop -// stopPlugins(); -// // Reset the optimizer state -// { -// std::lock_guard lock(optimization_requested_mutex_); -// optimization_request_ = false; -// } -// started_ = false; -// ignited_ = false; -// setStartTime(ros::Time(0, 0)); -// // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the -// // pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to -// // prevent the possibility of deadlocks. -// { -// std::lock_guard lock(optimization_mutex_); -// // Clear all pending transactions -// { -// std::lock_guard lock(pending_transactions_mutex_); -// pending_transactions_.clear(); -// } -// // Clear the graph and marginal tracking states -// graph_->clear(); -// marginal_transaction_ = fuse_core::Transaction(); -// timestamp_tracking_.clear(); -// lag_expiration_ = ros::Time(0, 0); -// } -// // Tell all the plugins to start -// startPlugins(); -// // Test for auto-start -// autostart(); -// } - } // namespace fuse_optimizers diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 2edefbccd..814bb0079 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -143,8 +143,9 @@ void Optimizer::loadMotionModels() || (!motion_model.hasMember("name")) || (!motion_model.hasMember("type"))) { - fuse_core::ErrorHandler::getHandler().invalidArgument("The 'motion_models' parameter should be a list of the form: " - "-{name: string, type: string}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'motion_models' parameter should be " + "a list of the form: " + "-{name: string, type: string}"); } motion_model_configs.emplace_back(static_cast(motion_model["name"]), @@ -160,8 +161,9 @@ void Optimizer::loadMotionModels() if ( (motion_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!motion_model_config.hasMember("type"))) { - fuse_core::ErrorHandler::getHandler().invalidArgument("The 'motion_models' parameter should be a struct of the form: " - "{string: {type: string}}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'motion_models' parameter should be" + " a struct of the form: " + "{string: {type: string}}"); } motion_model_configs.emplace_back(static_cast(motion_model.first), @@ -209,8 +211,10 @@ void Optimizer::loadSensorModels() || (!sensor_model.hasMember("name")) || (!sensor_model.hasMember("type"))) { - fuse_core::ErrorHandler::getHandler().invalidArgument("The 'sensor_models' parameter should be a list of the form: " - "-{name: string, type: string, motion_models: [name1, name2, ...]}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'sensor_models' parameter should be " + " a list of the form: " + "-{name: string, type: string, " + "motion_models: [name1, name2, ...]}"); } sensor_model_configs.emplace_back(static_cast(sensor_model["name"]), @@ -227,8 +231,10 @@ void Optimizer::loadSensorModels() if ( (sensor_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!sensor_model_config.hasMember("type"))) { - fuse_core::ErrorHandler::getHandler().invalidArgument("The 'sensor_models' parameter should be a struct of the form: " - "{string: {type: string, motion_models: [name1, name2, ...]}}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'sensor_models' parameter should be " + "a struct of the form: " + "{string: {type: string, " + "motion_models: [name1, name2, ...]}}"); } sensor_model_configs.emplace_back(static_cast(sensor_model.first), @@ -302,8 +308,9 @@ void Optimizer::loadPublishers() || (!publisher.hasMember("name")) || (!publisher.hasMember("type"))) { - fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be a list of the form: " - "-{name: string, type: string}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be " + "a list of the form: " + "-{name: string, type: string}"); } publisher_configs.emplace_back(static_cast(publisher["name"]), @@ -320,8 +327,9 @@ void Optimizer::loadPublishers() if ( (publisher_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!publisher_config.hasMember("type"))) { - fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be a struct of the form: " - "{string: {type: string}}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be " + "a struct of the form: " + "{string: {type: string}}"); } publisher_configs.emplace_back(static_cast(publisher.first), @@ -330,8 +338,11 @@ void Optimizer::loadPublishers() } else { - fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be a list of the form: " - "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be " + "a list of the form: " + "-{name: string, type: string} or " + "a struct of the form: " + "{string: {type: string}}"); } for (const auto& config : publisher_configs) diff --git a/fuse_viz/include/fuse_viz/conversions.h b/fuse_viz/include/fuse_viz/conversions.h index 8e36ef8e6..4d7dc86de 100644 --- a/fuse_viz/include/fuse_viz/conversions.h +++ b/fuse_viz/include/fuse_viz/conversions.h @@ -138,16 +138,18 @@ inline tf2::Transform getPose(const fuse_core::Graph& graph, const fuse_core::UU const auto position = dynamic_cast(&graph.getVariable(position_uuid)); if (!position) { - fuse_core::ErrorHandler::getHandler().runtimeError("Failed to get variable " + fuse_core::uuid::to_string(position_uuid) + - " from graph as fuse_variables::Position2DStamped."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to get variable " + + fuse_core::uuid::to_string(position_uuid) + + " from graph as fuse_variables::Position2DStamped."); } const auto orientation = dynamic_cast(&graph.getVariable(orientation_uuid)); if (!orientation) { - fuse_core::ErrorHandler::getHandler().runtimeError("Failed to get variable " + fuse_core::uuid::to_string(orientation_uuid) + - " from graph as fuse_variables::Orientation2DStamped."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to get variable " + + fuse_core::uuid::to_string(orientation_uuid) + + " from graph as fuse_variables::Orientation2DStamped."); } return getPose(*position, *orientation);