Merge branch 'main' into fix/cpp17_namespaces

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"/>
@@ -130,7 +135,7 @@
     </group>
   </group>
 
-  <!-- Dummy localization -->
+  <!-- localization -->
   <group if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;none&quot;')">
     <!-- Do nothing -->
   </group>
@@ -148,6 +153,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"/>
@@ -163,11 +212,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>

perception/autoware_multi_object_tracker/config/input_channels.param.yaml

@@ -40,8 +40,8 @@
           name: "apollo"
           short_name: "Lap"
       # LIDAR-CAMERA - DNN
-      # cspell:ignore lidar_pointpainitng pointpainting
-      lidar_pointpainitng:
+      # cspell:ignore lidar_pointpainting pointpainting
+      lidar_pointpainting:
         topic: "/perception/object_recognition/detection/pointpainting/objects"
         can_spawn_new_tracker: true
         optional:

perception/autoware_multi_object_tracker/schema/input_channels.schema.json

@@ -111,7 +111,7 @@
                     }
                   }
                 },
-                "lidar_pointpainitng": {
+                "lidar_pointpainting": {
                   "$ref": "#/definitions/input_channel",
                   "default": {
                     "topic": "/perception/object_recognition/detection/pointpainting/objects",
@@ -185,7 +185,7 @@
                 "lidar_centerpoint_validated",
                 "lidar_apollo",
                 "lidar_apollo_validated",
-                "lidar_pointpainitng",
+                "lidar_pointpainting",
                 "lidar_pointpainting_validated",
                 "camera_lidar_fusion",
                 "detection_by_tracker",

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

@@ -207,7 +207,7 @@ void GoalSearcher::countObjectsToAvoid(
   const double backward_length = parameters_.backward_goal_search_length;
 
   // calculate search start/end pose in pull over lanes
-  const auto [search_start_pose, search_end_pose] = std::invoke([&]() -> std::pair<Pose, Pose> {
+  const auto search_start_end_poses = std::invoke([&]() -> std::pair<Pose, Pose> {
     const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
       *route_handler, left_side_parking_, parameters_.backward_goal_search_length,
       parameters_.forward_goal_search_length);
@@ -221,6 +221,8 @@ void GoalSearcher::countObjectsToAvoid(
     return std::make_pair(
       center_line_path.points.front().point.pose, center_line_path.points.back().point.pose);
   });
+  const auto search_start_pose = std::get<0>(search_start_end_poses);
+  const auto search_end_pose = std::get<1>(search_start_end_poses);
 
   // generate current lane center line path to check collision with objects
   const auto current_lanes = utils::getExtendedCurrentLanes(

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp

@@ -92,8 +92,9 @@ double calcMinimumLaneChangeLength(
   }
 
   const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity;
-  const auto [min_lat_acc, max_lat_acc] =
-    lane_change_parameters.lane_change_lat_acc_map.find(min_vel);
+  const auto min_max_lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(min_vel);
+  // const auto min_lat_acc = std::get<0>(min_max_lat_acc);
+  const auto max_lat_acc = std::get<1>(min_max_lat_acc);
   const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk;
   const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer;
 

sensing/autoware_pointcloud_preprocessor/src/filter.cpp

@@ -359,19 +359,12 @@ bool autoware::pointcloud_preprocessor::Filter::convert_output_costly(
 void autoware::pointcloud_preprocessor::Filter::faster_input_indices_callback(
   const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices)
 {
-  // TODO(knzo25): This first branch is to give temporary compatibility with old perception data.
-  // Should be deleted soon
-  if (utils::is_data_layout_compatible_with_point_xyzi(*cloud)) {
-    RCLCPP_ERROR_THROTTLE(
-      get_logger(), *get_clock(), 10000,
-      "The pointcloud layout is compatible with PointXYZI. You may be using legacy "
-      "code/data. Continue at your own risk");
-  } else if (
+  if (
     !utils::is_data_layout_compatible_with_point_xyzircaedt(*cloud) &&
     !utils::is_data_layout_compatible_with_point_xyzirc(*cloud)) {
     RCLCPP_ERROR(
       get_logger(),
-      "The pointcloud layout is not compatible with PointXYZIRCAEDT/PointXYZIRC. Aborting");
+      "The pointcloud layout is not compatible with PointXYZIRCAEDT or PointXYZIRC. Aborting");
 
     if (utils::is_data_layout_compatible_with_point_xyziradrt(*cloud)) {
       RCLCPP_ERROR(

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

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

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;

vehicle/autoware_accel_brake_map_calibrator/scripts/calc_utils.py

@@ -37,7 +37,7 @@ def get_map_list(y_num, x_num):
 class CalcUtils:
     @staticmethod
     def average_filter(data, average_num):
-        if type(average_num) != int:
+        if not isinstance(average_num, int):
             print(
                 "Error in average_filter(data, average_num):\
                 Type of average_num must be int"