Skip to content

Fix deprecation warnings for upcoming ROS Galactic release #631

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 6 commits into from
Mar 22, 2021
Merged

Fix deprecation warnings for upcoming ROS Galactic release #631

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
6b36efd
Fix deprecation warnings when constructing rclcpp::Duration objects
jacobperron Mar 17, 2021
7055b82
Fix deprecated calls to declare_parameter
jacobperron Mar 17, 2021
ca9e807
Refactor
jacobperron Mar 17, 2021
afa37fa
Fix other deprecated uses of rclcpp::Duration
jacobperron Mar 17, 2021
774841b
Minor refactor
jacobperron Mar 18, 2021
3326e80
Catch exceptions when no parameter override is provided
jacobperron Mar 19, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 9 additions & 9 deletions src/filter_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand All @@ -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);

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
} */
}

Expand Down
2 changes: 1 addition & 1 deletion src/robot_localization_estimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
47 changes: 22 additions & 25 deletions src/ros_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand Down Expand Up @@ -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))) {
Copy link
Collaborator

@SteveMacenski SteveMacenski Mar 17, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ayrton04 should this be 1ns, 1s, ...?

Copy link
Contributor Author

@jacobperron jacobperron Mar 17, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks, I meant to ask the same question.

Copy link
Contributor Author

@jacobperron jacobperron Mar 18, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If I understand the equivalent ROS 1 code, it is also subtracting 1 nanosecond:

robot_localization/src/ros_filter.cpp

Line 611 in a2da33b

if (!revertTo(firstMeasurementTime - 1e-9)) // revertTo may invalidate firstMeasurement

Copy link
Collaborator

@ayrton04 ayrton04 Mar 22, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So the PRs involved here are

https://github.com/cra-ros-pkg/robot_localization/pull/266/files

and

https://github.com/cra-ros-pkg/robot_localization/pull/279/files

The only reason we subtract any time at all is to ensure that we go to a measurement (presumably the first) that is before first_measurement_time. But we could also just change this to use >=:

robot_localization/src/ros_filter.cpp

Lines 3322 to 3323 in 3326e80

while (!filter_state_history_.empty() &&
filter_state_history_.back()->last_measurement_time_ > time)

If I had to guess (I didn't dig too far), the measurement times were likely stored as doubles originally, and the 1e-9 thing was just me being careful with floating point numbers. But ROS times have exact representations, so I think the 1e-9 could go, and we can change the revertTo reverse iteration to check for >= instead of just >.

Copy link
Collaborator

@ayrton04 ayrton04 Mar 22, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

However, that change could be outside of scope for this. It was meant to be 1 nanosecond, yes.

RF_DEBUG(
"ERROR: history interval is too small to revert to time " <<
filter_utilities::toSec(first_measurement_time) << "\n");
Expand Down Expand Up @@ -815,7 +815,7 @@ void RosFilter<T>::loadParams()
// Try to resolve tf_prefix
std::string tf_prefix = "";
std::string tf_prefix_path = "";
this->declare_parameter("tf_prefix");
this->declare_parameter<std::string>("tf_prefix");
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_);
Expand All @@ -833,19 +833,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);
Expand All @@ -865,8 +863,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);
Expand All @@ -883,7 +880,7 @@ void RosFilter<T>::loadParams()
control_timeout = this->declare_parameter("control_timeout", 0.0);

if (use_control_) {
this->declare_parameter("control_config");
this->declare_parameter<std::vector<bool>>("control_config");
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 <<
Expand All @@ -899,7 +896,7 @@ void RosFilter<T>::loadParams()
use_control_ = false;
}

this->declare_parameter("acceleration_limits");
this->declare_parameter<std::vector<double>>("acceleration_limits");
if (this->get_parameter("acceleration_limits", acceleration_limits)) {
if (acceleration_limits.size() != TWIST_SIZE) {
std::cerr << "Acceleration configuration must be of size " << TWIST_SIZE <<
Expand All @@ -915,7 +912,7 @@ void RosFilter<T>::loadParams()
acceleration_limits.resize(TWIST_SIZE, 1.0);
}

this->declare_parameter("acceleration_gains");
this->declare_parameter<std::vector<double>>("acceleration_gains");
if (this->get_parameter("acceleration_gains", acceleration_gains)) {
const int size = acceleration_gains.size();
if (size != TWIST_SIZE) {
Expand All @@ -929,7 +926,7 @@ void RosFilter<T>::loadParams()
}
}

this->declare_parameter("deceleration_limits");
this->declare_parameter<std::vector<double>>("deceleration_limits");
if (this->get_parameter("deceleration_limits", deceleration_limits)) {
if (deceleration_limits.size() != TWIST_SIZE) {
std::cerr << "Deceleration configuration must be of size " << TWIST_SIZE <<
Expand All @@ -945,7 +942,7 @@ void RosFilter<T>::loadParams()
deceleration_limits = acceleration_limits;
}

this->declare_parameter("deceleration_gains");
this->declare_parameter<std::vector<double>>("deceleration_gains");
if (this->get_parameter("deceleration_gains", deceleration_gains)) {
const int size = deceleration_gains.size();
if (size != TWIST_SIZE) {
Expand Down Expand Up @@ -977,7 +974,7 @@ void RosFilter<T>::loadParams()
dynamic_process_noise_covariance);

std::vector<double> initial_state;
this->declare_parameter("initial_state");
this->declare_parameter<std::vector<double>>("initial_state");
if (this->get_parameter("initial_state", initial_state)) {
if (initial_state.size() != STATE_SIZE) {
std::cerr << "Initial state must be of size " << STATE_SIZE <<
Expand Down Expand Up @@ -1066,7 +1063,7 @@ 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);
this->declare_parameter<std::string>(odom_topic_name);

rclcpp::Parameter parameter;
if (rclcpp::PARAMETER_NOT_SET != this->get_parameter(odom_topic_name, parameter)) {
Expand Down Expand Up @@ -1217,7 +1214,7 @@ 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);
this->declare_parameter<std::string>(pose_topic_name);

rclcpp::Parameter parameter;
if (rclcpp::PARAMETER_NOT_SET != this->get_parameter(pose_topic_name, parameter)) {
Expand Down Expand Up @@ -1335,7 +1332,7 @@ 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);
this->declare_parameter<std::string>(twist_topic_name);

rclcpp::Parameter parameter;
if (rclcpp::PARAMETER_NOT_SET != this->get_parameter(twist_topic_name, parameter)) {
Expand Down Expand Up @@ -1417,7 +1414,7 @@ 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);
this->declare_parameter<std::string>(imu_topic_name);

rclcpp::Parameter parameter;
if (rclcpp::PARAMETER_NOT_SET != this->get_parameter(imu_topic_name, parameter)) {
Expand Down Expand Up @@ -1663,7 +1660,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);

Expand Down Expand Up @@ -1727,7 +1724,7 @@ void RosFilter<T>::loadParams()
process_noise_covariance.setZero();
std::vector<double> process_noise_covar_flat;

this->declare_parameter("process_noise_covariance");
this->declare_parameter<std::vector<double>>("process_noise_covariance");
if (this->get_parameter(
"process_noise_covariance",
process_noise_covar_flat))
Expand All @@ -1754,7 +1751,7 @@ void RosFilter<T>::loadParams()
initial_estimate_error_covariance.setZero();
std::vector<double> estimate_error_covar_flat;

this->declare_parameter("initial_estimate_covariance");
this->declare_parameter<std::vector<double>>("initial_estimate_covariance");
if (this->get_parameter(
"initial_estimate_covariance",
estimate_error_covar_flat))
Expand Down
2 changes: 1 addition & 1 deletion src/ros_filter_utilities.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
4 changes: 2 additions & 2 deletions test/test_filter_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down