From 08bbf355f3412e22e0ac7d940dcad31aadb13199 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 20 May 2021 17:46:35 -0700 Subject: [PATCH 1/3] 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 --- src/ros_filter.cpp | 65 ++++++++++------------------------------------ 1 file changed, 13 insertions(+), 52 deletions(-) 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)) From 212bfb038482b934abd2ed24748c5e26c37d8c31 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 21 May 2021 14:25:14 -0700 Subject: [PATCH 2/3] 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 --- CMakeLists.txt | 2 ++ package.xml | 1 + 2 files changed, 3 insertions(+) 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 From c8ced961219fe6449145a101c29c1a0f8e11ef3e Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Tue, 25 May 2021 13:37:24 -0700 Subject: [PATCH 3/3] Revert "Enable QoS overrides (#657)" This reverts commit 2816e92f6b41fd4d7780e2cb2f383411edefb4d7. --- .../ros_robot_localization_listener.hpp | 17 +---------------- src/navsat_transform.cpp | 19 +++++-------------- src/ros_filter.cpp | 7 ++----- src/ros_robot_localization_listener.cpp | 7 +++---- 4 files changed, 11 insertions(+), 39 deletions(-) diff --git a/include/robot_localization/ros_robot_localization_listener.hpp b/include/robot_localization/ros_robot_localization_listener.hpp index 333c53800..4f50d4a32 100644 --- a/include/robot_localization/ros_robot_localization_listener.hpp +++ b/include/robot_localization/ros_robot_localization_listener.hpp @@ -49,18 +49,6 @@ namespace robot_localization { -namespace detail -{ -rclcpp::SubscriptionOptions -get_subscription_options_with_default_qos_override_policies() -{ - auto subscription_options = rclcpp::SubscriptionOptions(); - subscription_options.qos_overriding_options = - rclcpp::QosOverridingOptions::with_default_policies(); - return subscription_options; -} -} // namespace detail - //! @brief RosRobotLocalizationListener class //! //! This class wraps the RobotLocalizationEstimator. It listens to topics over @@ -82,10 +70,7 @@ class RosRobotLocalizationListener //! //! @param[in] node - rclcpp node shared pointer //! - explicit RosRobotLocalizationListener( - rclcpp::Node::SharedPtr node, - rclcpp::SubscriptionOptions options = - detail::get_subscription_options_with_default_qos_override_policies()); + explicit RosRobotLocalizationListener(rclcpp::Node::SharedPtr node); //! @brief Destructor //! diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index 74be08f0a..1df5c1f18 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -170,32 +170,23 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(1)); - auto subscriber_options = rclcpp::SubscriptionOptions(); - subscriber_options.qos_overriding_options = - rclcpp::QosOverridingOptions::with_default_policies(); odom_sub_ = this->create_subscription( - "odometry/filtered", custom_qos, std::bind( - &NavSatTransform::odomCallback, this, _1), subscriber_options); + "odometry/filtered", custom_qos, std::bind(&NavSatTransform::odomCallback, this, _1)); gps_sub_ = this->create_subscription( - "gps/fix", custom_qos, std::bind(&NavSatTransform::gpsFixCallback, this, _1), - subscriber_options); + "gps/fix", custom_qos, std::bind(&NavSatTransform::gpsFixCallback, this, _1)); if (!use_odometry_yaw_ && !use_manual_datum_) { imu_sub_ = this->create_subscription( - "imu", custom_qos, std::bind(&NavSatTransform::imuCallback, this, _1), subscriber_options); + "imu", custom_qos, std::bind(&NavSatTransform::imuCallback, this, _1)); } - rclcpp::PublisherOptions publisher_options; - publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); gps_odom_pub_ = - this->create_publisher( - "odometry/gps", rclcpp::QoS(10), publisher_options); + this->create_publisher("odometry/gps", rclcpp::QoS(10)); if (publish_gps_) { filtered_gps_pub_ = - this->create_publisher( - "gps/filtered", rclcpp::QoS(10), publisher_options); + this->create_publisher("gps/filtered", rclcpp::QoS(10)); } // Sleep for the parameterized amount of time, to give diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 58a73393b..13fa46344 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -1970,17 +1970,14 @@ void RosFilter::initialize() tf2::toMsg(tf2::Transform::getIdentity()); // Position publisher - rclcpp::PublisherOptions publisher_options; - publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); position_pub_ = - this->create_publisher( - "odometry/filtered", rclcpp::QoS(10), publisher_options); + this->create_publisher("odometry/filtered", rclcpp::QoS(10)); // Optional acceleration publisher if (publish_acceleration_) { accel_pub_ = this->create_publisher( - "accel/filtered", rclcpp::QoS(10), publisher_options); + "accel/filtered", rclcpp::QoS(10)); } const std::chrono::duration timespan{1.0 / frequency_}; diff --git a/src/ros_robot_localization_listener.cpp b/src/ros_robot_localization_listener.cpp index a6faeae32..da6917fcb 100644 --- a/src/ros_robot_localization_listener.cpp +++ b/src/ros_robot_localization_listener.cpp @@ -75,12 +75,11 @@ FilterTypes::FilterType filterTypeFromString( } RosRobotLocalizationListener::RosRobotLocalizationListener( - rclcpp::Node::SharedPtr node, - rclcpp::SubscriptionOptions options) + rclcpp::Node::SharedPtr node) : qos1_(1), qos10_(10), - odom_sub_(node, "odom/filtered", qos1_.get_rmw_qos_profile(), options), - accel_sub_(node, "acceleration/filtered", qos1_.get_rmw_qos_profile(), options), + odom_sub_(node, "odom/filtered", qos1_.get_rmw_qos_profile()), + accel_sub_(node, "acceleration/filtered", qos1_.get_rmw_qos_profile()), sync_(odom_sub_, accel_sub_, 10u), node_clock_(node->get_node_clock_interface()->get_clock()), node_logger_(node->get_node_logging_interface()),