Skip to content

Commit

Permalink
Catch exceptions when no parameter override is provided
Browse files Browse the repository at this point in the history 
When there is a NoParameterOverrideProvided exception, the parameter
is not declared so we don't need to undeclare it.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>
  • Loading branch information
@jacobperron
jacobperron committed Mar 19, 2021
1 parent 774841b commit 3326e80
Showing 1 changed file with 52 additions and 17 deletions.
69 changes: 52 additions & 17 deletions src/ros_filter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string>("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_);
Expand Down Expand Up @@ -880,7 +883,10 @@ void RosFilter<T>::loadParams()
control_timeout = this->declare_parameter("control_timeout", 0.0);

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

this->declare_parameter<std::vector<double>>("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 <<
Expand All @@ -912,7 +921,10 @@ void RosFilter<T>::loadParams()
acceleration_limits.resize(TWIST_SIZE, 1.0);
}

this->declare_parameter<std::vector<double>>("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) {
Expand All @@ -926,7 +938,10 @@ void RosFilter<T>::loadParams()
}
}

this->declare_parameter<std::vector<double>>("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 <<
Expand All @@ -942,7 +957,10 @@ void RosFilter<T>::loadParams()
deceleration_limits = acceleration_limits;
}

this->declare_parameter<std::vector<double>>("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) {
Expand Down Expand Up @@ -974,7 +992,10 @@ void RosFilter<T>::loadParams()
dynamic_process_noise_covariance);

std::vector<double> initial_state;
this->declare_parameter<std::vector<double>>("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 <<
Expand Down Expand Up @@ -1063,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<std::string>(odom_topic_name);
try {
this->declare_parameter<std::string>(odom_topic_name);
} catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
}

rclcpp::Parameter parameter;
if (rclcpp::PARAMETER_NOT_SET != 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) {
Expand Down Expand Up @@ -1214,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<std::string>(pose_topic_name);
try {
this->declare_parameter<std::string>(pose_topic_name);
} catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
}

rclcpp::Parameter parameter;
if (rclcpp::PARAMETER_NOT_SET != 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) {
Expand Down Expand Up @@ -1332,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<std::string>(twist_topic_name);
try {
this->declare_parameter<std::string>(twist_topic_name);
} catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
}

rclcpp::Parameter parameter;
if (rclcpp::PARAMETER_NOT_SET != 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) {
Expand Down Expand Up @@ -1414,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<std::string>(imu_topic_name);
try {
this->declare_parameter<std::string>(imu_topic_name);
} catch (const rclcpp::exceptions::NoParameterOverrideProvided &) {
}

rclcpp::Parameter parameter;
if (rclcpp::PARAMETER_NOT_SET != 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) {
Expand Down Expand Up @@ -1724,7 +1753,10 @@ void RosFilter<T>::loadParams()
process_noise_covariance.setZero();
std::vector<double> process_noise_covar_flat;

this->declare_parameter<std::vector<double>>("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))
Expand All @@ -1751,7 +1783,10 @@ 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");
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))
Expand Down

0 comments on commit 3326e80

Please sign in to comment.