diff --git a/src/filter_base.cpp b/src/filter_base.cpp index c2b1d6770..5b75d0cf6 100644 --- a/src/filter_base.cpp +++ b/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); } */ } diff --git a/src/robot_localization_estimator.cpp b/src/robot_localization_estimator.cpp index 4f183ca3f..adb547e91 100644 --- a/src/robot_localization_estimator.cpp +++ b/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; diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 5e254eaf9..42e0d074f 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -72,12 +72,12 @@ RosFilter::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(this->get_clock()); tf_listener_ = std::make_unique(*tf_buffer_); @@ -604,7 +604,7 @@ void RosFilter::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::loadParams() // Try to resolve tf_prefix std::string tf_prefix = ""; std::string tf_prefix_path = ""; - this->declare_parameter("tf_prefix"); + try { + this->declare_parameter("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::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::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::loadParams() control_timeout = this->declare_parameter("control_timeout", 0.0); if (use_control_) { - this->declare_parameter("control_config"); + try { + this->declare_parameter>("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::loadParams() use_control_ = false; } - this->declare_parameter("acceleration_limits"); + try { + this->declare_parameter>("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::loadParams() acceleration_limits.resize(TWIST_SIZE, 1.0); } - this->declare_parameter("acceleration_gains"); + try { + this->declare_parameter>("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::loadParams() } } - this->declare_parameter("deceleration_limits"); + try { + this->declare_parameter>("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::loadParams() deceleration_limits = acceleration_limits; } - this->declare_parameter("deceleration_gains"); + try { + this->declare_parameter>("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::loadParams() dynamic_process_noise_covariance); std::vector initial_state; - this->declare_parameter("initial_state"); + try { + this->declare_parameter>("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,7 +1084,10 @@ void RosFilter::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(odom_topic_name); + } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { + } rclcpp::Parameter parameter; if (rclcpp::PARAMETER_NOT_SET != this->get_parameter(odom_topic_name, parameter)) { @@ -1074,7 +1095,6 @@ void RosFilter::loadParams() odom_topic = parameter.as_string(); } else { more_params = false; - this->undeclare_parameter(odom_topic_name); } if (more_params) { @@ -1217,7 +1237,10 @@ void RosFilter::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(pose_topic_name); + } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { + } rclcpp::Parameter parameter; if (rclcpp::PARAMETER_NOT_SET != this->get_parameter(pose_topic_name, parameter)) { @@ -1225,7 +1248,6 @@ void RosFilter::loadParams() pose_topic = parameter.as_string(); } else { more_params = false; - this->undeclare_parameter(pose_topic_name); } if (more_params) { @@ -1335,7 +1357,10 @@ void RosFilter::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(twist_topic_name); + } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { + } rclcpp::Parameter parameter; if (rclcpp::PARAMETER_NOT_SET != this->get_parameter(twist_topic_name, parameter)) { @@ -1343,7 +1368,6 @@ void RosFilter::loadParams() twist_topic = parameter.as_string(); } else { more_params = false; - this->undeclare_parameter(twist_topic_name); } if (more_params) { @@ -1417,7 +1441,10 @@ void RosFilter::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(imu_topic_name); + } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { + } rclcpp::Parameter parameter; if (rclcpp::PARAMETER_NOT_SET != this->get_parameter(imu_topic_name, parameter)) { @@ -1425,7 +1452,6 @@ void RosFilter::loadParams() imu_topic = parameter.as_string(); } else { more_params = false; - this->undeclare_parameter(imu_topic_name); } if (more_params) { @@ -1663,7 +1689,7 @@ void RosFilter::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::loadParams() process_noise_covariance.setZero(); std::vector process_noise_covar_flat; - this->declare_parameter("process_noise_covariance"); + try { + this->declare_parameter>("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::loadParams() initial_estimate_error_covariance.setZero(); std::vector estimate_error_covar_flat; - this->declare_parameter("initial_estimate_covariance"); + try { + this->declare_parameter>("initial_estimate_covariance"); + } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { + } if (this->get_parameter( "initial_estimate_covariance", estimate_error_covar_flat)) diff --git a/src/ros_filter_utilities.cpp b/src/ros_filter_utilities.cpp index a9e645eb9..2530ce545 100644 --- a/src/ros_filter_utilities.cpp +++ b/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( diff --git a/test/test_filter_base.cpp b/test/test_filter_base.cpp index bd29eddf1..0c0a889fa 100644 --- a/test/test_filter_base.cpp +++ b/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));