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/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/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/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 125656748..13fa46344 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))
@@ -2009,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()),