Rjd 736/autoware msgs support and localization sim mode support

external/concealer/include/concealer/autoware_universe.hpp

@@ -45,6 +45,7 @@ class AutowareUniverse : public Autoware
   using GearCommand                 = autoware_vehicle_msgs::msg::GearCommand;
   using GearReport                  = autoware_vehicle_msgs::msg::GearReport;
   using PathWithLaneId              = tier4_planning_msgs::msg::PathWithLaneId;
+  using PoseWithCovarianceStamped   = geometry_msgs::msg::PoseWithCovarianceStamped;
   using SteeringReport              = autoware_vehicle_msgs::msg::SteeringReport;
   using TurnIndicatorsCommand       = autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
   using TurnIndicatorsReport        = autoware_vehicle_msgs::msg::TurnIndicatorsReport;
@@ -57,6 +58,7 @@ class AutowareUniverse : public Autoware
 
   PublisherWrapper<AccelWithCovarianceStamped> setAcceleration;
   PublisherWrapper<nav_msgs::msg::Odometry>    setOdometry;
+  PublisherWrapper<PoseWithCovarianceStamped>  setPose;
   PublisherWrapper<SteeringReport>             setSteeringReport;
   PublisherWrapper<GearReport>                 setGearReport;
   PublisherWrapper<ControlModeReport>          setControlModeReport;
@@ -83,7 +85,7 @@ class AutowareUniverse : public Autoware
   auto stopAndJoin() -> void;
 
 public:
-  CONCEALER_PUBLIC explicit AutowareUniverse();
+  CONCEALER_PUBLIC explicit AutowareUniverse(bool);
 
   ~AutowareUniverse();
 

external/concealer/src/autoware_universe.cpp

@@ -16,15 +16,25 @@
 
 namespace concealer
 {
-AutowareUniverse::AutowareUniverse()
+AutowareUniverse::AutowareUniverse(bool simulate_localization)
 : getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this),
   getGearCommandImpl("/control/command/gear_cmd", rclcpp::QoS(1), *this),
   getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this),
   getPathWithLaneId(
     "/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1),
     *this),
-  setAcceleration("/localization/acceleration", *this),
-  setOdometry("/localization/kinematic_state", *this),
+  setAcceleration(
+    simulate_localization ? "/localization/acceleration"
+                          : "/simulation/debug/localization/acceleration",
+    *this),
+  setOdometry(
+    simulate_localization ? "/localization/kinematic_state"
+                          : "/simulation/debug/localization/kinematic_state",
+    *this),
+  setPose(
+    simulate_localization ? "/simulation/debug/localization/pose_estimator/pose_with_covariance"
+                          : "/localization/pose_estimator/pose_with_covariance",
+    *this),
   setSteeringReport("/vehicle/status/steering_status", *this),
   setGearReport("/vehicle/status/gear_status", *this),
   setControlModeReport("/vehicle/status/control_mode", *this),
@@ -121,6 +131,21 @@ auto AutowareUniverse::updateLocalization() -> void
     return message;
   }());
 
+  setPose([this]() {
+    // See https://github.com/tier4/autoware.universe/blob/45ab20af979c5663e5a8d4dda787b1dea8d6e55b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L785-L803
+    geometry_msgs::msg::PoseWithCovarianceStamped message;
+    message.header.stamp = get_clock()->now();
+    message.header.frame_id = "map";
+    message.pose.pose = current_pose.load();
+    message.pose.covariance.at(6 * 0 + 0) = 0.0225;    // XYZRPY_COV_IDX::X_X
+    message.pose.covariance.at(6 * 1 + 1) = 0.0225;    // XYZRPY_COV_IDX::Y_Y
+    message.pose.covariance.at(6 * 2 + 2) = 0.0225;    // XYZRPY_COV_IDX::Z_Z
+    message.pose.covariance.at(6 * 3 + 3) = 0.000625;  // XYZRPY_COV_IDX::ROLL_ROLL
+    message.pose.covariance.at(6 * 4 + 4) = 0.000625;  // XYZRPY_COV_IDX::PITCH_PITCH
+    message.pose.covariance.at(6 * 5 + 5) = 0.000625;  // XYZRPY_COV_IDX::YAW_YAW
+    return message;
+  }());
+
   setTransform(current_pose.load());
 }
 

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -41,7 +41,8 @@ EgoEntitySimulation::EgoEntitySimulation(
   const traffic_simulator_msgs::msg::VehicleParameters & parameters, double step_time,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils,
   const rclcpp::Parameter & use_sim_time, const bool consider_acceleration_by_road_slope)
-: autoware(std::make_unique<concealer::AutowareUniverse>()),
+: autoware(
+    std::make_unique<concealer::AutowareUniverse>(getParameter<bool>("simulate_localization"))),
   vehicle_model_type_(getVehicleModelType()),
   vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)),
   status_(initial_status, std::nullopt),

simulation/traffic_simulator/src/entity/ego_entity.cpp

@@ -54,6 +54,7 @@ auto EgoEntity::makeFieldOperatorApplication(
     parameters.push_back("use_foa:=false");
     parameters.push_back("perception/enable_traffic_light:=" + std::string(architecture_type >= "awf/universe/20230906" ? "true" : "false"));
     parameters.push_back("use_sim_time:=" + std::string(getParameter<bool>(node_parameters, "use_sim_time", false) ? "true" : "false"));
+    parameters.push_back("localization_sim_mode:=" + std::string(getParameter<bool>(node_parameters, "simulate_localization") ? "api" : "pose_twist_estimator"));
     // clang-format on
 
     return getParameter<bool>(node_parameters, "launch_autoware", true)

test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py

@@ -87,6 +87,7 @@ def launch_setup(context, *args, **kwargs):
     scenario                            = LaunchConfiguration("scenario",                               default=Path("/dev/null"))
     sensor_model                        = LaunchConfiguration("sensor_model",                           default="")
     sigterm_timeout                     = LaunchConfiguration("sigterm_timeout",                        default=8)
+    simulate_localization               = LaunchConfiguration("simulate_localization",                  default=True)
     speed_condition                     = LaunchConfiguration("speed_condition",                        default="legacy")
     use_sim_time                        = LaunchConfiguration("use_sim_time",                           default=False)
     vehicle_model                       = LaunchConfiguration("vehicle_model",                          default="")
@@ -112,6 +113,7 @@ def launch_setup(context, *args, **kwargs):
     print(f"scenario                            := {scenario.perform(context)}")
     print(f"sensor_model                        := {sensor_model.perform(context)}")
     print(f"sigterm_timeout                     := {sigterm_timeout.perform(context)}")
+    print(f"simulate_localization               := {simulate_localization.perform(context)}")
     print(f"speed_condition                     := {speed_condition.perform(context)}")
     print(f"use_sim_time                        := {use_sim_time.perform(context)}")
     print(f"vehicle_model                       := {vehicle_model.perform(context)}")
@@ -137,6 +139,7 @@ def make_parameters():
             {"rviz_config": rviz_config},
             {"sensor_model": sensor_model},
             {"sigterm_timeout": sigterm_timeout},
+            {"simulate_localization": simulate_localization},
             {"speed_condition": speed_condition},
             {"use_sim_time": use_sim_time},
             {"vehicle_model": vehicle_model},
@@ -182,6 +185,7 @@ def collect_prefixed_parameters():
         DeclareLaunchArgument("scenario",                            default_value=scenario                           ),
         DeclareLaunchArgument("sensor_model",                        default_value=sensor_model                       ),
         DeclareLaunchArgument("sigterm_timeout",                     default_value=sigterm_timeout                    ),
+        DeclareLaunchArgument("simulate_localization",               default_value=simulate_localization              ),
         DeclareLaunchArgument("speed_condition",                     default_value=speed_condition                      ),
         DeclareLaunchArgument("use_sim_time",                        default_value=use_sim_time                       ),
         DeclareLaunchArgument("vehicle_model",                       default_value=vehicle_model                      ),