Merge branch 'main' into refactor/start_planner_post_process

Signed-off-by: kyoichi-sugahara <[email protected]>

launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml

@@ -145,7 +145,8 @@
   </group>
 
   <group if="$(var use_eagleye_pose)">
-    <include file="$(var directory_with_related_launches)/fix2pose.launch.xml">
+    <include file="$(find-pkg-share eagleye_geo_pose_fusion)/launch/geo_pose_fusion.launch.xml"/>
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml">
       <arg name="output_pose_with_cov_name" value="$(var output_pose_with_cov_name)"/>
     </include>
   </group>

launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/fix2pose.launch.xml → launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml

@@ -1,30 +1,23 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <arg name="output_pose_with_cov_name" default="/localization/pose_estimator/pose_with_covariance"/>
-  <node pkg="eagleye_fix2pose" name="fix2pose_node" exec="fix2pose">
-    <!-- /eagleye/fix or  /GNSS/fix(GNSS pose only mode)-->
-    <remap from="eagleye/fix" to="eagleye/fix"/>
+
+  <node pkg="eagleye_geo_pose_converter" name="geo_pose_converter_node" exec="geo_pose_converter" output="screen">
     <remap from="eagleye/pose_with_covariance" to="$(var output_pose_with_cov_name)"/>
+
     <!-- plane rectangular coordinate number -->
     <param name="plane" value="7"/>
     <!-- 1 : plane rectangular coordinate  2 : MGRS -->
     <param name="tf_num" value="2"/>
     <!-- 0 : No convert  1 : ellipsoid -> altitude  2 : altitude -> ellipsoid -->
     <param name="convert_height_num" value="0"/>
-    <!-- 0 : EGM2008-1  1 : Japanese Geoid 2011 Ver2.1 -->
+    <!-- cspell:ignore GSIGEO2011 -->
+    <!-- 0 : EGM2008-1  1 : GSIGEO2011 Ver2.1 -->
     <param name="geoid_type" value="1"/>
 
     <param name="parent_frame_id" value="map"/>
     <param name="child_frame_id" value="eagleye_base_link"/>
     <param name="base_link_frame_id" value="base_link"/>
     <param name="gnss_frame_id" value="gnss_link"/>
-
-    <param name="fix_only_publish" value="false"/>
-    <!-- 0:fix.status 1:fix.position_covariance[0] (if fix_only_publish==true)-->
-    <param name="fix_judgement_type" value="0"/>
-    <!--Fix status(if fix_judgement_type is 0)-->
-    <param name="fix_gnss_status" value="0"/>
-    <!-- Minimum covariance of the pose topics to be published(if fix_judgement_type is 0)-->
-    <param name="fix_std_pos_thres" value="0.1"/>
   </node>
 </launch>

launch/tier4_localization_launch/package.xml

@@ -16,7 +16,8 @@
 
   <exec_depend>ar_tag_based_localizer</exec_depend>
   <exec_depend>automatic_pose_initializer</exec_depend>
-  <exec_depend>eagleye_fix2pose</exec_depend>
+  <exec_depend>eagleye_geo_pose_converter</exec_depend>
+  <exec_depend>eagleye_geo_pose_fusion</exec_depend>
   <exec_depend>eagleye_gnss_converter</exec_depend>
   <exec_depend>eagleye_rt</exec_depend>
   <exec_depend>ekf_localizer</exec_depend>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -5,7 +5,7 @@
   <arg name="input_vector_map_topic_name" default="/map/vector_map"/>
   <arg name="input_pointcloud_map_topic_name" default="/map/pointcloud_map"/>
 
-  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="">
+  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" output="screen">
     <composable_node pkg="behavior_path_planner" plugin="behavior_path_planner::BehaviorPathPlannerNode" name="behavior_path_planner" namespace="">
       <!-- topic remap -->
       <remap from="~/input/route" to="$(var input_route_topic_name)"/>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

@@ -2,7 +2,7 @@
   <arg name="interface_input_topic" default="/planning/scenario_planning/lane_driving/behavior_planning/path"/>
   <arg name="interface_output_topic" default="/planning/scenario_planning/lane_driving/trajectory"/>
 
-  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="motion_planning_container" namespace="" args="">
+  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="motion_planning_container" namespace="" args="" output="screen">
     <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
   </node_container>
 

launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml

@@ -1,32 +1,38 @@
 <launch>
-  <arg name="vehicle_param_file"/>
-
   <!-- parking scenario -->
-  <group>
+  <group if="$(var launch_parking_module)">
     <push-ros-namespace namespace="parking"/>
-    <group>
-      <include file="$(find-pkg-share costmap_generator)/launch/costmap_generator.launch.xml">
-        <arg name="input_objects" value="/perception/object_recognition/objects"/>
-        <arg name="input_points_no_ground" value="/perception/obstacle_segmentation/pointcloud"/>
-        <arg name="input_lanelet_map" value="/map/vector_map"/>
-        <arg name="input_scenario" value="/planning/scenario_planning/scenario"/>
-        <arg name="output_grid_map" value="costmap_generator/grid_map"/>
-        <arg name="output_occupancy_grid" value="costmap_generator/occupancy_grid"/>
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
-      </include>
-    </group>
 
-    <group>
-      <include file="$(find-pkg-share freespace_planner)/launch/freespace_planner.launch.xml">
-        <arg name="input_route" value="/planning/mission_planning/route"/>
-        <arg name="input_occupancy_grid" value="costmap_generator/occupancy_grid"/>
-        <arg name="input_scenario" value="/planning/scenario_planning/scenario"/>
-        <arg name="input_odometry" value="/localization/kinematic_state"/>
-        <arg name="output_trajectory" value="/planning/scenario_planning/parking/trajectory"/>
-        <arg name="is_completed" value="/planning/scenario_planning/parking/is_completed"/>
-        <arg name="param_file" value="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml"/>
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
-      </include>
-    </group>
+    <node_container pkg="rclcpp_components" exec="$(var container_type)" name="parking_container" namespace="" output="screen">
+      <composable_node pkg="costmap_generator" plugin="CostmapGenerator" name="costmap_generator" namespace="">
+        <!-- topic remap -->
+        <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
+        <remap from="~/input/points_no_ground" to="/perception/obstacle_segmentation/pointcloud"/>
+        <remap from="~/input/vector_map" to="/map/vector_map"/>
+        <remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
+        <remap from="~/output/grid_map" to="costmap_generator/grid_map"/>
+        <remap from="~/output/occupancy_grid" to="costmap_generator/occupancy_grid"/>
+        <!-- params -->
+        <param from="$(var vehicle_param_file)"/>
+        <param from="$(var costmap_generator_param_path)"/>
+        <extra_arg name="use_intra_process_comms" value="false"/>
+      </composable_node>
+
+      <composable_node pkg="freespace_planner" plugin="freespace_planner::FreespacePlannerNode" name="freespace_planner" namespace="">
+        <!-- topic remap -->
+        <remap from="~/input/route" to="/planning/mission_planning/route"/>
+        <remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
+        <remap from="~/input/occupancy_grid" to="costmap_generator/occupancy_grid"/>
+        <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+        <remap from="~/output/trajectory" to="/planning/scenario_planning/parking/trajectory"/>
+        <remap from="is_completed" to="/planning/scenario_planning/parking/is_completed"/>
+        <!-- params -->
+        <param from="$(var vehicle_param_file)"/>
+        <param from="$(var freespace_planner_param_path)"/>
+        <extra_arg name="use_intra_process_comms" value="false"/>
+      </composable_node>
+
+      <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
+    </node_container>
   </group>
 </launch>

launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml

@@ -54,8 +54,8 @@
     </group>
     <!-- parking -->
     <group>
-      <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/parking.launch.py">
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
+      <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/parking.launch.xml">
+        <arg name="container_type" value="component_container_mt"/>
       </include>
     </group>
   </group>

localization/ndt_scan_matcher/README.md

@@ -34,6 +34,8 @@ One optional function is regularization. Please see the regularization chapter i
 | `points_aligned`                  | `sensor_msgs::msg::PointCloud2`                 | [debug topic] pointcloud aligned by scan matching                                                                                        |
 | `points_aligned_no_ground`        | `sensor_msgs::msg::PointCloud2`                 | [debug topic] de-grounded pointcloud aligned by scan matching                                                                            |
 | `initial_pose_with_covariance`    | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching                                                                                         |
+| `multi_ndt_pose`                  | `geometry_msgs::msg::PoseArray`                 | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation                                             |
+| `multi_initial_pose`              | `geometry_msgs::msg::PoseArray`                 | [debug topic] initial poses for real-time covariance estimation                                                                          |
 | `exe_time_ms`                     | `tier4_debug_msgs::msg::Float32Stamped`         | [debug topic] execution time for scan matching [ms]                                                                                      |
 | `transform_probability`           | `tier4_debug_msgs::msg::Float32Stamped`         | [debug topic] score of scan matching                                                                                                     |
 | `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped`         | [debug topic] score of scan matching based on de-grounded LiDAR scan                                                                     |
@@ -257,3 +259,26 @@ This is a function that uses de-grounded LiDAR scan to estimate the scan matchin
 | ------------------------------------- | ------ | ------------------------------------------------------------------------------------- |
 | `estimate_scores_for_degrounded_scan` | bool   | Flag for using scan matching score based on de-grounded LiDAR scan (FALSE by default) |
 | `z_margin_for_ground_removal`         | double | Z-value margin for removal ground points                                              |
+
+## 2D real-time covariance estimation
+
+### Abstract
+
+Calculate 2D covariance (xx, xy, yx, yy) in real time using the NDT convergence from multiple initial poses.
+The arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function.
+In this implementation, the number of initial positions is fixed to simplify the code.
+The covariance can be seen as error ellipse from ndt_pose_with_covariance setting on rviz2.
+[original paper](https://www.fujipress.jp/jrm/rb/robot003500020435/).
+
+Note that this function may spoil healthy system behavior if it consumes much calculation resources.
+
+### Parameters
+
+initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix.
+initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.
+
+| Name                          | Type                | Description                                                       |
+| ----------------------------- | ------------------- | ----------------------------------------------------------------- |
+| `use_covariance_estimation`   | bool                | Flag for using real-time covariance estimation (FALSE by default) |
+| `initial_pose_offset_model_x` | std::vector<double> | X-axis offset [m]                                                 |
+| `initial_pose_offset_model_y` | std::vector<double> | Y-axis offset [m]                                                 |

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -72,6 +72,14 @@
         0.0,   0.0,   0.0,   0.0,      0.0,      0.000625,
       ]
 
+    # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value)
+    use_covariance_estimation: false
+
+    # Offset arrangement in covariance estimation [m]
+    # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.
+    initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0]
+    initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0]
+
     # Regularization switch
     regularization_enabled: false