Fix/galactic/load parameters (#692)

* Fix runtime exception thrown when parameter override not provided (#675)

The templated declare_parameter method needs an override (or default value) since
it returns the actual parameter value, otherwise we get an exception.
To correctly declare a parameter without a default value, we should use a different
signature.

Related upstream issue: ros2/rclcpp#1691

Signed-off-by: Jacob Perron <[email protected]>

* Fix different time sources error (#679)

Signed-off-by: Christophe Bedard <[email protected]>

Co-authored-by: Jacob Perron <[email protected]>
Co-authored-by: Christophe Bedard <[email protected]>

Author: 3 people

src/filter_base.cpp

@@ -47,7 +47,7 @@ namespace robot_localization
 FilterBase::FilterBase()
 : initialized_(false), use_control_(false),
   use_dynamic_process_noise_covariance_(false), control_timeout_(0, 0u),
-  last_measurement_time_(0, 0, RCL_ROS_TIME), latest_control_time_(0),
+  last_measurement_time_(0, 0, RCL_ROS_TIME), latest_control_time_(0, 0, RCL_ROS_TIME),
   sensor_timeout_(0, 0u), debug_stream_(nullptr),
   acceleration_gains_(TWIST_SIZE, 0.0),
   acceleration_limits_(TWIST_SIZE, 0.0),

src/ros_filter.cpp

@@ -819,7 +819,7 @@ void RosFilter<T>::loadParams()
   // Try to resolve tf_prefix
   std::string tf_prefix = "";
   std::string tf_prefix_path = "";
-  this->declare_parameter<std::string>("tf_prefix");
+  this->declare_parameter("tf_prefix", rclcpp::PARAMETER_STRING);
   if (this->get_parameter("tf_prefix", tf_prefix_path)) {
     // Append the tf prefix in a tf2-friendly manner
     filter_utilities::appendPrefix(tf_prefix, map_frame_id_);
@@ -887,7 +887,7 @@ void RosFilter<T>::loadParams()
   control_timeout = this->declare_parameter("control_timeout", 0.0);
 
   if (use_control_) {
-    this->declare_parameter<std::vector<bool>>("control_config");
+    this->declare_parameter("control_config", rclcpp::PARAMETER_BOOL_ARRAY);
     if (this->get_parameter("control_config", control_update_vector)) {
       if (control_update_vector.size() != TWIST_SIZE) {
         std::cerr << "Control configuration must be of size " << TWIST_SIZE <<
@@ -903,7 +903,7 @@ void RosFilter<T>::loadParams()
       use_control_ = false;
     }
 
-    this->declare_parameter<std::vector<double>>("acceleration_limits");
+    this->declare_parameter("acceleration_limits", rclcpp::PARAMETER_DOUBLE_ARRAY);
     if (this->get_parameter("acceleration_limits", acceleration_limits)) {
       if (acceleration_limits.size() != TWIST_SIZE) {
         std::cerr << "Acceleration configuration must be of size " << TWIST_SIZE <<
@@ -919,7 +919,7 @@ void RosFilter<T>::loadParams()
       acceleration_limits.resize(TWIST_SIZE, 1.0);
     }
 
-    this->declare_parameter<std::vector<double>>("acceleration_gains");
+    this->declare_parameter("acceleration_gains", rclcpp::PARAMETER_DOUBLE_ARRAY);
     if (this->get_parameter("acceleration_gains", acceleration_gains)) {
       const int size = acceleration_gains.size();
       if (size != TWIST_SIZE) {
@@ -933,7 +933,7 @@ void RosFilter<T>::loadParams()
       }
     }
 
-    this->declare_parameter<std::vector<double>>("deceleration_limits");
+    this->declare_parameter("deceleration_limits", rclcpp::PARAMETER_DOUBLE_ARRAY);
     if (this->get_parameter("deceleration_limits", deceleration_limits)) {
       if (deceleration_limits.size() != TWIST_SIZE) {
         std::cerr << "Deceleration configuration must be of size " << TWIST_SIZE <<
@@ -949,7 +949,7 @@ void RosFilter<T>::loadParams()
       deceleration_limits = acceleration_limits;
     }
 
-    this->declare_parameter<std::vector<double>>("deceleration_gains");
+    this->declare_parameter("deceleration_gains", rclcpp::PARAMETER_DOUBLE_ARRAY);
     if (this->get_parameter("deceleration_gains", deceleration_gains)) {
       const int size = deceleration_gains.size();
       if (size != TWIST_SIZE) {
@@ -981,7 +981,7 @@ void RosFilter<T>::loadParams()
     dynamic_process_noise_covariance);
 
   std::vector<double> initial_state;
-  this->declare_parameter<std::vector<double>>("initial_state");
+  this->declare_parameter("initial_state", rclcpp::PARAMETER_DOUBLE_ARRAY);
   if (this->get_parameter("initial_state", initial_state)) {
     if (initial_state.size() != STATE_SIZE) {
       std::cerr << "Initial state must be of size " << STATE_SIZE <<
@@ -1070,7 +1070,7 @@ void RosFilter<T>::loadParams()
     ss << "odom" << topic_ind++;
     std::string odom_topic_name = ss.str();
     std::string odom_topic;
-    this->declare_parameter<std::string>(odom_topic_name);
+    this->declare_parameter(odom_topic_name, rclcpp::PARAMETER_STRING);
 
     rclcpp::Parameter parameter;
     if (this->get_parameter(odom_topic_name, parameter)) {
@@ -1220,7 +1220,7 @@ void RosFilter<T>::loadParams()
     ss << "pose" << topic_ind++;
     std::string pose_topic_name = ss.str();
     std::string pose_topic;
-    this->declare_parameter<std::string>(pose_topic_name);
+    this->declare_parameter(pose_topic_name, rclcpp::PARAMETER_STRING);
 
     rclcpp::Parameter parameter;
     if (this->get_parameter(pose_topic_name, parameter)) {
@@ -1337,7 +1337,7 @@ void RosFilter<T>::loadParams()
     ss << "twist" << topic_ind++;
     std::string twist_topic_name = ss.str();
     std::string twist_topic;
-    this->declare_parameter<std::string>(twist_topic_name);
+    this->declare_parameter(twist_topic_name, rclcpp::PARAMETER_STRING);
 
     rclcpp::Parameter parameter;
     if (this->get_parameter(twist_topic_name, parameter)) {
@@ -1418,7 +1418,7 @@ void RosFilter<T>::loadParams()
     ss << "imu" << topic_ind++;
     std::string imu_topic_name = ss.str();
     std::string imu_topic;
-    this->declare_parameter<std::string>(imu_topic_name);
+    this->declare_parameter(imu_topic_name, rclcpp::PARAMETER_STRING);
 
     rclcpp::Parameter parameter;
     if (this->get_parameter(imu_topic_name, parameter)) {
@@ -1727,7 +1727,7 @@ void RosFilter<T>::loadParams()
   process_noise_covariance.setZero();
   std::vector<double> process_noise_covar_flat;
 
-  this->declare_parameter<std::vector<double>>("process_noise_covariance");
+  this->declare_parameter("process_noise_covariance", rclcpp::PARAMETER_DOUBLE_ARRAY);
   if (this->get_parameter(
       "process_noise_covariance",
       process_noise_covar_flat))
@@ -1754,7 +1754,7 @@ void RosFilter<T>::loadParams()
   initial_estimate_error_covariance.setZero();
   std::vector<double> estimate_error_covar_flat;
 
-  this->declare_parameter<std::vector<double>>("initial_estimate_covariance");
+  this->declare_parameter("initial_estimate_covariance", rclcpp::PARAMETER_DOUBLE_ARRAY);
   if (this->get_parameter(
       "initial_estimate_covariance",
       estimate_error_covar_flat))