diff --git a/fuse_core/include/fuse_core/variable.h b/fuse_core/include/fuse_core/variable.h index 2b7252b14..22ee9b4ab 100644 --- a/fuse_core/include/fuse_core/variable.h +++ b/fuse_core/include/fuse_core/variable.h @@ -325,7 +325,7 @@ class Variable * @param[in] index The variable dimension of interest * @return The lower bound for the requested variable dimension */ - virtual double lowerBound(size_t index) const + virtual double lowerBound(size_t /* index */) const { return std::numeric_limits::lowest(); } @@ -338,7 +338,7 @@ class Variable * @param[in] index The variable dimension of interest * @return The upper bound for the requested variable dimension */ - virtual double upperBound(size_t index) const + virtual double upperBound(size_t /* index */) const { return std::numeric_limits::max(); } diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 798787394..55bf90843 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -34,6 +34,7 @@ add_compile_options(-Wall -Werror) add_library(${PROJECT_NAME} src/batch_optimizer.cpp src/fixed_lag_smoother.cpp + src/step_wise_fixed_lag_smoother.cpp src/optimizer.cpp src/variable_stamp_index.cpp ) @@ -98,6 +99,28 @@ set_target_properties(fixed_lag_smoother_node CXX_STANDARD_REQUIRED YES ) +## step_wise_fixed_lag_smoother node +add_executable(step_wise_fixed_lag_smoother_node + src/step_wise_fixed_lag_smoother_node.cpp +) +add_dependencies(step_wise_fixed_lag_smoother_node + ${catkin_EXPORTED_TARGETS} +) +target_include_directories(step_wise_fixed_lag_smoother_node + PRIVATE + include + ${catkin_INCLUDE_DIRS} +) +target_link_libraries(step_wise_fixed_lag_smoother_node + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +set_target_properties(step_wise_fixed_lag_smoother_node + PROPERTIES + CXX_STANDARD 14 + CXX_STANDARD_REQUIRED YES +) + ############# ## Install ## ############# @@ -114,6 +137,7 @@ install( TARGETS batch_optimizer_node fixed_lag_smoother_node + step_wise_fixed_lag_smoother_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h index cc92ee4c8..9a3416bfb 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h @@ -50,7 +50,7 @@ namespace fuse_optimizers { /** - * @brief Defines the set of parameters required by the fuse_optimizers::FixedLagSmoother class + * @brief Defines the set of parameters required by the fuse_optimizers::BatchOptimizer class */ struct BatchOptimizerParams { diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index eefacac2a..fbd080f9f 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -126,6 +126,8 @@ class FixedLagSmoother : public Optimizer */ virtual ~FixedLagSmoother(); + virtual void startOptimization(); + protected: /** * Structure containing the information required to process a transaction after it was received. diff --git a/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother.h new file mode 100644 index 000000000..62bce5fe8 --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother.h @@ -0,0 +1,175 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, ARTI - Autonomous Robot Technology GmbH + * 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_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_H +#define FUSE_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + + +namespace fuse_optimizers +{ + +/** + * @brief A step-wise fixed-lag smoother implementation that marginalizes out variables that are older than a defined lag time + * and optimizes step-wise adding more and more constraints from different sources + * + * This implementation assumes that all added variable types are either derived from the fuse_variables::Stamped class, + * or are directly connected to at least one fuse_variables::Stamped variable via a constraint. The current time of + * the fixed-lag smoother is determined by the newest stamp of all added fuse_variables::Stamped variables. + * + * During optimization: + * (1) a set of new variables and constraints are computed + * (2) each optimization step adds the constraints with the defined source(s) to the graph + * (2) the augmented graph is optimized and the variable values are updated + * (3) steps 2 and 3 are continued until all optimization steps are completed + * (3) all motion models, sensors, and publishers are notified of the updated graph + * (4) all variables older than "current time - lag duration" are marginalized out. + * + * Optimization is performed at a fixed frequency, controlled by the \p optimization_frequency parameter. Received + * sensor transactions are queued while the optimization is processing, then applied to the graph at the start of the + * next optimization cycle. If the previous optimization is not yet complete when the optimization period elapses, + * then a warning will be logged but a new optimization will *not* be started. The previous optimization will run to + * completion, and the next optimization will not begin until the next scheduled optimization period. + * + * Parameters: + * - lag_duration (float, default: 5.0) The duration of the smoothing window in seconds + * - motion_models (struct array) The set of motion model plugins to load + * @code{.yaml} + * - name: string (A unique name for this motion model) + * type: string (The plugin loader class string for the desired motion model type) + * - ... + * @endcode + * - optimization_frequency (float, default: 10.0) The target frequency for optimization cycles. If an optimization + * takes longer than expected, an optimization cycle may be skipped. + * - publishers (struct array) The set of publisher plugins to load + * @code{.yaml} + * - name: string (A unique name for this publisher) + * type: string (The plugin loader class string for the desired publisher type) + * - ... + * @endcode + * - sensor_models (struct array) The set of sensor model plugins to load + * @code{.yaml} + * - name: string (A unique name for this sensor model) + * type: string (The plugin loader class string for the desired sensor model type) + * motion_models: [name1, name2, ...] (An optional list of motion model names that should be applied) + * - ... + * @endcode + * - transaction_timeout (float, default: 0.10) The maximum time to wait for motion models to be generated for a + * received transactions. Transactions are processes sequentially, so + * no new transactions will be added to the graph while waiting for + * motion models to be generated. Once the timeout expires, that + * transaction will be deleted from the queue. + * - optimization_steps (struct array) The set of sources that should be optimized for each optimization step + * It is recommended to always add the ignition source(s) and motion model source(s) + * to the first optimization step. The constraints and variables of the marginalization + * are always added to the first optimization step. + * @code{.yaml} + * - sources: string array (A list of strings of the names of the sources of the constraints. + * The names refer to the names that are used in the other parameters sensor_models and motion_models) + * - ... + * @endcode + * full example: + * @code{.yaml} + * optimization_steps: + * - + * sources: + * - "initial_localization_sensor" + * - "unicycle_motion_model" + * - + * sources: + * - "odometry_sensor" + * - "imu_sensor" + * - + * sources: + * - "scan_to_scan" + * - + * sources: + * - "scan_to_map" + * @endcode + */ +class StepWiseFixedLagSmoother : public FixedLagSmoother +{ +public: + FUSE_SMART_PTR_DEFINITIONS(StepWiseFixedLagSmoother); + using ParameterType = StepWiseFixedLagSmootherParams; + + /** + * @brief Constructor + * + * @param[in] graph The derived graph object. This allows different graph implementations to be used + * with the same optimizer code. + * @param[in] node_handle A node handle in the global namespace + * @param[in] private_node_handle A node handle in the node's private namespace + */ + StepWiseFixedLagSmoother( + fuse_core::Graph::UniquePtr graph, + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + + /** + * @brief Destructor + */ + virtual ~StepWiseFixedLagSmoother(); + + /** + * @brief Function that optimizes all constraints, designed to be run in a separate thread. + * + * This function waits for an optimization or shutdown signal, then either calls optimize() or exits appropriately. + */ + void optimizationLoop(); + + void startOptimization() override; + +protected: + // Read-only after construction + ParameterType params_; //!< Configuration settings for this fixed-lag smoother +}; + +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_H diff --git a/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother_params.h new file mode 100644 index 000000000..76ff18de2 --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/step_wise_fixed_lag_smoother_params.h @@ -0,0 +1,106 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, ARTI - Autonomous Robot Technology GmbH + * 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_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_PARAMS_H +#define FUSE_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_PARAMS_H + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + + +namespace fuse_optimizers +{ + +/** + * @brief Defines the set of parameters required by the fuse_optimizers::StepWiseFixedLagSmoother class + */ +struct StepWiseFixedLagSmootherParams : FixedLagSmootherParams +{ +public: + /** + * @brief The ordered list of steps for the optimization with each source inside + * + * The optimization is performed sequentially where each step adds + * more constraints from different sources to the graph. + */ + std::vector> optimization_steps; + + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] nh - The ROS node handle with which to load parameters + */ + void loadFromROS(const ros::NodeHandle& nh) + { + FixedLagSmootherParams::loadFromROS(nh); + + // load optimization steps + XmlRpc::XmlRpcValue optimization_steps_raw; + fuse_core::getParamRequired(nh, "optimization_steps", optimization_steps_raw); + + ROS_ASSERT(optimization_steps_raw.getType() == XmlRpc::XmlRpcValue::TypeArray); + + for (int step_i = 0; step_i < optimization_steps_raw.size(); ++step_i) + { + ROS_ASSERT(optimization_steps_raw[step_i].getType() == XmlRpc::XmlRpcValue::TypeStruct); + + ROS_ASSERT(optimization_steps_raw[step_i]["sources"].getType() == XmlRpc::XmlRpcValue::TypeArray); + + std::vector optimization_step_sources; + + for (int source_i = 0; source_i < optimization_steps_raw[step_i]["sources"].size(); ++source_i) + { + ROS_ASSERT(optimization_steps_raw[step_i]["sources"][source_i].getType() == XmlRpc::XmlRpcValue::TypeString); + + optimization_step_sources.push_back( + static_cast(optimization_steps_raw[step_i]["sources"][source_i])); + } + + optimization_steps.push_back(optimization_step_sources); + } + } +}; + +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS_STEP_WISE_FIXED_LAG_SMOOTHER_PARAMS_H diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index a91850825..d1e6dff0b 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -93,6 +93,15 @@ FixedLagSmoother::FixedLagSmoother( // Test for auto-start autostart(); + // Advertise a service that resets the optimizer to its initial state + reset_service_server_ = node_handle_.advertiseService( + ros::names::resolve(params_.reset_service), + &FixedLagSmoother::resetServiceCallback, + this); +} + +void FixedLagSmoother::startOptimization() +{ // Start the optimization thread optimization_thread_ = std::thread(&FixedLagSmoother::optimizationLoop, this); @@ -101,12 +110,6 @@ FixedLagSmoother::FixedLagSmoother( params_.optimization_period, &FixedLagSmoother::optimizerTimerCallback, this); - - // Advertise a service that resets the optimizer to its initial state - reset_service_server_ = node_handle_.advertiseService( - ros::names::resolve(params_.reset_service), - &FixedLagSmoother::resetServiceCallback, - this); } FixedLagSmoother::~FixedLagSmoother() diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp index 25c8230fb..bdf34dc3a 100644 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother_node.cpp @@ -44,6 +44,7 @@ int main(int argc, char **argv) fuse_graphs::HashGraphParams hash_graph_params; hash_graph_params.loadFromROS(private_node_handle); fuse_optimizers::FixedLagSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params)); + optimizer.startOptimization(); ros::spin(); return 0; diff --git a/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp b/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp new file mode 100644 index 000000000..ee5b5d0dd --- /dev/null +++ b/fuse_optimizers/src/step_wise_fixed_lag_smoother.cpp @@ -0,0 +1,236 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fuse_optimizers +{ + +StepWiseFixedLagSmoother::StepWiseFixedLagSmoother( + fuse_core::Graph::UniquePtr graph, + const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) : + fuse_optimizers::FixedLagSmoother(std::move(graph), node_handle, private_node_handle) +{ + params_.loadFromROS(private_node_handle); + + // add its own marginal constraints source to the optimization steps + if (params_.optimization_steps.empty()) + { + params_.optimization_steps.push_back({ros::this_node::getName()}); // NOLINT + } + else + { + params_.optimization_steps[0].push_back(ros::this_node::getName()); + } +} + +void StepWiseFixedLagSmoother::startOptimization() +{ + // Start the optimization thread + optimization_thread_ = std::thread(&StepWiseFixedLagSmoother::optimizationLoop, this); + + // Configure a timer to trigger optimizations + optimize_timer_ = node_handle_.createTimer( + params_.optimization_period, + &StepWiseFixedLagSmoother::optimizerTimerCallback, + this); +} + +StepWiseFixedLagSmoother::~StepWiseFixedLagSmoother() +{ + // Wake up any sleeping threads + optimization_running_ = false; + optimization_requested_.notify_all(); + // Wait for the threads to shutdown + if (optimization_thread_.joinable()) + { + optimization_thread_.join(); + } +} + +void StepWiseFixedLagSmoother::optimizationLoop() +{ + auto exit_wait_condition = [this]() + { + return this->optimization_request_ || !this->optimization_running_ || !ros::ok(); + }; + // Optimize constraints until told to exit + while (ros::ok() && optimization_running_) + { + // Wait for the next signal to start the next optimization cycle + auto optimization_deadline = ros::Time(0, 0); + { + std::unique_lock lock(optimization_requested_mutex_); + optimization_requested_.wait(lock, exit_wait_condition); + optimization_request_ = false; + optimization_deadline = optimization_deadline_; + } + // If a shutdown is requested, exit now. + if (!optimization_running_ || !ros::ok()) + { + break; + } + // Optimize + { + std::lock_guard lock(optimization_mutex_); + // Apply motion models + auto new_transaction = fuse_core::Transaction::make_shared(); + // DANGER: processQueue obtains a lock from the pending_transactions_mutex_ + // We do this to ensure state of the graph does not change between unlocking the pending_transactions + // queue and obtaining the lock for the graph. But we have now obtained two different locks. If we are + // not extremely careful, we could get a deadlock. + processQueue(*new_transaction, lag_expiration_); + // Skip this optimization cycle if the transaction is empty because something failed while processing the pending + // transactions queue. + if (new_transaction->empty()) + { + continue; + } + // Prepare for selecting the marginal variables + preprocessMarginalization(*new_transaction); + // Combine the new transactions with any marginal transaction from the end of the last cycle + new_transaction->merge(marginal_transaction_); + + ROS_DEBUG_STREAM("New transaction for all optimization steps: " << *new_transaction); + + for (auto &&types : params_.optimization_steps) + { + ROS_DEBUG_STREAM("optimization_step"); + for (auto &&type : types) + { + ROS_DEBUG_STREAM("type: " << type); + } + + fuse_core::Transaction::SharedPtr new_transaction_for_loop = new_transaction->clone(); + + // Keep only the sensor models that will be optimized in this loop + std::unordered_set constraints_for_removal; + for (const auto& constraint : new_transaction_for_loop->addedConstraints()) + { + if (std::find(types.begin(), types.end(), constraint.source()) == types.end()) + { + constraints_for_removal.insert(constraint.uuid()); + } + } + + // remove all constraints marked for removal + for (auto &&i : constraints_for_removal) + { + new_transaction_for_loop->removeConstraint(i); + } + + // skip if there are no elements for optimization in this optimization step + if (boost::empty(new_transaction_for_loop->addedConstraints()) || new_transaction_for_loop->empty()) + { + ROS_DEBUG_STREAM("Skipping because no elements in transaction"); + continue; + } + + ROS_DEBUG_STREAM("New transaction within optimization step: " << *new_transaction_for_loop); + + // Update the graph + try + { + graph_->update(*new_transaction_for_loop); + } + catch (const std::exception& ex) + { + std::ostringstream oss; + oss << "Graph:\n"; + graph_->print(oss); + oss << "\nTransaction:\n"; + new_transaction_for_loop->print(oss); + + ROS_FATAL_STREAM("Failed to update graph with transaction: " << ex.what() + << "\nLeaving optimization loop and requesting " + "node shutdown...\n" << oss.str()); + ros::requestShutdown(); + break; + } + // 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_for_loop->stamp(); + notify(std::move(new_transaction_for_loop), graph_->clone()); + + // Abort if optimization failed. Not converging is not a failure because the solution found is usable. + if (!summary_.IsSolutionUsable()) + { + ROS_FATAL_STREAM("Optimization failed after updating the graph with the transaction with timestamp " + << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); + ROS_INFO_STREAM(summary_.FullReport()); + ros::requestShutdown(); + break; + } + } + + // Compute a transaction that marginalizes out those variables. + lag_expiration_ = computeLagExpirationTime(); + marginal_transaction_ = fuse_constraints::marginalizeVariables( + ros::this_node::getName(), + computeVariablesToMarginalize(lag_expiration_), + *graph_); + // Perform any post-marginal cleanup + postprocessMarginalization(marginal_transaction_); + // Note: The marginal transaction will not be applied until the next optimization iteration + // Log a warning if the optimization took too long + auto optimization_complete = ros::Time::now(); + if (optimization_complete > optimization_deadline) + { + ROS_WARN_STREAM_THROTTLE(10.0, "Optimization exceeded the configured duration by " + << (optimization_complete - optimization_deadline) << "s"); + } + } + } +} + +} // namespace fuse_optimizers diff --git a/fuse_optimizers/src/step_wise_fixed_lag_smoother_node.cpp b/fuse_optimizers/src/step_wise_fixed_lag_smoother_node.cpp new file mode 100644 index 000000000..4dbe52a9c --- /dev/null +++ b/fuse_optimizers/src/step_wise_fixed_lag_smoother_node.cpp @@ -0,0 +1,51 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, ARTI - Autonomous Robot Technology GmbH + * 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 + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "step_wise_fixed_lag_smoother_node"); + ros::NodeHandle private_node_handle("~"); + fuse_graphs::HashGraphParams hash_graph_params; + hash_graph_params.loadFromROS(private_node_handle); + fuse_optimizers::StepWiseFixedLagSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params)); + optimizer.startOptimization(); + ros::spin(); + + return 0; +} diff --git a/fuse_tutorials/config/range_sensor_stepwise_optimization_tutorial.yaml b/fuse_tutorials/config/range_sensor_stepwise_optimization_tutorial.yaml new file mode 100644 index 000000000..7da7fdfbb --- /dev/null +++ b/fuse_tutorials/config/range_sensor_stepwise_optimization_tutorial.yaml @@ -0,0 +1,72 @@ +# Step-wise Fixed-lag smoother configuration +optimization_frequency: 20 +transaction_timeout: 0.01 +lag_duration: 0.5 + +optimization_steps: + - + sources: + - "initial_localization_sensor" + - "unicycle_motion_model" + - + sources: + - "odometry_sensor" + - "imu_sensor" + - + sources: + - "range_sensor" + +motion_models: + unicycle_motion_model: + type: fuse_models::Unicycle2D + +unicycle_motion_model: + # x y yaw vx vy vyaw ax ay + process_noise_diagonal: [0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.1, 0.1] + +sensor_models: + initial_localization_sensor: + type: fuse_models::Unicycle2DIgnition + motion_models: [unicycle_motion_model] + ignition: true + odometry_sensor: + type: fuse_models::Odometry2D + motion_models: [unicycle_motion_model] + imu_sensor: + type: fuse_models::Imu2D + motion_models: [unicycle_motion_model] + range_sensor: + type: fuse_tutorials::RangeSensorModel + motion_models: [unicycle_motion_model] + +initial_localization_sensor: + publish_on_startup: false + +odometry_sensor: + topic: 'wheel_odom' + linear_velocity_dimensions: ['x', 'y'] + angular_velocity_dimensions: ['yaw'] + +imu_sensor: + topic: 'imu' + angular_velocity_dimensions: ['yaw'] + +range_sensor: + {} + +publishers: + filtered_publisher: + type: fuse_models::Odometry2DPublisher + beacon_publisher: + type: fuse_tutorials::BeaconPublisher + +filtered_publisher: + topic: 'odom_filtered' + base_link_frame_id: 'base_link' + odom_frame_id: 'odom' + map_frame_id: 'map' + world_frame_id: 'map' + publish_tf: false + +beacon_publisher: + map_frame_id: 'map' diff --git a/fuse_tutorials/launch/range_sensor_stepwise_optimization_tutorial.launch b/fuse_tutorials/launch/range_sensor_stepwise_optimization_tutorial.launch new file mode 100644 index 000000000..b566f2917 --- /dev/null +++ b/fuse_tutorials/launch/range_sensor_stepwise_optimization_tutorial.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + +