From 2b0fe2b69d2a1a2c135cd669a8c35988abf18612 Mon Sep 17 00:00:00 2001 From: meliketanrikulu Date: Tue, 5 Mar 2024 12:35:30 +0300 Subject: [PATCH] add set_parameters callback Signed-off-by: melike --- .../CMakeLists.txt | 7 +- .../package.xml | 4 +- .../src/aw_pose_covariance_modifier_node.cpp | 66 ++++++++++++------- .../aw_pose_covariance_modifier_node.hpp | 9 ++- localization/ndt_scan_matcher/CMakeLists.txt | 1 + .../ndt_scan_matcher_core.hpp | 13 ++-- localization/ndt_scan_matcher/package.xml | 1 + .../src/ndt_scan_matcher_core.cpp | 52 +++++++++------ 8 files changed, 93 insertions(+), 60 deletions(-) diff --git a/localization/aw_pose_covariance_modifier_node/CMakeLists.txt b/localization/aw_pose_covariance_modifier_node/CMakeLists.txt index 97c20b35c4e16..be045d1c14581 100644 --- a/localization/aw_pose_covariance_modifier_node/CMakeLists.txt +++ b/localization/aw_pose_covariance_modifier_node/CMakeLists.txt @@ -5,14 +5,13 @@ find_package(autoware_cmake REQUIRED) autoware_package() find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(std_srvs REQUIRED) - +find_package(rcl_interfaces REQUIRED) set(NODE_NAME ${PROJECT_NAME}) set(EXEC_NAME ${PROJECT_NAME}_exe) add_executable(${PROJECT_NAME} src/aw_pose_covariance_modifier_node.cpp) -ament_target_dependencies(${PROJECT_NAME} rclcpp geometry_msgs std_srvs) +ament_target_dependencies(${PROJECT_NAME} rclcpp geometry_msgs rcl_interfaces) install(TARGETS ${PROJECT_NAME} @@ -30,4 +29,4 @@ include_directories(src/include/) ament_auto_package(INSTALL_TO_SHARE - launch) + launch) \ No newline at end of file diff --git a/localization/aw_pose_covariance_modifier_node/package.xml b/localization/aw_pose_covariance_modifier_node/package.xml index dfffdf0f4ae19..5df24a955c9be 100644 --- a/localization/aw_pose_covariance_modifier_node/package.xml +++ b/localization/aw_pose_covariance_modifier_node/package.xml @@ -17,9 +17,9 @@ geometry_msgs rclcpp rclcpp_components - std_srvs + rcl_interfaces ament_cmake - + \ No newline at end of file diff --git a/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp b/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp index 7e197eaaf011c..c68d607c16f3d 100644 --- a/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp +++ b/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp @@ -29,39 +29,57 @@ AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode() : Node("AWPoseCovar "output_pose_with_covariance_topic", 10); client_ = - this->create_client("/localization/pose_estimator/covariance_modifier"); + this->create_client("/localization/pose_estimator/ndt_scan_matcher/set_parameters"); - startNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier(); - if (startNDTCovModifier == 1) { + while (!client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO( + this->get_logger(), + "Waiting for aw_pose_covariance_modifier_node service..."); + } + + activateNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier(); + if (activateNDTCovModifier == 1) { RCLCPP_INFO(get_logger(), "NDT pose covariance modifier activated ..."); } + else{ + RCLCPP_WARN(get_logger(), "Failed to enable NDT pose covariance modifier ..."); + } } bool AWPoseCovarianceModifierNode::callNDTCovarianceModifier() { - while (!client_->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(get_logger(), "Interrupted while waiting for the service. Exiting."); - return false; - } - RCLCPP_INFO(get_logger(), "Service not available, waiting again..."); - } - auto request = std::make_shared(); - request->data = true; + auto request = std::make_shared(); - auto future_result = client_->async_send_request(request); - if ( - rclcpp::spin_until_future_complete(get_node_base_interface(), future_result) == - rclcpp::FutureReturnCode::SUCCESS) { - auto response = future_result.get(); - RCLCPP_INFO(get_logger(), "Response: %d", response->success); - return true; - } else { - RCLCPP_ERROR(get_logger(), "Failed to receive response."); - return false; - } + rcl_interfaces::msg::Parameter parameter; + parameter.name = "aw_pose_covariance_modifier.enable"; + parameter.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + parameter.value.bool_value = true; + + request->parameters.push_back(parameter); + + client_->async_send_request( + request, [this](rclcpp::Client::SharedFuture result_) { + auto result_status = result_.wait_for(std::chrono::seconds(1)); + + if (result_status == std::future_status::ready) { + auto response = result_.get(); + + if (response && response->results.data()) { + RCLCPP_INFO(this->get_logger(), "aw_pose_covariance_modifier.enable parameter set successfully."); + return true; + } else { + RCLCPP_ERROR(this->get_logger(), "An error occurred while setting the aw_pose_covariance_modifier.enable parameter."); + return false; + } + } else { + RCLCPP_ERROR(this->get_logger(), "The request was not completed within the specified time."); + return false; + } + }); + return true; } + void AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg) { @@ -91,4 +109,4 @@ int main(int argc, char * argv[]) rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} +} \ No newline at end of file diff --git a/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp b/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp index 2184d30e70869..0da87150a2d0c 100644 --- a/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp +++ b/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp @@ -17,8 +17,7 @@ #include #include -#include - +#include #include class AWPoseCovarianceModifierNode : public rclcpp::Node @@ -30,7 +29,7 @@ class AWPoseCovarianceModifierNode : public rclcpp::Node trusted_pose_with_cov_sub_; rclcpp::Publisher::SharedPtr new_pose_estimator_pub_; - rclcpp::Client::SharedPtr client_; + rclcpp::Client::SharedPtr client_; void trusted_pose_with_cov_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg); @@ -39,7 +38,7 @@ class AWPoseCovarianceModifierNode : public rclcpp::Node private: double trusted_pose_rmse_; double trusted_pose_yaw_rmse_in_degrees_; - bool startNDTCovModifier = 0; + bool activateNDTCovModifier = 0; }; -#endif // AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_ +#endif // AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_ \ No newline at end of file diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index b4d656cb1810b..09d47ce198395 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.14) project(ndt_scan_matcher) find_package(autoware_cmake REQUIRED) +find_package(rcl_interfaces REQUIRED) autoware_package() # Compile flags for SIMD instructions diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 54bf90851f1eb..0cf83cc935ee7 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -42,6 +42,7 @@ #include #include #include +#include "rcl_interfaces/srv/set_parameters.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -83,9 +84,9 @@ class NDTScanMatcher : public rclcpp::Node void service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); - void activate_pose_covariance_modifier( - const std_srvs::srv::SetBool::Request::SharedPtr req, - std_srvs::srv::SetBool::Response::SharedPtr res); + void setParametersCallback( + const rcl_interfaces::srv::SetParameters::Request::SharedPtr request, + rcl_interfaces::srv::SetParameters::Response::SharedPtr response); void callback_timer(); void callback_sensor_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame); @@ -178,7 +179,7 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::Service::SharedPtr service_; rclcpp::Service::SharedPtr service_trigger_node_; - rclcpp::Service::SharedPtr server_covariance_modifier_; + rclcpp::Service::SharedPtr set_parameters_service_; tf2_ros::TransformBroadcaster tf2_broadcaster_; tf2_ros::Buffer tf2_buffer_; @@ -215,9 +216,11 @@ class NDTScanMatcher : public rclcpp::Node std::array covariance_modifier(std::array & in_ndt_covariance); - // To be used for timeout control + // To be used for AW Pose Covariance Modifier - Trusted Pose timeout control bool checkTrustedPoseTimeout(); rclcpp::Time trustedPoseCallbackTime; + std::mutex mutex_cov_modifier_; + void createTrustedSourcePoseSubscriber(); HyperParameters param_; }; diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 0913ee04174f5..a1d3fa823d93d 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -42,6 +42,7 @@ tier4_localization_msgs tree_structured_parzen_estimator visualization_msgs + rcl_interfaces ament_cmake_cppcheck ament_lint_auto diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 44732cc146fe0..679eff04d8a80 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -71,7 +71,6 @@ NDTScanMatcher::NDTScanMatcher() tf2_listener_(tf2_buffer_), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), - activate_pose_covariance_modifier_(false), is_activated_(false), param_(this) { @@ -103,12 +102,6 @@ NDTScanMatcher::NDTScanMatcher() std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), sensor_sub_opt); - trusted_source_pose_sub_ = - this->create_subscription( - "input_trusted_pose_topic", 100, - std::bind(&NDTScanMatcher::callback_trusted_source_pose, this, std::placeholders::_1), - initial_pose_sub_opt); - // Only if regularization is enabled, subscribe to the regularization base pose if (param_.ndt_regularization_enable) { // NOTE: The reason that the regularization subscriber does not belong to the @@ -182,11 +175,10 @@ NDTScanMatcher::NDTScanMatcher() &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); - server_covariance_modifier_ = this->create_service( - "/localization/pose_estimator/covariance_modifier", + set_parameters_service_ = this->create_service( + "ndt_scan_matcher/set_parameters", std::bind( - &NDTScanMatcher::activate_pose_covariance_modifier, this, std::placeholders::_1, - std::placeholders::_2), + &NDTScanMatcher::setParametersCallback, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); ndt_ptr_->setParams(param_.ndt); @@ -200,17 +192,37 @@ NDTScanMatcher::NDTScanMatcher() logger_configure_ = std::make_unique(this); } - -void NDTScanMatcher::activate_pose_covariance_modifier( - const std_srvs::srv::SetBool::Request::SharedPtr req, - std_srvs::srv::SetBool::Response::SharedPtr res) +void NDTScanMatcher::setParametersCallback( + const rcl_interfaces::srv::SetParameters::Request::SharedPtr request, + rcl_interfaces::srv::SetParameters::Response::SharedPtr response) { - activate_pose_covariance_modifier_ = req->data; - RCLCPP_INFO(this->get_logger(), " Pose Covariance Modifier Activated ..."); - res->success = true; - res->message = "Covariance Modifier Activated for NDT"; -} + std::lock_guard lock(mutex_cov_modifier_); + + for (const auto ¶m : request->parameters) { + + if(param.name == "aw_pose_covariance_modifier.enable"){ + activate_pose_covariance_modifier_ = param.value.bool_value; + RCLCPP_INFO(this->get_logger(), "aw_pose_covariance_modifier.enable set to : %d", activate_pose_covariance_modifier_); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "Parameter successfully set."; + + response->results.push_back(result); + } + } + + if(activate_pose_covariance_modifier_ == 1){ + NDTScanMatcher::createTrustedSourcePoseSubscriber(); + } +} +void NDTScanMatcher::createTrustedSourcePoseSubscriber(){ + trusted_source_pose_sub_ = + this->create_subscription( + "input_trusted_pose_topic", 100, + std::bind(&NDTScanMatcher::callback_trusted_source_pose, this, std::placeholders::_1)); +} void NDTScanMatcher::publish_diagnostic() { diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;