add set_parameters callback

Signed-off-by: melike <[email protected]>

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)

localization/aw_pose_covariance_modifier_node/package.xml

@@ -17,9 +17,9 @@
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
-  <depend>std_srvs</depend>
+  <depend>rcl_interfaces</depend>
 
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

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<std_srvs::srv::SetBool>("/localization/pose_estimator/covariance_modifier");
+    this->create_client<rcl_interfaces::srv::SetParameters>("/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<std_srvs::srv::SetBool::Request>();
-  request->data = true;
+  auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
 
-  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<rcl_interfaces::srv::SetParameters>::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<AWPoseCovarianceModifierNode>());
   rclcpp::shutdown();
   return 0;
-}
+}

localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp

@@ -17,8 +17,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <std_srvs/srv/set_bool.hpp>
-
+#include <rcl_interfaces/srv/set_parameters.hpp>
 #include <string>
 
 class AWPoseCovarianceModifierNode : public rclcpp::Node
@@ -30,7 +29,7 @@ class AWPoseCovarianceModifierNode : public rclcpp::Node
     trusted_pose_with_cov_sub_;
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     new_pose_estimator_pub_;
-  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client_;
+  rclcpp::Client<rcl_interfaces::srv::SetParameters>::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_

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

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -42,6 +42,7 @@
 #include <tf2/transform_datatypes.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
+#include "rcl_interfaces/srv/set_parameters.hpp"
 
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -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<tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedPtr service_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_trigger_node_;
-  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr server_covariance_modifier_;
+  rclcpp::Service<rcl_interfaces::srv::SetParameters>::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<double, 36> covariance_modifier(std::array<double, 36> & 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_;
 };

localization/ndt_scan_matcher/package.xml

@@ -42,6 +42,7 @@
   <depend>tier4_localization_msgs</depend>
   <depend>tree_structured_parzen_estimator</depend>
   <depend>visualization_msgs</depend>
+  <depend>rcl_interfaces</depend>
 
   <test_depend>ament_cmake_cppcheck</test_depend>
   <test_depend>ament_lint_auto</test_depend>

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<std::string, std::string>),
-  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<geometry_msgs::msg::PoseWithCovarianceStamped>(
-      "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<std_srvs::srv::SetBool>(
-    "/localization/pose_estimator/covariance_modifier",
+  set_parameters_service_ = this->create_service<rcl_interfaces::srv::SetParameters>(
+    "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<tier4_autoware_utils::LoggerLevelConfigure>(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<std::mutex> lock(mutex_cov_modifier_);
+
+  for (const auto &param : 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<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "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;