From d7b870f615ff9840333ad58bd84f5356b34ed8b5 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 24 May 2021 14:06:31 -0700 Subject: [PATCH] Cherry picked commits to make galactic release build (#667) * Add missing message_filters dependency (#666) Headers from message_filters are included here: https://github.com/cra-ros-pkg/robot_localization/blob/67098c2341b5d1ccbcceb8eede60e79db74814a6/include/robot_localization/ros_robot_localization_listener.h\#L41-L42 Signed-off-by: Jacob Perron * Remove try-catch blocks around declare_parameter (#663) Try-catches were added in https://github.com/cra-ros-pkg/robot_localization/pull/631 due to a new rclcpp feature enforcing static parameter. The behavior was later patched for this particular use-case in https://github.com/ros2/rclcpp/pull/1673, so now we can avoid having to try-catch. Signed-off-by: Jacob Perron Co-authored-by: Jacob Perron --- CMakeLists.txt | 2 ++ package.xml | 1 + src/ros_filter.cpp | 65 ++++++++++------------------------------------ 3 files changed, 16 insertions(+), 52 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ceacfa58e..b49b9c81a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(diagnostic_msgs REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(geographic_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) find_package(nav_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(sensor_msgs REQUIRED) @@ -107,6 +108,7 @@ ament_target_dependencies( diagnostic_updater geographic_msgs geometry_msgs + message_filters nav_msgs rclcpp sensor_msgs diff --git a/package.xml b/package.xml index c755790d1..372e2943e 100644 --- a/package.xml +++ b/package.xml @@ -26,6 +26,7 @@ diagnostic_msgs diagnostic_updater geographiclib + message_filters nav_msgs rclcpp rmw_implementation diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 125656748..58a73393b 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -819,10 +819,7 @@ void RosFilter::loadParams() // Try to resolve tf_prefix std::string tf_prefix = ""; std::string tf_prefix_path = ""; - try { - this->declare_parameter("tf_prefix"); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter("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_); @@ -890,10 +887,7 @@ void RosFilter::loadParams() control_timeout = this->declare_parameter("control_timeout", 0.0); if (use_control_) { - try { - this->declare_parameter>("control_config"); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter>("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 << @@ -909,10 +903,7 @@ void RosFilter::loadParams() use_control_ = false; } - try { - this->declare_parameter>("acceleration_limits"); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter>("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 << @@ -928,10 +919,7 @@ void RosFilter::loadParams() acceleration_limits.resize(TWIST_SIZE, 1.0); } - try { - this->declare_parameter>("acceleration_gains"); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter>("acceleration_gains"); if (this->get_parameter("acceleration_gains", acceleration_gains)) { const int size = acceleration_gains.size(); if (size != TWIST_SIZE) { @@ -945,10 +933,7 @@ void RosFilter::loadParams() } } - try { - this->declare_parameter>("deceleration_limits"); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter>("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 << @@ -964,10 +949,7 @@ void RosFilter::loadParams() deceleration_limits = acceleration_limits; } - try { - this->declare_parameter>("deceleration_gains"); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter>("deceleration_gains"); if (this->get_parameter("deceleration_gains", deceleration_gains)) { const int size = deceleration_gains.size(); if (size != TWIST_SIZE) { @@ -999,10 +981,7 @@ void RosFilter::loadParams() dynamic_process_noise_covariance); std::vector initial_state; - try { - this->declare_parameter>("initial_state"); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter>("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 << @@ -1091,10 +1070,7 @@ void RosFilter::loadParams() ss << "odom" << topic_ind++; std::string odom_topic_name = ss.str(); std::string odom_topic; - try { - this->declare_parameter(odom_topic_name); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter(odom_topic_name); rclcpp::Parameter parameter; if (this->get_parameter(odom_topic_name, parameter)) { @@ -1244,10 +1220,7 @@ void RosFilter::loadParams() ss << "pose" << topic_ind++; std::string pose_topic_name = ss.str(); std::string pose_topic; - try { - this->declare_parameter(pose_topic_name); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter(pose_topic_name); rclcpp::Parameter parameter; if (this->get_parameter(pose_topic_name, parameter)) { @@ -1364,10 +1337,7 @@ void RosFilter::loadParams() ss << "twist" << topic_ind++; std::string twist_topic_name = ss.str(); std::string twist_topic; - try { - this->declare_parameter(twist_topic_name); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter(twist_topic_name); rclcpp::Parameter parameter; if (this->get_parameter(twist_topic_name, parameter)) { @@ -1448,10 +1418,7 @@ void RosFilter::loadParams() ss << "imu" << topic_ind++; std::string imu_topic_name = ss.str(); std::string imu_topic; - try { - this->declare_parameter(imu_topic_name); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter(imu_topic_name); rclcpp::Parameter parameter; if (this->get_parameter(imu_topic_name, parameter)) { @@ -1760,10 +1727,7 @@ void RosFilter::loadParams() process_noise_covariance.setZero(); std::vector process_noise_covar_flat; - try { - this->declare_parameter>("process_noise_covariance"); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter>("process_noise_covariance"); if (this->get_parameter( "process_noise_covariance", process_noise_covar_flat)) @@ -1790,10 +1754,7 @@ void RosFilter::loadParams() initial_estimate_error_covariance.setZero(); std::vector estimate_error_covar_flat; - try { - this->declare_parameter>("initial_estimate_covariance"); - } catch (const rclcpp::exceptions::NoParameterOverrideProvided &) { - } + this->declare_parameter>("initial_estimate_covariance"); if (this->get_parameter( "initial_estimate_covariance", estimate_error_covar_flat))