Merge branch 'test/mrm0.5' of github.com:tier4/autoware.universe into…

… feat/add-remapper-node

launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml

@@ -3,6 +3,9 @@
   <arg name="launch_deprecated_api" default="false"/>
   <arg name="launch_default_ad_api" default="true"/>
   <arg name="launch_rviz_adaptors" default="true"/>
+  <arg name="launch_ad_api_distributor" default="true"/>
+  <arg name="is_main" if="$(eval &quot;'$(var ecu_name)'=='main'&quot;)" value="true"/>
+  <arg name="is_main" unless="$(eval &quot;'$(var ecu_name)'=='main'&quot;)" value="false"/>
 
   <!-- rosbridge options -->
   <arg name="rosbridge_enabled" default="false"/>
@@ -15,8 +18,13 @@
   </group>
 
   <!-- AD API -->
+  <group if="$(var is_main)">
+    <include file="$(find-pkg-share ad_api_distributor)/launch/distributor.launch.xml" if="$(var launch_ad_api_distributor)"/>
+  </group>
   <group>
-    <include file="$(find-pkg-share default_ad_api)/launch/default_ad_api.launch.py" if="$(var launch_default_ad_api)"/>
+    <include file="$(find-pkg-share default_ad_api)/launch/remapped_api.launch.py" if="$(var launch_default_ad_api)">
+      <arg name="prefix" value="$(var ecu_name)"/>
+    </include>
     <include file="$(find-pkg-share ad_api_adaptors)/launch/rviz_adaptors.launch.xml" if="$(var launch_rviz_adaptors)"/>
   </group>
 

launch/tier4_simulator_launch/launch/simulator_with_vcu_simulation.launch.xml

@@ -0,0 +1,137 @@
+<launch>
+  <!-- Parameter files -->
+  <arg name="fault_injection_param_path"/>
+  <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>
+  <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="pose_initializer_param_path"/>
+
+  <arg name="launch_dummy_perception"/>
+  <arg name="launch_dummy_vehicle"/>
+  <arg name="launch_dummy_localization"/>
+  <arg name="launch_diagnostic_converter"/>
+  <arg name="vehicle_info_param_file"/>
+
+  <arg name="perception/enable_detection_failure" default="true" description="enable to simulate detection failure when using dummy perception"/>
+  <arg name="perception/enable_object_recognition" default="false" description="enable object recognition"/>
+  <arg name="perception/enable_elevation_map" default="false" description="enable elevation map loader"/>
+  <arg name="perception/enable_traffic_light" default="true" description="enable traffic light"/>
+  <arg name="fusion_only" default="true" description="enable only camera and V2X fusion when enabling traffic light"/>
+  <arg name="perception/use_base_link_z" default="true" description="dummy perception uses base_link z axis coordinate if it is true"/>
+  <arg name="sensing/visible_range" default="300.0" description="visible range when using dummy perception"/>
+
+  <arg name="vehicle_model" description="vehicle model name"/>
+  <arg name="initial_engage_state" default="true" description="/vehicle/engage state after starting Autoware"/>
+
+  <let name="vehicle_model_pkg" value="$(find-pkg-share $(var vehicle_model)_description)"/>
+
+  <group if="$(var scenario_simulation)">
+    <include file="$(find-pkg-share fault_injection)/launch/fault_injection.launch.xml">
+      <arg name="config_file" value="$(var fault_injection_param_path)"/>
+    </include>
+  </group>
+
+  <!-- Dummy Perception -->
+  <group if="$(var launch_dummy_perception)">
+    <include file="$(find-pkg-share dummy_perception_publisher)/launch/dummy_perception_publisher.launch.xml">
+      <arg name="real" value="$(var perception/enable_detection_failure)"/>
+      <arg name="use_object_recognition" value="$(var perception/enable_object_recognition)"/>
+      <arg name="use_base_link_z" value="$(var perception/use_base_link_z)"/>
+      <arg name="visible_range" value="$(var sensing/visible_range)"/>
+    </include>
+  </group>
+  <group unless="$(var scenario_simulation)">
+    <!-- Occupancy Grid -->
+    <push-ros-namespace namespace="occupancy_grid_map"/>
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml">
+      <arg name="input_obstacle_pointcloud" value="true"/>
+      <arg name="input_obstacle_and_raw_pointcloud" value="false"/>
+      <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/pointcloud"/>
+      <arg name="output" value="/perception/occupancy_grid_map/map"/>
+      <arg name="occupancy_grid_map_method" value="laserscan_based_occupancy_grid_map"/>
+      <arg name="occupancy_grid_map_param_path" value="$(var laserscan_based_occupancy_grid_map_param_path)"/>
+      <arg name="occupancy_grid_map_updater" value="$(var occupancy_grid_map_updater)"/>
+      <arg name="occupancy_grid_map_updater_param_path" value="$(var occupancy_grid_map_updater_param_path)"/>
+    </include>
+  </group>
+
+  <!-- perception module -->
+  <group>
+    <push-ros-namespace namespace="perception"/>
+    <!-- object recognition -->
+    <group if="$(var perception/enable_object_recognition)">
+      <push-ros-namespace namespace="object_recognition"/>
+      <!-- tracking module -->
+      <group>
+        <push-ros-namespace namespace="tracking"/>
+        <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/tracking/tracking.launch.xml"/>
+      </group>
+      <!-- prediction module -->
+      <group>
+        <push-ros-namespace namespace="prediction"/>
+        <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/prediction/prediction.launch.xml">
+          <arg name="use_vector_map" value="true"/>
+        </include>
+      </group>
+    </group>
+
+    <!-- publish empty objects instead of object recognition module -->
+    <group unless="$(var perception/enable_object_recognition)">
+      <push-ros-namespace namespace="object_recognition"/>
+      <node pkg="dummy_perception_publisher" exec="empty_objects_publisher" name="empty_objects_publisher" output="screen">
+        <remap from="~/output/objects" to="/perception/object_recognition/objects"/>
+      </node>
+    </group>
+
+    <group if="$(var perception/enable_elevation_map)">
+      <push-ros-namespace namespace="obstacle_segmentation/elevation_map"/>
+      <node pkg="elevation_map_loader" exec="elevation_map_loader" name="elevation_map_loader" output="screen">
+        <remap from="output/elevation_map" to="map"/>
+        <remap from="input/pointcloud_map" to="/map/pointcloud_map"/>
+        <remap from="input/vector_map" to="/map/vector_map"/>
+        <param name="use_lane_filter" value="false"/>
+        <param name="use_inpaint" value="true"/>
+        <param name="inpaint_radius" value="1.0"/>
+        <param name="param_file_path" value="$(var obstacle_segmentation_ground_segmentation_elevation_map_param_path)"/>
+        <param name="elevation_map_directory" value="$(find-pkg-share elevation_map_loader)/data/elevation_maps"/>
+        <param name="use_elevation_map_cloud_publisher" value="false"/>
+      </node>
+    </group>
+
+    <!-- traffic light module -->
+    <group if="$(var perception/enable_traffic_light)">
+      <push-ros-namespace namespace="traffic_light_recognition"/>
+      <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light.launch.xml"/>
+    </group>
+  </group>
+
+  <!-- Dummy localization -->
+  <group if="$(var launch_dummy_localization)">
+    <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+      <arg name="ndt_enabled" value="false"/>
+      <arg name="gnss_enabled" value="false"/>
+      <arg name="ekf_enabled" value="false"/>
+      <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>
+
+  <!-- Diagnostic Converter -->
+  <group if="$(var launch_diagnostic_converter)">
+    <node name="diagnostic_converter" exec="diagnostic_converter" pkg="diagnostic_converter" output="screen">
+      <param name="diagnostic_topics" value="[/diagnostic/planning_evaluator/metrics, /diagnostics_err]"/>
+    </node>
+  </group>
+
+  <!-- Simulator model -->
+  <group if="$(var launch_dummy_vehicle)">
+    <arg name="simulator_model" default="$(find-pkg-share vcu_planning_simulator)/config/simulator_model.param.yaml" description="path to the file of simulator model"/>
+    <include file="$(find-pkg-share vcu_planning_simulator)/launch/vcu_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)"/>
+    </include>
+  </group>
+</launch>

launch/tier4_simulator_launch/package.xml

@@ -17,6 +17,7 @@
   <exec_depend>dummy_perception_publisher</exec_depend>
   <exec_depend>fault_injection</exec_depend>
   <exec_depend>simple_planning_simulator</exec_depend>
+  <exec_depend>vcu_planning_simulator</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml

@@ -2,7 +2,8 @@
   ros__parameters:
     simulated_frame_id: "base_link"
     origin_frame_id: "map"
-    vehicle_model_type: "DELAY_STEER_ACC_GEARED"
+    # vehicle_model_type: "DELAY_STEER_ACC_GEARED"
+    vehicle_model_type: "IDEAL_STEER_VEL"
     initialize_source: "INITIAL_POSE_TOPIC"
     timer_sampling_time_ms: 25
     add_measurement_noise: False

simulator/vcu_planning_simulator/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.14)
+project(vcu_planning_simulator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# Component
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  include/vcu_planning_simulator/vcu_planning_simulator_core.hpp
+  include/vcu_planning_simulator/visibility_control.hpp
+  src/vcu_planning_simulator/vcu_planning_simulator_core.cpp
+  src/vcu_planning_simulator/vehicle_model/sim_model_interface.cpp
+  src/vcu_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp
+  src/vcu_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp
+)
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${tf2_INCLUDE_DIRS})
+
+target_compile_options(${PROJECT_NAME} PRIVATE -Wno-old-style-cast) # RCLCPP_ERROR_THROTTLE() has built-in old-style casts.
+
+# Node executable
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "simulation::vcu_planning_simulator::VcuPlanningSimulator"
+  EXECUTABLE ${PROJECT_NAME}_exe
+)
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_vcu_planning_simulator
+    test/test_vcu_planning_simulator.cpp
+    TIMEOUT 120
+  )
+
+  target_link_libraries(test_vcu_planning_simulator
+    ${PROJECT_NAME}
+  )
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE param launch config)

simulator/vcu_planning_simulator/config/simulator_model.param.yaml

@@ -0,0 +1,19 @@
+/**:
+  ros__parameters:
+    simulated_frame_id: "base_link"
+    origin_frame_id: "map"
+    vehicle_model_type: "IDEAL_STEER_VEL"
+    # vehicle_model_type: "DELAY_STEER_ACC_GEARED"
+    initialize_source: "INITIAL_POSE_TOPIC"
+    timer_sampling_time_ms: 25
+    add_measurement_noise: False
+    vel_lim: 50.0
+    vel_rate_lim: 7.0
+    steer_lim: 1.0
+    steer_rate_lim: 5.0
+    acc_time_delay: 0.1
+    acc_time_constant: 0.1
+    steer_time_delay: 0.24
+    steer_time_constant: 0.27
+    x_stddev: 0.0001 # x standard deviation for dummy covariance in map coordinate
+    y_stddev: 0.0001 # y standard deviation for dummy covariance in map coordinate