From 13e04ccd4eb3df90b8963f8f6f9ae63e92c4e456 Mon Sep 17 00:00:00 2001
From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com>
Date: Mon, 19 Aug 2024 17:51:44 +0900
Subject: [PATCH] feat(psim)!: preapre settings to launch localization modules
 on psim (#8212)

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../launch/simulator.launch.xml               | 57 +++++++++++++++-
 .../simple_planning_simulator_core.hpp        |  8 ++-
 .../simple_planning_simulator.launch.py       | 68 ++++++++++++-------
 .../simple_planning_simulator_core.cpp        | 23 ++++++-
 4 files changed, 129 insertions(+), 27 deletions(-)

diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml
index 3677eefa2cffb..e1822884e5722 100644
--- a/launch/tier4_simulator_launch/launch/simulator.launch.xml
+++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml
@@ -5,13 +5,18 @@
   <arg name="laserscan_based_occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater"/>
   <arg name="occupancy_grid_map_updater_param_path"/>
+
+  <arg name="localization_error_monitor_param_path"/>
+  <arg name="ekf_localizer_param_path"/>
   <arg name="pose_initializer_param_path"/>
+  <arg name="twist2accel_param_path"/>
+
   <arg name="launch_simulator_perception_modules" default="true"/>
   <arg name="launch_dummy_perception"/>
   <arg name="launch_dummy_vehicle"/>
   <arg
     name="localization_sim_mode"
-    description="Select localization mode. Options are 'none' or 'api'. 'api' starts an external API for initial position estimation. 'none' does not start any localization-related process."
+    description="Select localization mode. Options are 'none', 'api' or 'pose_twist_estimator'. 'pose_twist_estimator' starts most of the localization modules except for the ndt_scan_matcher. 'api' starts an external API for initial position estimation. 'none' does not start any localization-related process."
   />
   <arg name="launch_dummy_doors"/>
   <arg name="launch_diagnostic_converter"/>
@@ -129,7 +134,7 @@
     </group>
   </group>
 
-  <!-- Dummy localization -->
+  <!-- localization -->
   <group if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;none&quot;')">
     <!-- Do nothing -->
   </group>
@@ -147,6 +152,50 @@
     </include>
   </group>
 
+  <group if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')">
+    <group>
+      <!-- start name space localization -->
+      <push-ros-namespace namespace="localization"/>
+      <group>
+        <push-ros-namespace namespace="util"/>
+        <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+          <arg name="user_defined_initial_pose/enable" value="false"/>
+          <arg name="user_defined_initial_pose/pose" value="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]"/>
+          <arg name="ndt_enabled" value="false"/>
+          <arg name="gnss_enabled" value="false"/>
+          <arg name="ekf_enabled" value="true"/>
+          <arg name="yabloc_enabled" value="false"/>
+          <arg name="stop_check_enabled" value="false"/>
+          <arg name="config_file" value="$(var pose_initializer_param_path)"/>
+        </include>
+      </group>
+
+      <group>
+        <push-ros-namespace namespace="twist_estimator"/>
+        <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/gyro_odometer.launch.xml"/>
+      </group>
+
+      <!-- pose_twist_fusion_filter module -->
+      <group>
+        <push-ros-namespace namespace="pose_twist_fusion_filter"/>
+        <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml"/>
+      </group>
+    </group>
+    <!-- end name space localization -->
+    <group>
+      <push-ros-namespace namespace="sensing"/>
+      <arg name="input_vehicle_velocity_topic" default="/vehicle/status/velocity_status"/>
+      <arg name="output_twist_with_covariance" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+      <arg name="config_file" default="$(find-pkg-share vehicle_velocity_converter)/config/vehicle_velocity_converter.param.yaml"/>
+
+      <node pkg="vehicle_velocity_converter" exec="vehicle_velocity_converter_node" output="both">
+        <param from="$(var config_file)"/>
+        <remap from="velocity_status" to="$(var input_vehicle_velocity_topic)"/>
+        <remap from="twist_with_covariance" to="$(var output_twist_with_covariance)"/>
+      </node>
+    </group>
+  </group>
+
   <!-- Dummy doors -->
   <group if="$(var launch_dummy_doors)">
     <include file="$(find-pkg-share vehicle_door_simulator)/launch/vehicle_door_simulator.launch.xml"/>
@@ -162,11 +211,15 @@
   <!-- Simulator model -->
   <group if="$(var launch_dummy_vehicle)">
     <arg name="simulator_model" default="$(var vehicle_model_pkg)/config/simulator_model.param.yaml" description="path to the file of simulator model"/>
+    <!-- 'pose_only' mode publishes the pose topic as an alternative to ndt_localizer. 'full_motion' mode publishes the odometry and acceleration topics as an alternative to localization modules. -->
+    <let name="motion_publish_mode" value="pose_only" if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')"/>
+    <let name="motion_publish_mode" value="full_motion" unless="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')"/>
     <include file="$(find-pkg-share simple_planning_simulator)/launch/simple_planning_simulator.launch.py">
       <arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
       <arg name="simulator_model_param_file" value="$(var simulator_model)"/>
       <arg name="initial_engage_state" value="$(var initial_engage_state)"/>
       <arg name="raw_vehicle_cmd_converter_param_path" value="$(var raw_vehicle_cmd_converter_param_path)"/>
+      <arg name="motion_publish_mode" value="$(var motion_publish_mode)"/>
     </include>
   </group>
 </launch>
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
index 0ed603960a6c5..547689dc847a0 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
@@ -137,7 +137,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   rclcpp::Publisher<TurnIndicatorsReport>::SharedPtr pub_turn_indicators_report_;
   rclcpp::Publisher<HazardLightsReport>::SharedPtr pub_hazard_lights_report_;
   rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
-  rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
+  rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pub_current_pose_;
 
   rclcpp::Subscription<GearCommand>::SharedPtr sub_gear_cmd_;
   rclcpp::Subscription<GearCommand>::SharedPtr sub_manual_gear_cmd_;
@@ -346,6 +346,12 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
    */
   void publish_odometry(const Odometry & odometry);
 
+  /**
+   * @brief publish pose
+   * @param [in] odometry The odometry to publish its pose
+   */
+  void publish_pose(const Odometry & odometry);
+
   /**
    * @brief publish steering
    * @param [in] steer The steering to publish
diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
index afdbeb120e2d3..e6a35bd420bf3 100644
--- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
+++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
@@ -41,6 +41,50 @@ def launch_setup(context, *args, **kwargs):
         param_file=simulator_model_param_path, allow_substs=True
     )
 
+    # Base remappings
+    remappings = [
+        ("input/vector_map", "/map/vector_map"),
+        ("input/initialpose", "/initialpose3d"),
+        ("input/ackermann_control_command", "/control/command/control_cmd"),
+        ("input/actuation_command", "/control/command/actuation_cmd"),
+        ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
+        ("input/gear_command", "/control/command/gear_cmd"),
+        ("input/manual_gear_command", "/vehicle/command/manual_gear_command"),
+        ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
+        ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
+        ("input/trajectory", "/planning/scenario_planning/trajectory"),
+        ("input/engage", "/vehicle/engage"),
+        ("input/control_mode_request", "/control/control_mode_request"),
+        ("output/twist", "/vehicle/status/velocity_status"),
+        ("output/imu", "/sensing/imu/imu_data"),
+        ("output/steering", "/vehicle/status/steering_status"),
+        ("output/gear_report", "/vehicle/status/gear_status"),
+        ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),
+        ("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"),
+        ("output/control_mode_report", "/vehicle/status/control_mode"),
+    ]
+
+    # Additional remappings
+    if LaunchConfiguration("motion_publish_mode").perform(context) == "pose_only":
+        remappings.extend(
+            [
+                ("output/odometry", "/simulation/debug/localization/kinematic_state"),
+                ("output/acceleration", "/simulation/debug/localization/acceleration"),
+                ("output/pose", "/localization/pose_estimator/pose_with_covariance"),
+            ]
+        )
+    elif LaunchConfiguration("motion_publish_mode").perform(context) == "full_motion":
+        remappings.extend(
+            [
+                ("output/odometry", "/localization/kinematic_state"),
+                ("output/acceleration", "/localization/acceleration"),
+                (
+                    "output/pose",
+                    "/simulation/debug/localization/pose_estimator/pose_with_covariance",
+                ),
+            ]
+        )
+
     simple_planning_simulator_node = Node(
         package="simple_planning_simulator",
         executable="simple_planning_simulator_exe",
@@ -55,29 +99,7 @@ def launch_setup(context, *args, **kwargs):
                 "initial_engage_state": LaunchConfiguration("initial_engage_state"),
             },
         ],
-        remappings=[
-            ("input/vector_map", "/map/vector_map"),
-            ("input/initialpose", "/initialpose3d"),
-            ("input/ackermann_control_command", "/control/command/control_cmd"),
-            ("input/actuation_command", "/control/command/actuation_cmd"),
-            ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
-            ("input/gear_command", "/control/command/gear_cmd"),
-            ("input/manual_gear_command", "/vehicle/command/manual_gear_command"),
-            ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
-            ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
-            ("input/trajectory", "/planning/scenario_planning/trajectory"),
-            ("input/engage", "/vehicle/engage"),
-            ("input/control_mode_request", "/control/control_mode_request"),
-            ("output/twist", "/vehicle/status/velocity_status"),
-            ("output/odometry", "/localization/kinematic_state"),
-            ("output/acceleration", "/localization/acceleration"),
-            ("output/imu", "/sensing/imu/imu_data"),
-            ("output/steering", "/vehicle/status/steering_status"),
-            ("output/gear_report", "/vehicle/status/gear_status"),
-            ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),
-            ("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"),
-            ("output/control_mode_report", "/vehicle/status/control_mode"),
-        ],
+        remappings=remappings,
     )
 
     # Determine if we should launch raw_vehicle_cmd_converter based on the vehicle_model_type
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
index f2ccb38843416..f4574174b4cfd 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -165,7 +165,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
     create_publisher<TurnIndicatorsReport>("output/turn_indicators_report", QoS{1});
   pub_hazard_lights_report_ =
     create_publisher<HazardLightsReport>("output/hazard_lights_report", QoS{1});
-  pub_current_pose_ = create_publisher<PoseStamped>("output/debug/pose", QoS{1});
+  pub_current_pose_ = create_publisher<PoseWithCovarianceStamped>("output/pose", QoS{1});
   pub_velocity_ = create_publisher<VelocityReport>("output/twist", QoS{1});
   pub_odom_ = create_publisher<Odometry>("output/odometry", QoS{1});
   pub_steer_ = create_publisher<SteeringReport>("output/steering", QoS{1});
@@ -444,6 +444,7 @@ void SimplePlanningSimulator::on_timer()
 
   // publish vehicle state
   publish_odometry(current_odometry_);
+  publish_pose(current_odometry_);
   publish_velocity(current_velocity_);
   publish_steering(current_steer_);
   publish_acceleration();
@@ -749,6 +750,26 @@ void SimplePlanningSimulator::publish_odometry(const Odometry & odometry)
   pub_odom_->publish(msg);
 }
 
+void SimplePlanningSimulator::publish_pose(const Odometry & odometry)
+{
+  geometry_msgs::msg::PoseWithCovarianceStamped msg;
+
+  msg.pose = odometry.pose;
+  using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+  constexpr auto COV_POS = 0.0225;      // same value as current ndt output
+  constexpr auto COV_ANGLE = 0.000625;  // same value as current ndt output
+  msg.pose.covariance.at(COV_IDX::X_X) = COV_POS;
+  msg.pose.covariance.at(COV_IDX::Y_Y) = COV_POS;
+  msg.pose.covariance.at(COV_IDX::Z_Z) = COV_POS;
+  msg.pose.covariance.at(COV_IDX::ROLL_ROLL) = COV_ANGLE;
+  msg.pose.covariance.at(COV_IDX::PITCH_PITCH) = COV_ANGLE;
+  msg.pose.covariance.at(COV_IDX::YAW_YAW) = COV_ANGLE;
+
+  msg.header.frame_id = origin_frame_id_;
+  msg.header.stamp = get_clock()->now();
+  pub_current_pose_->publish(msg);
+}
+
 void SimplePlanningSimulator::publish_steering(const SteeringReport & steer)
 {
   SteeringReport msg = steer;