Adding Ceres options

Author: ayrton04

fuse_core/CMakeLists.txt

@@ -43,7 +43,7 @@ add_library(
 #    src/async_motion_model.cpp
 #    src/async_publisher.cpp
 #    src/async_sensor_model.cpp
-#    src/ceres_options.cpp
+    src/ceres_options.cpp
     src/constraint.cpp
     src/graph.cpp
     src/graph_deserializer.cpp

fuse_core/include/fuse_core/ceres_options.h

@@ -36,8 +36,8 @@
 
 #include <fuse_core/ceres_macros.h>
 
-#include <ros/console.h>
-#include <ros/node_handle.h>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/node.hpp>
 
 #include <ceres/version.h>
 #include <ceres/covariance.h>
@@ -185,23 +185,23 @@ CERES_OPTION_STRING_DEFINITIONS(VisibilityClusteringType)
 /**
  * @brief Helper function that loads a Ceres Option (e.g. ceres::LinearSolverType) value from the parameter server
  *
- * @param[in] node_handle - The node handle used to load the parameter
+ * @param[in] node - The node handle used to load the parameter
  * @param[in] parameter_name - The parameter name to load
  * @param[in] default_value - A default value to use if the provided parameter name does not exist
  * @return The loaded (or default) value
  */
 template <class T>
-T getParam(const ros::NodeHandle& node_handle, const std::string& parameter_name, const T& default_value)
+T getParam(rclcpp::Node& node, const std::string& parameter_name, const T& default_value)
 {
   const std::string default_string_value{ ToString(default_value) };
 
   std::string string_value;
-  node_handle.param(parameter_name, string_value, default_string_value);
+  string_value = node.declare_parameter(parameter_name, default_string_value);
 
   T value;
   if (!FromString(string_value, &value))
   {
-    ROS_WARN_STREAM("The requested " << parameter_name << " (" << string_value
+    RCLCPP_WARN_STREAM(node.get_logger(), "The requested " << parameter_name << " (" << string_value
                                      << ") is not supported. Using the default value (" << default_string_value
                                      << ") instead.");
     value = default_value;
@@ -216,23 +216,23 @@ T getParam(const ros::NodeHandle& node_handle, const std::string& parameter_name
  * @param[in] nh - A node handle in a namespace containing ceres::Covariance::Options settings
  * @param[out] covariance_options - The ceres::Covariance::Options object to update
  */
-void loadCovarianceOptionsFromROS(const ros::NodeHandle& nh, ceres::Covariance::Options& covariance_options);
+void loadCovarianceOptionsFromROS(rclcpp::Node& nh, ceres::Covariance::Options& covariance_options);
 
 /**
  * @brief Populate a ceres::Problem::Options object with information from the parameter server
  *
  * @param[in] nh - A node handle in a namespace containing ceres::Problem::Options settings
  * @param[out] problem_options - The ceres::Problem::Options object to update
  */
-void loadProblemOptionsFromROS(const ros::NodeHandle& nh, ceres::Problem::Options& problem_options);
+void loadProblemOptionsFromROS(rclcpp::Node& nh, ceres::Problem::Options& problem_options);
 
 /**
  * @brief Populate a ceres::Solver::Options object with information from the parameter server
  *
  * @param[in] nh - A node handle in a namespace containing ceres::Solver::Options settings
  * @param[out] solver_options - The ceres::Solver::Options object to update
  */
-void loadSolverOptionsFromROS(const ros::NodeHandle& nh, ceres::Solver::Options& solver_options);
+void loadSolverOptionsFromROS(rclcpp::Node& nh, ceres::Solver::Options& solver_options);
 
 }  // namespace fuse_core