style(pre-commit): autofix

launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

@@ -121,5 +121,4 @@
       <arg name="input_pointcloud" value="$(var override_input_pointcloud)"/>
     </include>
   </group>
-
 </launch>

localization/aw_pose_covariance_modifier_node/README.md

@@ -1 +1 @@
-# aw_pose_covariance_modifier
+# aw_pose_covariance_modifier

localization/aw_pose_covariance_modifier_node/launch/aw_pose_covariance_modifier_node.launch.xml

@@ -3,10 +3,8 @@
   <arg name="input_trusted_pose_with_cov_topic" default="/sensing/gnss/pose_with_covariance" description="Trusted pose input .. "/>
   <arg name="output_pose_with_covariance_topic" default="/localization/pose_estimator/pose_with_covariance" description="Estimated self position with covariance"/>
 
-
-  <node pkg="aw_pose_covariance_modifier_node" exec="aw_pose_covariance_modifier_node" name="aw_pose_covariance_modifier_node" output="screen" >
-  <remap from="input_trusted_pose_with_cov_topic" to="$(var input_trusted_pose_with_cov_topic)"/>
-  <remap from="output_pose_with_covariance_topic" to="$(var output_pose_with_covariance_topic)"/>
-
+  <node pkg="aw_pose_covariance_modifier_node" exec="aw_pose_covariance_modifier_node" name="aw_pose_covariance_modifier_node" output="screen">
+    <remap from="input_trusted_pose_with_cov_topic" to="$(var input_trusted_pose_with_cov_topic)"/>
+    <remap from="output_pose_with_covariance_topic" to="$(var output_pose_with_covariance_topic)"/>
   </node>
 </launch>

localization/aw_pose_covariance_modifier_node/package.xml

@@ -14,12 +14,12 @@
 
   <build_depend>rosidl_default_generators</build_depend>
 
+  <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
-
-  <depend>geometry_msgs</depend>
   <depend>std_srvs</depend>
 
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp

@@ -13,78 +13,82 @@
 // limitations under the License.
 
 #include "include/aw_pose_covariance_modifier_node.hpp"
-#include <rclcpp/rclcpp.hpp>
 
+#include <rclcpp/rclcpp.hpp>
 
-AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode()
-            : Node("AWPoseCovarianceModifierNode")
+AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode() : Node("AWPoseCovarianceModifierNode")
 {
-    trusted_pose_with_cov_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
-            "input_trusted_pose_with_cov_topic",10000,std::bind(&AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback,this,std::placeholders::_1));
-
-
-    new_pose_estimator_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("output_pose_with_covariance_topic",10);
-
-    client_ = this->create_client<std_srvs::srv::SetBool>("/localization/pose_estimator/covariance_modifier");
-
-    startNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier();
-    if(startNDTCovModifier == 1){
-        RCLCPP_INFO(get_logger(), "NDT pose covariance modifier activated ...");
-
-    }
-
+  trusted_pose_with_cov_sub_ =
+    this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "input_trusted_pose_with_cov_topic", 10000,
+      std::bind(
+        &AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback, this,
+        std::placeholders::_1));
+
+  new_pose_estimator_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "output_pose_with_covariance_topic", 10);
+
+  client_ =
+    this->create_client<std_srvs::srv::SetBool>("/localization/pose_estimator/covariance_modifier");
+
+  startNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier();
+  if (startNDTCovModifier == 1) {
+    RCLCPP_INFO(get_logger(), "NDT pose covariance modifier activated ...");
+  }
 }
 
-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 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;
+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 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;
+  }
 }
-void AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr &msg) {
-
-    geometry_msgs::msg::PoseWithCovarianceStamped  pose_estimator_pose = *msg;
-
-    trusted_pose_rmse_ = (std::sqrt(pose_estimator_pose.pose.covariance[0]) + std::sqrt(pose_estimator_pose.pose.covariance[7]) ) / 2;
-    trusted_pose_yaw_rmse_in_degrees_ = std::sqrt(pose_estimator_pose.pose.covariance[35]) * 180 / M_PI;
-
-    if (trusted_pose_rmse_ > 0.25){
-        RCLCPP_INFO(this->get_logger(),"Trusted Pose RMSE is under the threshold. It will not be used as a pose source.");
-    }
-    else{
-
-        if (trusted_pose_yaw_rmse_in_degrees_ >= 0.3){
-            pose_estimator_pose.pose.covariance[35] = 1000000;
-        }
-
-        new_pose_estimator_pub_->publish(pose_estimator_pose);
-
+void AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg)
+{
+  geometry_msgs::msg::PoseWithCovarianceStamped pose_estimator_pose = *msg;
+
+  trusted_pose_rmse_ = (std::sqrt(pose_estimator_pose.pose.covariance[0]) +
+                        std::sqrt(pose_estimator_pose.pose.covariance[7])) /
+                       2;
+  trusted_pose_yaw_rmse_in_degrees_ =
+    std::sqrt(pose_estimator_pose.pose.covariance[35]) * 180 / M_PI;
+
+  if (trusted_pose_rmse_ > 0.25) {
+    RCLCPP_INFO(
+      this->get_logger(),
+      "Trusted Pose RMSE is under the threshold. It will not be used as a pose source.");
+  } else {
+    if (trusted_pose_yaw_rmse_in_degrees_ >= 0.3) {
+      pose_estimator_pose.pose.covariance[35] = 1000000;
     }
 
+    new_pose_estimator_pub_->publish(pose_estimator_pose);
+  }
+}
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<AWPoseCovarianceModifierNode>());
+  rclcpp::shutdown();
+  return 0;
 }
-int main(int argc, char *argv[]) {
-    rclcpp::init(argc, 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

@@ -15,8 +15,10 @@
 #define AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_
 
 #include <rclcpp/rclcpp.hpp>
-#include <std_srvs/srv/set_bool.hpp>
+
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <std_srvs/srv/set_bool.hpp>
+
 #include <string>
 
 using namespace std::chrono_literals;
@@ -26,17 +28,20 @@ class AWPoseCovarianceModifierNode : public rclcpp::Node
 public:
   AWPoseCovarianceModifierNode();
 
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr trusted_pose_with_cov_sub_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr new_pose_estimator_pub_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    trusted_pose_with_cov_sub_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    new_pose_estimator_pub_;
   rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client_;
 
-  void trusted_pose_with_cov_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr &msg);
+  void trusted_pose_with_cov_callback(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
   bool callNDTCovarianceModifier();
+
 private:
   double trusted_pose_rmse_;
   double trusted_pose_yaw_rmse_in_degrees_;
   bool startNDTCovModifier = 0;
 };
 
-
 #endif  // AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -140,7 +140,8 @@ class NDTScanMatcher : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr map_update_timer_;
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_sub_;
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr regularization_pose_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    regularization_pose_sub_;
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     trusted_source_pose_sub_;
 
@@ -177,7 +178,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<std_srvs::srv::SetBool>::SharedPtr server_covariance_modifier_;
 
   tf2_ros::TransformBroadcaster tf2_broadcaster_;
   tf2_ros::Buffer tf2_buffer_;
@@ -202,10 +203,11 @@ class NDTScanMatcher : public rclcpp::Node
   geometry_msgs::msg::PoseWithCovarianceStamped trusted_source_pose_;
   bool activate_pose_covariance_modifier_;
   bool close_ndt_pose_source_ = false;
-  struct trusted_pose_{
-      double pose_avarage_rmse_xy = 0.0;
-      double yaw_rmse = 0.0;
-  }trustedPose;
+  struct trusted_pose_
+  {
+    double pose_avarage_rmse_xy = 0.0;
+    double yaw_rmse = 0.0;
+  } trustedPose;
 
   std::atomic<bool> is_activated_;
   std::unique_ptr<MapUpdateModule> map_update_module_;