Merge pull request #666 from tier4/sync-upstream

chore: sync upstream

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -1,6 +1,10 @@
 <?xml version="1.0"?>
 <launch>
   <!-- Parameter files -->
+  <arg name="object_recognition_detection_compare_map_param_path"/>
+  <arg name="object_recognition_detection_euclidean_cluster_param_path"/>
+  <arg name="object_recognition_detection_outlier_param_path"/>
+  <arg name="object_recognition_detection_voxel_grid_based_euclidean_param_path"/>
   <arg name="object_recognition_detection_object_lanelet_filter_param_path"/>
   <arg name="object_recognition_detection_object_position_filter_param_path"/>
   <arg name="object_recognition_detection_pointcloud_map_filter_param_path"/>
@@ -121,6 +125,11 @@
           <arg name="camera_info7" value="$(var camera_info7)"/>
           <arg name="image_number" value="$(var image_number)"/>
           <arg name="sync_param_path" value="$(var object_recognition_detection_fusion_sync_param_path)"/>
+          <arg name="compare_map_param_path" value="$(var object_recognition_detection_compare_map_param_path)"/>
+          <arg name="euclidean_param_path" value="$(var object_recognition_detection_euclidean_cluster_param_path)"/>
+          <arg name="outlier_param_path" value="$(var object_recognition_detection_outlier_param_path)"/>
+          <arg name="voxel_grid_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_param_path)"/>
+          <arg name="voxel_grid_based_euclidean_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_cluster_param_path)"/>
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
           <arg name="use_object_filter" value="$(var use_object_filter)"/>
           <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
@@ -129,6 +138,7 @@
           <arg name="trust_distance" value="$(var trust_distance)"/>
         </include>
       </group>
+
       <!-- tracking module -->
       <group>
         <push-ros-namespace namespace="tracking"/>

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -85,9 +85,10 @@ class NDTScanMatcher : public rclcpp::Node
     const std_srvs::srv::SetBool::Request::SharedPtr req,
     std_srvs::srv::SetBool::Response::SharedPtr res);
 
-  void callback_sensor_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
+  void callback_sensor_points(
+    sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame);
   void callback_initial_pose(
-    geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);
+    geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr);
   void callback_regularization_pose(
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);
 
@@ -96,9 +97,9 @@ class NDTScanMatcher : public rclcpp::Node
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov);
 
   void transform_sensor_measurement(
-    const std::string source_frame, const std::string target_frame,
-    const pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_input_ptr,
-    pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_output_ptr);
+    const std::string & source_frame, const std::string & target_frame,
+    const pcl::shared_ptr<pcl::PointCloud<PointSource>> & sensor_points_input_ptr,
+    pcl::shared_ptr<pcl::PointCloud<PointSource>> & sensor_points_output_ptr);
 
   void publish_tf(
     const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg);
@@ -107,18 +108,18 @@ class NDTScanMatcher : public rclcpp::Node
     const bool is_converged);
   void publish_point_cloud(
     const rclcpp::Time & sensor_ros_time, const std::string & frame_id,
-    const pcl::shared_ptr<pcl::PointCloud<PointSource>> & sensor_points_mapTF_ptr);
+    const pcl::shared_ptr<pcl::PointCloud<PointSource>> & sensor_points_in_map_ptr);
   void publish_marker(
     const rclcpp::Time & sensor_ros_time, const std::vector<geometry_msgs::msg::Pose> & pose_array);
   void publish_initial_to_result_distances(
-    const rclcpp::Time & sensor_ros_msg, const geometry_msgs::msg::Pose & result_pose_msg,
+    const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg,
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_cov_msg,
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg,
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_new_msg);
 
   bool validate_num_iteration(const int iter_num, const int max_iter_num);
   bool validate_score(
-    const double score, const double score_threshold, const std::string score_name);
+    const double score, const double score_threshold, const std::string & score_name);
   bool validate_converged_param(
     const double & transform_probability, const double & nearest_voxel_transformation_likelihood);