fix(localizatation):add eagleye and area swithing setting[experiment][new]

launch/tier4_localization_launch/launch/localization.launch.xml

@@ -35,4 +35,9 @@
       <include file="$(find-pkg-share tier4_localization_launch)/launch/localization_error_monitor/localization_error_monitor.launch.xml"/>
     </group>
   </group>
+
+  <group if="$(eval &quot;'$(var pose_source)'=='ndt_and_eagleye'&quot;)">
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/switching.launch.xml">
+    </include>
+  </group>
 </launch>

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

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-  <arg name="output_pose_with_cov_name" default="/localization/pose_estimator/pose_with_covariance"/>
+  <arg name="output_pose_with_cov_name" default="/localization/pose_estimator/pose_with_covariance_TMP"/>
   <arg name="output_twist_with_cov_name" default="/localization/twist_estimator/twist_with_covariance"/>
   <arg name="directory_with_related_launches" default="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye"/>
   <arg name="use_eagleye_pose" default="true"/>
@@ -9,6 +9,9 @@
   <arg name="enable_additional_rolling" default="false"/>
   <arg name="use_multi_antenna_mode" default="false"/>
 
+  <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/septentrio_velocity_converter.launch.py">
+    </include>
+
   <group>
     <push-ros-namespace namespace="eagleye"/>
 
@@ -37,14 +40,14 @@
       <param from="$(var eagleye_param_path)"/>
       <param name="yaml_file" value="$(var eagleye_param_path)"/>
     </node>
-    <node pkg="eagleye_rt" name="yaw_rate_offset_node_1st" exec="yaw_rate_offset" args="1st">
+    <!-- <node pkg="eagleye_rt" name="yaw_rate_offset_node_1st" exec="yaw_rate_offset" args="1st">
       <param from="$(var eagleye_param_path)"/>
       <param name="yaml_file" value="$(var eagleye_param_path)"/>
     </node>
     <node pkg="eagleye_rt" name="yaw_rate_offset_node_2nd" exec="yaw_rate_offset" args="2nd">
       <param from="$(var eagleye_param_path)"/>
       <param name="yaml_file" value="$(var eagleye_param_path)"/>
-    </node>
+    </node> -->
     <node pkg="eagleye_rt" name="heading_node_1st" exec="heading" args="1st" if="$(eval &quot;'$(var use_rtk_heading)'=='false'&quot;)">
       <param from="$(var eagleye_param_path)"/>
       <param name="yaml_file" value="$(var eagleye_param_path)"/>
@@ -72,10 +75,10 @@
       <param from="$(var eagleye_param_path)"/>
       <param name="yaml_file" value="$(var eagleye_param_path)"/>
     </node>
-    <node pkg="eagleye_rt" name="slip_angle_node" exec="slip_angle">
+    <!-- <node pkg="eagleye_rt" name="slip_angle_node" exec="slip_angle">
       <param from="$(var eagleye_param_path)"/>
       <param name="yaml_file" value="$(var eagleye_param_path)"/>
-    </node>
+    </node> -->
     <node pkg="eagleye_rt" name="distance_node" exec="distance">
       <param from="$(var eagleye_param_path)"/>
       <param name="yaml_file" value="$(var eagleye_param_path)"/>
@@ -97,7 +100,7 @@
       <param name="yaml_file" value="$(var eagleye_param_path)"/>
     </node>
     <group if="$(var use_eagleye_pose)">
-      <node pkg="eagleye_rt" name="position_node" exec="position" if="$(eval &quot;'$(var use_rtk_dead_reckoning)'=='false'&quot;)">
+      <!-- <node pkg="eagleye_rt" name="position_node" exec="position" if="$(eval &quot;'$(var use_rtk_dead_reckoning)'=='false'&quot;)">
         <param from="$(var eagleye_param_path)"/>
         <param name="yaml_file" value="$(var eagleye_param_path)"/>
       </node>
@@ -133,12 +136,18 @@
         <param from="$(var eagleye_param_path)"/>
         <param name="yaml_file" value="$(var eagleye_param_path)"/>
       </node>
+      <node pkg="eagleye_rt" name="rtkfix_plane_validation" exec="rtkfix_plane_validation">
+            <param from="$(var eagleye_param_path)"/>
+            <param name="yaml_file" value="$(var eagleye_param_path)"/>
+            <remap from="gnss/fix" to="/sensing/gnss/septentrio/nav_sat_fix"/>
+            <remap from="gnss/reliable_rtkfix" to="/sensing/gnss/septentrio/reliable_nav_sat_fix"/>
+      </node> -->
     </group>
 
-    <node pkg="eagleye_rt" name="monitor" exec="monitor">
+    <!-- <node pkg="eagleye_rt" name="monitor" exec="monitor">
       <param from="$(var eagleye_param_path)"/>
       <param name="yaml_file" value="$(var eagleye_param_path)"/>
-    </node>
+    </node> -->
     <include file="$(var directory_with_related_launches)/gnss_converter.launch.xml">
       <arg name="config_path" value="$(var eagleye_param_path)"/>
     </include>

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

@@ -3,14 +3,15 @@
   <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"/>
+    <remap from="eagleye/fix" to="/sensing/gnss/septentrio/nav_sat_fix"/>
+    <!-- <remap from="eagleye/fix" to="/sensing/gnss/septentrio/reliable_nav_sat_fix"/> -->
     <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"/>
+    <param name="convert_height_num" value="1"/>
     <!-- 0 : EGM2008-1  1 : Japanese Geoid 2011 Ver2.1 -->
     <param name="geoid_type" value="1"/>
 
@@ -19,9 +20,9 @@
     <param name="base_link_frame_id" value="base_link"/>
     <param name="gnss_frame_id" value="gnss_link"/>
 
-    <param name="fix_only_publish" value="false"/>
+    <param name="fix_only_publish" value="true"/>
     <!-- 0:fix.status 1:fix.position_covariance[0] (if fix_only_publish==true)-->
-    <param name="fix_judgement_type" value="0"/>
+    <param name="fix_judgement_type" value="1"/>
     <!--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)-->

launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/septentrio_velocity_converter.launch.py

@@ -0,0 +1,15 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='septentrio_velocity_converter',
+            executable='septentrio_velocity_converter_node',
+            name='septentrio_velocity_converter_node',
+            output='screen',
+            parameters=[
+                {'coordinate_system': 'Cartesian'}  # Geodetic or Cartesian
+            ]
+        )
+    ])

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

@@ -1,13 +1,14 @@
 <?xml version="1.0"?>
 <launch>
+  <arg name="output_pose_with_covariance_topic" default="/localization/pose_estimator/pose_with_covariance_TMP"/>
   <group>
     <include file="$(find-pkg-share ndt_scan_matcher)/launch/ndt_scan_matcher.launch.xml">
       <arg name="input_map_points_topic" value="/map/pointcloud_map"/>
       <arg name="input/pointcloud" value="/localization/util/downsample/pointcloud"/>
       <arg name="input_initial_pose_topic" value="/localization/pose_twist_fusion_filter/biased_pose_with_covariance"/>
 
       <arg name="output_pose_topic" value="/localization/pose_estimator/pose"/>
-      <arg name="output_pose_with_covariance_topic" value="/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="output_pose_with_covariance_topic" value="$(var output_pose_with_covariance_topic)"/>
 
       <arg name="param_file" value="$(var ndt_scan_matcher_param_path)"/>
     </include>

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

@@ -7,7 +7,9 @@
   <group if="$(eval &quot;'$(var pose_source)'=='ndt'&quot;)">
     <group>
       <push-ros-namespace namespace="pose_estimator"/>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml">
+        <arg name="output_pose_with_covariance_topic" value="/localization/pose_estimator/pose_with_covariance"/>
+      </include>
     </group>
     <group>
       <push-ros-namespace namespace="util"/>
@@ -135,4 +137,38 @@
       <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py"/>
     </group>
   </group>
+
+  <!-- Eagleye Launch (as pose estimator) -->
+  <group if="$(eval &quot;'$(var pose_source)'=='ndt_and_eagleye' and '$(var twist_source)'!='eagleye'&quot;)">
+    <group>
+      <push-ros-namespace namespace="pose_estimator"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml">
+        <arg name="output_pose_with_covariance_topic" value="/localization/pose_estimator/ndt_scan_matcher/pose_with_covariance"/>
+      </include>
+    </group>
+    <group>
+      <push-ros-namespace namespace="pose_estimator"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml">
+        <arg name="output_pose_with_cov_name" value="/localization/pose_estimator/eagleye/pose_with_covariance"/>
+        <arg name="use_eagleye_pose" value="true"/>
+        <arg name="use_eagleye_twist" value="false"/>
+      </include>
+    </group>
+    <!-- pose initilizer for ndt-->
+    <group>
+      <push-ros-namespace namespace="util"/>
+      <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+        <arg name="ndt_enabled" value="true"/>
+        <arg name="gnss_enabled" value="true"/>
+        <arg name="ekf_enabled" value="true"/>
+        <arg name="yabloc_enabled" value="false"/>
+        <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
+        <arg name="config_file" value="$(var pose_initializer_param_path)"/>
+        <arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/>
+      </include>
+      <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py"/>
+    </group>
+  </group>
+
 </launch>

launch/tier4_localization_launch/launch/switching.launch.xml

@@ -0,0 +1,25 @@
+<launch>
+
+  <arg name="param_file_path" default="$(find-pkg-share gnss_area_filter)/config/gnss_area_filter.param.yaml" />
+  <arg name="input_lanelet2_map_bin_topic" default="/map/vector_map" />
+  <arg name="input_pose_with_covariance_topic" default="/localization/pose_twist_fusion_filter/biased_pose_with_covariance" />
+  <node pkg="gnss_area_filter" exec="gnss_area_filter" name="gnss_area_filter">
+    <remap from="input_lanelet2_map_bin_topic" to="$(var input_lanelet2_map_bin_topic)" />
+    <remap from="input_pose_with_covariance_topic" to="$(var input_pose_with_covariance_topic)" />
+
+    <param from="$(var param_file_path)" />
+  </node>
+
+  <node pkg="pose_switcher" name="pose_switcher" exec="pose_switcher" output="screen">
+    <remap from="gnss_pose" to="/localization/pose_estimator/eagleye/pose_with_covariance"/>
+    <remap from="lidar_pose" to="/localization/pose_estimator/ndt_scan_matcher/pose_with_covariance"/>
+    <remap from="selected_pose" to="/localization/pose_estimator/pose_with_covariance"/>
+    <param name="error_2d_threshold" value="0.3"/> <!-- [m] -->
+    <param name="error_yaw_threshold" value="1.0"/> <!-- [deg] -->
+    <param name="judge_switching_ealpsed_time_threshold" value="1.0"/> <!-- [s] -->
+  </node>
+
+  <node pkg="pointcloud_relay" name="pointcloud_relay" exec="pointcloud_relay_node">
+  </node>
+
+</launch>

map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp

@@ -118,6 +118,10 @@
   lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map);
   lanelet::ConstPolygons3d obstacle_polygons =
     lanelet::utils::query::getAllObstaclePolygons(viz_lanelet_map);
+  lanelet::ConstPolygons3d gnss_available_area_polygon =
+    lanelet::utils::query::getAllGnssAvailableArea(viz_lanelet_map);
+  lanelet::ConstPolygons3d switching_area_polygon =
+    lanelet::utils::query::getAllSwitchingArea(viz_lanelet_map);
   lanelet::ConstPolygons3d no_obstacle_segmentation_area =
     lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "no_obstacle_segmentation_area");
   lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out =
@@ -133,7 +137,7 @@
     cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id,
     cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area,
     cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area,
-    cl_hatched_road_markings_line, cl_no_parking_areas;
+    cl_hatched_road_markings_line, cl_no_parking_areas, cl_gnss_available_area, cl_switching_area;
   setColor(&cl_road, 0.27, 0.27, 0.27, 0.999);
   setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999);
   setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5);
@@ -156,6 +160,8 @@
   setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5);
   setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999);
   setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5);
+  setColor(&cl_gnss_available_area, 0.4, 0.27, 0.27, 0.5);
+  setColor(&cl_switching_area, 0.27, 0.27, 0.4, 0.5);
 
   visualization_msgs::msg::MarkerArray map_marker_array;
 
@@ -242,6 +248,14 @@
     &map_marker_array,
     lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas));
 
+  insertMarkerArray(
+    &map_marker_array,
+    lanelet::visualization::gnssavailableareaPolygonsAsMarkerArray(gnss_available_area_polygon, cl_gnss_available_area));
+
+  insertMarkerArray(
+    &map_marker_array,
+    lanelet::visualization::switchingareaPolygonsAsMarkerArray(switching_area_polygon, cl_switching_area));
+
   pub_marker_->publish(map_marker_array);
 }