From 07cef9f45201d72c612cc8621e20a8c35c612235 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 26 Jan 2024 16:48:19 +0900 Subject: [PATCH 1/3] fix(build): fix boost::optional errors Signed-off-by: Kotaro Yoshimoto --- .../object_recognition_utils/predicted_path_utils.hpp | 1 + .../include/signal_processing/lowpass_filter.hpp | 4 ++-- .../include/signal_processing/lowpass_filter_1d.hpp | 7 ++++--- common/signal_processing/src/lowpass_filter.cpp | 4 ++-- common/signal_processing/src/lowpass_filter_1d.cpp | 10 +++++----- .../test/src/lowpass_filter_1d_test.cpp | 4 ++-- .../signal_processing/test/src/lowpass_filter_test.cpp | 4 ++-- 7 files changed, 18 insertions(+), 16 deletions(-) diff --git a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp index 73cdce6c6444e..7aa4decbb1a30 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp @@ -21,6 +21,7 @@ #include #include +#include #include diff --git a/common/signal_processing/include/signal_processing/lowpass_filter.hpp b/common/signal_processing/include/signal_processing/lowpass_filter.hpp index 89b7a6da263f7..99ea7e64d6a94 100644 --- a/common/signal_processing/include/signal_processing/lowpass_filter.hpp +++ b/common/signal_processing/include/signal_processing/lowpass_filter.hpp @@ -27,7 +27,7 @@ template class LowpassFilterInterface { protected: - boost::optional x_; //!< @brief current filtered value + std::optional x_; //!< @brief current filtered value double gain_; //!< @brief gain value of first-order low-pass filter public: @@ -36,7 +36,7 @@ class LowpassFilterInterface void reset() { x_ = {}; } void reset(const T & x) { x_ = x; } - boost::optional getValue() const { return x_; } + std::optional getValue() const { return x_; } virtual T filter(const T & u) = 0; }; diff --git a/common/signal_processing/include/signal_processing/lowpass_filter_1d.hpp b/common/signal_processing/include/signal_processing/lowpass_filter_1d.hpp index bef0a99f38403..2e38738288ab2 100644 --- a/common/signal_processing/include/signal_processing/lowpass_filter_1d.hpp +++ b/common/signal_processing/include/signal_processing/lowpass_filter_1d.hpp @@ -15,7 +15,8 @@ #ifndef SIGNAL_PROCESSING__LOWPASS_FILTER_1D_HPP_ #define SIGNAL_PROCESSING__LOWPASS_FILTER_1D_HPP_ -#include +// #include +#include namespace signal_processing { @@ -29,7 +30,7 @@ double lowpassFilter(const double current_val, const double prev_val, const doub class LowpassFilter1d { private: - boost::optional x_; //!< @brief current filtered value + std::optional x_; //!< @brief current filtered value double gain_; //!< @brief gain value of first-order low-pass filter public: @@ -38,7 +39,7 @@ class LowpassFilter1d void reset(); void reset(const double x); - boost::optional getValue() const; + std::optional getValue() const; double filter(const double u); }; diff --git a/common/signal_processing/src/lowpass_filter.cpp b/common/signal_processing/src/lowpass_filter.cpp index c5119828f4c52..2e89358726f17 100644 --- a/common/signal_processing/src/lowpass_filter.cpp +++ b/common/signal_processing/src/lowpass_filter.cpp @@ -25,9 +25,9 @@ geometry_msgs::msg::Twist LowpassFilterTwist::filter(const geometry_msgs::msg::T x_->angular.y = gain_ * x_->angular.y + (1.0 - gain_) * u.angular.y; x_->angular.z = gain_ * x_->angular.z + (1.0 - gain_) * u.angular.z; - return x_.get(); + return x_.value(); } x_ = u; - return x_.get(); + return x_.value(); } diff --git a/common/signal_processing/src/lowpass_filter_1d.cpp b/common/signal_processing/src/lowpass_filter_1d.cpp index ce85d276d4a70..2a0deb503bba1 100644 --- a/common/signal_processing/src/lowpass_filter_1d.cpp +++ b/common/signal_processing/src/lowpass_filter_1d.cpp @@ -28,7 +28,7 @@ LowpassFilter1d::LowpassFilter1d(const double gain) : gain_(gain) void LowpassFilter1d::reset() { - x_ = {}; + x_ = std::nullopt; } void LowpassFilter1d::reset(const double x) @@ -36,7 +36,7 @@ void LowpassFilter1d::reset(const double x) x_ = x; } -boost::optional LowpassFilter1d::getValue() const +std::optional LowpassFilter1d::getValue() const { return x_; } @@ -44,11 +44,11 @@ boost::optional LowpassFilter1d::getValue() const double LowpassFilter1d::filter(const double u) { if (x_) { - const double ret = gain_ * x_.get() + (1.0 - gain_) * u; + const double ret = gain_ * x_.value() + (1.0 - gain_) * u; x_ = ret; - return x_.get(); + return x_.value(); } x_ = u; - return x_.get(); + return x_.value(); } diff --git a/common/signal_processing/test/src/lowpass_filter_1d_test.cpp b/common/signal_processing/test/src/lowpass_filter_1d_test.cpp index 5cce36884588e..283d407184c6c 100644 --- a/common/signal_processing/test/src/lowpass_filter_1d_test.cpp +++ b/common/signal_processing/test/src/lowpass_filter_1d_test.cpp @@ -23,7 +23,7 @@ TEST(lowpass_filter_1d, filter) LowpassFilter1d lowpass_filter_1d(0.1); // initial state - EXPECT_EQ(lowpass_filter_1d.getValue(), boost::none); + EXPECT_EQ(lowpass_filter_1d.getValue(), std::nullopt); // random filter EXPECT_NEAR(lowpass_filter_1d.filter(0.0), 0.0, epsilon); @@ -33,7 +33,7 @@ TEST(lowpass_filter_1d, filter) // reset without value lowpass_filter_1d.reset(); - EXPECT_EQ(lowpass_filter_1d.getValue(), boost::none); + EXPECT_EQ(lowpass_filter_1d.getValue(), std::nullopt); // reset with value lowpass_filter_1d.reset(-1.1); diff --git a/common/signal_processing/test/src/lowpass_filter_test.cpp b/common/signal_processing/test/src/lowpass_filter_test.cpp index 8dfea4dcae02e..992a7ed98e3ea 100644 --- a/common/signal_processing/test/src/lowpass_filter_test.cpp +++ b/common/signal_processing/test/src/lowpass_filter_test.cpp @@ -39,7 +39,7 @@ TEST(lowpass_filter_twist, filter) LowpassFilterTwist lowpass_filter_(0.1); { // initial state - EXPECT_EQ(lowpass_filter_.getValue(), boost::none); + EXPECT_EQ(lowpass_filter_.getValue(), std::nullopt); } { // random filter @@ -56,7 +56,7 @@ TEST(lowpass_filter_twist, filter) { // reset without value lowpass_filter_.reset(); - EXPECT_EQ(lowpass_filter_.getValue(), boost::none); + EXPECT_EQ(lowpass_filter_.getValue(), std::nullopt); } { // reset with value From 4e646f82d8d44b22d9a1f99cecc6c142600bc76a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 26 Jan 2024 16:49:32 +0900 Subject: [PATCH 2/3] chore: use .hpp instead of .h Signed-off-by: Kotaro Yoshimoto --- localization/yabloc/yabloc_common/src/cv_decompress.cpp | 2 +- localization/yabloc/yabloc_common/src/pub_sub.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/yabloc/yabloc_common/src/cv_decompress.cpp b/localization/yabloc/yabloc_common/src/cv_decompress.cpp index dc232488e147c..451ba2cc53944 100644 --- a/localization/yabloc/yabloc_common/src/cv_decompress.cpp +++ b/localization/yabloc/yabloc_common/src/cv_decompress.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include diff --git a/localization/yabloc/yabloc_common/src/pub_sub.cpp b/localization/yabloc/yabloc_common/src/pub_sub.cpp index 102a9012033fe..36d8812b1273b 100644 --- a/localization/yabloc/yabloc_common/src/pub_sub.cpp +++ b/localization/yabloc/yabloc_common/src/pub_sub.cpp @@ -14,7 +14,7 @@ #include "yabloc_common/pub_sub.hpp" -#include +#include #include namespace yabloc::common From 2638ef6b489719ab3776dbf6483e39d2770bd562 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 26 Jan 2024 16:50:16 +0900 Subject: [PATCH 3/3] chore: use rclcpp::QoS instead of rmw qos profile Signed-off-by: Kotaro Yoshimoto --- map/map_height_fitter/src/map_height_fitter.cpp | 2 +- planning/rtc_interface/src/rtc_interface.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index a05c63d44ebce..d57ab7383e780 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -61,7 +61,7 @@ MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), n if (partial_load) { group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); cli_map_ = node_->create_client( - "~/partial_map_load", rmw_qos_profile_default, group_); + "~/partial_map_load", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)), group_); } else { const auto durable_qos = rclcpp::QoS(1).transient_local(); sub_map_ = node_->create_subscription( diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index a7661f4c419a5..3130097745414 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -105,10 +105,10 @@ RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const srv_commands_ = node->create_service( cooperate_commands_namespace_ + "/" + name, std::bind(&RTCInterface::onCooperateCommandService, this, _1, _2), - rmw_qos_profile_services_default, callback_group_); + rclcpp::QoS(rclcpp::ServicesQoS()), callback_group_); srv_auto_mode_ = node->create_service( enable_auto_mode_namespace_ + "/" + name, - std::bind(&RTCInterface::onAutoModeService, this, _1, _2), rmw_qos_profile_services_default, + std::bind(&RTCInterface::onAutoModeService, this, _1, _2), rclcpp::QoS(rclcpp::ServicesQoS()), callback_group_); // Module