Fix deprecation warnings for upcoming ROS Galactic release (#631)

* Fix deprecation warnings when constructing rclcpp::Duration objects

If we are constructing from nanoseconds we must be explicit about the units.

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

* Fix deprecated calls to declare_parameter

If a default value is not provided, then we must specify the parameter type.

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

* Refactor

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

* Fix other deprecated uses of rclcpp::Duration

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

* Minor refactor

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

* Catch exceptions when no parameter override is provided

When there is a NoParameterOverrideProvided exception, the parameter
is not declared so we don't need to undeclare it.

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

Author: jacobperron

src/filter_base.cpp

@@ -46,9 +46,9 @@ namespace robot_localization
 {
 FilterBase::FilterBase()
 : initialized_(false), use_control_(false),
-  use_dynamic_process_noise_covariance_(false), control_timeout_(0),
+  use_dynamic_process_noise_covariance_(false), control_timeout_(0, 0u),
   last_measurement_time_(0, 0, RCL_ROS_TIME), latest_control_time_(0),
-  sensor_timeout_(0), debug_stream_(nullptr),
+  sensor_timeout_(0, 0u), debug_stream_(nullptr),
   acceleration_gains_(TWIST_SIZE, 0.0),
   acceleration_limits_(TWIST_SIZE, 0.0),
   deceleration_gains_(TWIST_SIZE, 0.0),
@@ -99,7 +99,7 @@ void FilterBase::reset()
   covariance_epsilon_ *= 0.001;
 
   // Assume 30Hz from sensor data (configurable)
-  sensor_timeout_ = rclcpp::Duration(0.033333333);
+  sensor_timeout_ = rclcpp::Duration::from_seconds(0.033333333);
 
   // Initialize our last update and measurement times
   last_measurement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
@@ -197,7 +197,7 @@ void FilterBase::processMeasurement(const Measurement & measurement)
     "------ FilterBase::processMeasurement (" << measurement.topic_name_ <<
       ") ------\n");
 
-  rclcpp::Duration delta(0);
+  rclcpp::Duration delta(0, 0u);
 
   // If we've had a previous reading, then go through the predict/update
   // cycle. Otherwise, set our state and covariance to whatever we get
@@ -215,7 +215,7 @@ void FilterBase::processMeasurement(const Measurement & measurement)
 
     // Only want to carry out a prediction if it's
     // forward in time. Otherwise, just correct.
-    if (delta > rclcpp::Duration(0)) {
+    if (delta > rclcpp::Duration(0, 0u)) {
       validateDelta(delta);
       predict(measurement.time_, delta);
 
@@ -247,7 +247,7 @@ void FilterBase::processMeasurement(const Measurement & measurement)
     initialized_ = true;
   }
 
-  if (delta >= rclcpp::Duration(0)) {
+  if (delta >= rclcpp::Duration(0, 0u)) {
     // Update the last measurement and update time.
     // The measurement time is based on the time stamp of the
     // measurement, whereas the update time is based on this
@@ -337,15 +337,15 @@ void FilterBase::setState(const Eigen::VectorXd & state) {state_ = state;}
 void FilterBase::validateDelta(rclcpp::Duration & /*delta*/)
 {
   // TODO(someone): Need to verify this condition B'Coz
-  // rclcpp::Duration(100000.0) value is 0.00010000000000000000479
+  // rclcpp::Duration::from_seconds(100000.0) value is 0.00010000000000000000479
   // This handles issues with ROS time when use_sim_time is on and we're playing
   // from bags.
-  /* if (delta > rclcpp::Duration(100000.0))
+  /* if (delta > rclcpp::Duration::from_seconds(100000.0))
   {
     FB_DEBUG("Delta was very large. Suspect playing from bag file. Setting to
   0.01\n");
 
-    delta = rclcpp::Duration(0.01);
+    delta = rclcpp::Duration::from_seconds(0.01);
   } */
 }
 

src/robot_localization_estimator.cpp

@@ -182,7 +182,7 @@ void RobotLocalizationEstimator::extrapolate(
   rclcpp::Time time_stamp = rclcpp::Time(
     boundary_state.time_stamp *
     1000000000);
-  rclcpp::Duration delta_duration = rclcpp::Duration(delta * 1000000000);
+  rclcpp::Duration delta_duration = rclcpp::Duration::from_seconds(delta);
   filter_->predict(time_stamp, delta_duration);
 
   state_at_req_time.time_stamp = requested_time;

src/ros_filter.cpp

@@ -72,12 +72,12 @@ RosFilter<T>::RosFilter(const rclcpp::NodeOptions & options)
   static_diag_error_level_(diagnostic_msgs::msg::DiagnosticStatus::OK),
   frequency_(30.0),
   gravitational_acceleration_(9.80665),
-  history_length_(0),
+  history_length_(0ns),
   latest_control_(),
   last_set_pose_time_(0, 0, RCL_ROS_TIME),
   latest_control_time_(0, 0, RCL_ROS_TIME),
-  tf_timeout_(0),
-  tf_time_offset_(0)
+  tf_timeout_(0ns),
+  tf_time_offset_(0ns)
 {
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
   tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
@@ -604,7 +604,7 @@ void RosFilter<T>::integrateMeasurements(const rclcpp::Time & current_time)
       const std::string first_measurement_topic =
         first_measurement->topic_name_;
       // revertTo may invalidate first_measurement
-      if (!revertTo(first_measurement_time - rclcpp::Duration(1))) {
+      if (!revertTo(first_measurement_time - rclcpp::Duration(1ns))) {
         RF_DEBUG(
           "ERROR: history interval is too small to revert to time " <<
             filter_utilities::toSec(first_measurement_time) << "\n");
@@ -815,7 +815,10 @@ void RosFilter<T>::loadParams()
   // Try to resolve tf_prefix
   std::string tf_prefix = "";
   std::string tf_prefix_path = "";
-  this->declare_parameter("tf_prefix");
+  try {
+    this->declare_parameter<std::string>("tf_prefix");
+  } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+  }
   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_);
@@ -833,19 +836,17 @@ void RosFilter<T>::loadParams()
 
   // Transform future dating
   double offset_tmp = this->declare_parameter("transform_time_offset", 0.0);
-  tf_time_offset_ =
-    rclcpp::Duration(filter_utilities::secToNanosec(offset_tmp));
+  tf_time_offset_ = rclcpp::Duration::from_seconds(offset_tmp);
 
   // Transform timeout
   double timeout_tmp = this->declare_parameter("transform_timeout", 0.0);
-  tf_timeout_ = rclcpp::Duration(filter_utilities::secToNanosec(timeout_tmp));
+  tf_timeout_ = rclcpp::Duration::from_seconds(timeout_tmp);
 
   // Update frequency and sensor timeout
   frequency_ = this->declare_parameter("frequency", 30.0);
 
   double sensor_timeout = this->declare_parameter("sensor_timeout", 1.0 / frequency_);
-  filter_.setSensorTimeout(
-    rclcpp::Duration(filter_utilities::secToNanosec(sensor_timeout)));
+  filter_.setSensorTimeout(rclcpp::Duration::from_seconds(sensor_timeout));
 
   // Determine if we're in 2D mode
   two_d_mode_ = this->declare_parameter("two_d_mode", false);
@@ -865,8 +866,7 @@ void RosFilter<T>::loadParams()
       " specified. Absolute value will be assumed.";
   }
 
-  history_length_ = rclcpp::Duration(
-    filter_utilities::secToNanosec(std::abs(history_length_double)));
+  history_length_ = rclcpp::Duration::from_seconds(std::abs(history_length_double));
 
   // Whether we reset filter on jump back in time
   reset_on_time_jump_ = this->declare_parameter("reset_on_time_jump", false);
@@ -883,7 +883,10 @@ void RosFilter<T>::loadParams()
   control_timeout = this->declare_parameter("control_timeout", 0.0);
 
   if (use_control_) {
-    this->declare_parameter("control_config");
+    try {
+      this->declare_parameter<std::vector<bool>>("control_config");
+    } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+    }
     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 <<
@@ -899,7 +902,10 @@ void RosFilter<T>::loadParams()
       use_control_ = false;
     }
 
-    this->declare_parameter("acceleration_limits");
+    try {
+      this->declare_parameter<std::vector<double>>("acceleration_limits");
+    } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+    }
     if (this->get_parameter("acceleration_limits", acceleration_limits)) {
       if (acceleration_limits.size() != TWIST_SIZE) {
         std::cerr << "Acceleration configuration must be of size " << TWIST_SIZE <<
@@ -915,7 +921,10 @@ void RosFilter<T>::loadParams()
       acceleration_limits.resize(TWIST_SIZE, 1.0);
     }
 
-    this->declare_parameter("acceleration_gains");
+    try {
+      this->declare_parameter<std::vector<double>>("acceleration_gains");
+    } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+    }
     if (this->get_parameter("acceleration_gains", acceleration_gains)) {
       const int size = acceleration_gains.size();
       if (size != TWIST_SIZE) {
@@ -929,7 +938,10 @@ void RosFilter<T>::loadParams()
       }
     }
 
-    this->declare_parameter("deceleration_limits");
+    try {
+      this->declare_parameter<std::vector<double>>("deceleration_limits");
+    } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+    }
     if (this->get_parameter("deceleration_limits", deceleration_limits)) {
       if (deceleration_limits.size() != TWIST_SIZE) {
         std::cerr << "Deceleration configuration must be of size " << TWIST_SIZE <<
@@ -945,7 +957,10 @@ void RosFilter<T>::loadParams()
       deceleration_limits = acceleration_limits;
     }
 
-    this->declare_parameter("deceleration_gains");
+    try {
+      this->declare_parameter<std::vector<double>>("deceleration_gains");
+    } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+    }
     if (this->get_parameter("deceleration_gains", deceleration_gains)) {
       const int size = deceleration_gains.size();
       if (size != TWIST_SIZE) {
@@ -977,7 +992,10 @@ void RosFilter<T>::loadParams()
     dynamic_process_noise_covariance);
 
   std::vector<double> initial_state;
-  this->declare_parameter("initial_state");
+  try {
+    this->declare_parameter<std::vector<double>>("initial_state");
+  } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+  }
   if (this->get_parameter("initial_state", initial_state)) {
     if (initial_state.size() != STATE_SIZE) {
       std::cerr << "Initial state must be of size " << STATE_SIZE <<
@@ -1066,15 +1084,17 @@ void RosFilter<T>::loadParams()
     ss << "odom" << topic_ind++;
     std::string odom_topic_name = ss.str();
     std::string odom_topic;
-    this->declare_parameter(odom_topic_name);
+    try {
+      this->declare_parameter<std::string>(odom_topic_name);
+    } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+    }
 
     rclcpp::Parameter parameter;
     if (this->get_parameter(odom_topic_name, parameter)) {
       more_params = true;
       odom_topic = parameter.as_string();
     } else {
       more_params = false;
-      this->undeclare_parameter(odom_topic_name);
     }
 
     if (more_params) {
@@ -1217,15 +1237,17 @@ void RosFilter<T>::loadParams()
     ss << "pose" << topic_ind++;
     std::string pose_topic_name = ss.str();
     std::string pose_topic;
-    this->declare_parameter(pose_topic_name);
+    try {
+      this->declare_parameter<std::string>(pose_topic_name);
+    } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+    }
 
     rclcpp::Parameter parameter;
     if (this->get_parameter(pose_topic_name, parameter)) {
       more_params = true;
       pose_topic = parameter.as_string();
     } else {
       more_params = false;
-      this->undeclare_parameter(pose_topic_name);
     }
 
     if (more_params) {
@@ -1335,15 +1357,17 @@ void RosFilter<T>::loadParams()
     ss << "twist" << topic_ind++;
     std::string twist_topic_name = ss.str();
     std::string twist_topic;
-    this->declare_parameter(twist_topic_name);
+    try {
+      this->declare_parameter<std::string>(twist_topic_name);
+    } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+    }
 
     rclcpp::Parameter parameter;
     if (this->get_parameter(twist_topic_name, parameter)) {
       more_params = true;
       twist_topic = parameter.as_string();
     } else {
       more_params = false;
-      this->undeclare_parameter(twist_topic_name);
     }
 
     if (more_params) {
@@ -1417,15 +1441,17 @@ void RosFilter<T>::loadParams()
     ss << "imu" << topic_ind++;
     std::string imu_topic_name = ss.str();
     std::string imu_topic;
-    this->declare_parameter(imu_topic_name);
+    try {
+      this->declare_parameter<std::string>(imu_topic_name);
+    } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+    }
 
     rclcpp::Parameter parameter;
     if (this->get_parameter(imu_topic_name, parameter)) {
       more_params = true;
       imu_topic = parameter.as_string();
     } else {
       more_params = false;
-      this->undeclare_parameter(imu_topic_name);
     }
 
     if (more_params) {
@@ -1663,7 +1689,7 @@ void RosFilter<T>::loadParams()
 
     filter_.setControlParams(
       control_update_vector,
-      rclcpp::Duration(filter_utilities::secToNanosec(control_timeout)),
+      rclcpp::Duration::from_seconds(control_timeout),
       acceleration_limits, acceleration_gains, deceleration_limits,
       deceleration_gains);
 
@@ -1727,7 +1753,10 @@ void RosFilter<T>::loadParams()
   process_noise_covariance.setZero();
   std::vector<double> process_noise_covar_flat;
 
-  this->declare_parameter("process_noise_covariance");
+  try {
+    this->declare_parameter<std::vector<double>>("process_noise_covariance");
+  } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+  }
   if (this->get_parameter(
       "process_noise_covariance",
       process_noise_covar_flat))
@@ -1754,7 +1783,10 @@ void RosFilter<T>::loadParams()
   initial_estimate_error_covariance.setZero();
   std::vector<double> estimate_error_covar_flat;
 
-  this->declare_parameter("initial_estimate_covariance");
+  try {
+    this->declare_parameter<std::vector<double>>("initial_estimate_covariance");
+  } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
+  }
   if (this->get_parameter(
       "initial_estimate_covariance",
       estimate_error_covar_flat))

src/ros_filter_utilities.cpp

@@ -193,7 +193,7 @@ bool lookupTransformSafe(
 {
   return lookupTransformSafe(
     buffer, target_frame, source_frame, time,
-    rclcpp::Duration(0), target_frame_trans, silent);
+    rclcpp::Duration(0, 0u), target_frame_trans, silent);
 }
 
 void quatToRPY(

test/test_filter_base.cpp

@@ -138,8 +138,8 @@ TEST(FilterBaseTest, DerivedFilterGetSet) {
 
   // Simple get/set checks
   double timeout = 7.4;
-  derived.setSensorTimeout(rclcpp::Duration(timeout));
-  EXPECT_EQ(derived.getSensorTimeout(), rclcpp::Duration(timeout));
+  derived.setSensorTimeout(rclcpp::Duration::from_seconds(timeout));
+  EXPECT_EQ(derived.getSensorTimeout(), rclcpp::Duration::from_seconds(timeout));
 
   double lastMeasTime = 3.83;
   derived.setLastMeasurementTime(rclcpp::Time(lastMeasTime));