Skip to content

Commit

Permalink
feat(vcu_planning_simulator): add vcu_planning_simulator
Browse files Browse the repository at this point in the history
Squashed commit of the following:

commit cf2e969
Author: TetsuKawa <[email protected]>
Date:   Wed Jan 10 20:28:55 2024 +0900

    feat: add param_path for traking

commit afade45
Author: TetsuKawa <[email protected]>
Date:   Wed Jan 10 15:52:13 2024 +0900

    fix: update how to include tier4_autoware_utils

commit f16cd78
Author: TetsuKawa <[email protected]>
Date:   Fri Dec 15 14:02:01 2023 +0900

    feat: add control report

commit 5722309
Author: TetsuKawa <[email protected]>
Date:   Mon Nov 27 15:14:12 2023 +0900

    feat: vcu_sim publish acceleration

commit d0e4817
Author: TetsuKawa <[email protected]>
Date:   Fri Nov 17 00:10:05 2023 +0900

    feat: fix vcu_planning_simulation path

commit 9f931aa
Author: TetsuKawa <[email protected]>
Date:   Thu Nov 16 22:29:17 2023 +0900

    feat: fix derectory name

commit e95642b
Author: TetsuKawa <[email protected]>
Date:   Thu Nov 16 21:28:57 2023 +0900

    feat: restore to origin vcu_planning_simulator

commit e93f515
Author: TetsuKawa <[email protected]>
Date:   Thu Nov 16 21:10:20 2023 +0900

    fix: add using hadmapbin

commit 8322521
Author: TetsuKawa <[email protected]>
Date:   Thu Nov 16 20:25:17 2023 +0900

    feat: add had_map_bin.hpp

commit 8b2c1e6
Author: TetsuKawa <[email protected]>
Date:   Thu Nov 16 12:33:08 2023 +0900

    feat: add vcu_planning_simulation

Signed-off-by: Makoto Kurihara <[email protected]>
  • Loading branch information
mkuri committed Jan 17, 2024
1 parent 0d5be4b commit 1ca9279
Show file tree
Hide file tree
Showing 20 changed files with 2,140 additions and 1 deletion.
Original file line number Diff line number Diff line change
@@ -0,0 +1,146 @@
<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">
<arg
name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path"
value="$(var object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path)"
/>
<arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path" value="$(var object_recognition_tracking_radar_object_tracker_tracking_setting_param_path)"/>
<arg name="object_recognition_tracking_radar_object_tracker_node_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_param_path)"/>
<arg name="object_recognition_tracking_object_merger_data_association_matrix_param_path" value="$(var object_recognition_tracking_object_merger_data_association_matrix_param_path)"/>
<arg name="object_recognition_tracking_object_merger_node_param_path" value="$(var object_recognition_tracking_object_merger_node_param_path)"/>
</include>
</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>
1 change: 1 addition & 0 deletions launch/tier4_simulator_launch/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
37 changes: 37 additions & 0 deletions simulator/vcu_planning_simulator/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)
19 changes: 19 additions & 0 deletions simulator/vcu_planning_simulator/config/simulator_model.param.yaml
Original file line number Diff line number Diff line change
@@ -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
Loading

0 comments on commit 1ca9279

Please sign in to comment.