diff --git a/build_depends.repos b/build_depends.repos index 0b81f20483..d7b72add90 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -3,3 +3,27 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_msgs.git version: main + autoware_utils: + type: git + url: https://github.com/autowarefoundation/autoware_utils + version: main + autoware_internal_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_internal_msgs + version: main + autoware_adapi_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_adapi_msgs + version: main + autoware_lanelet2_extension: + type: git + url: https://github.com/autowarefoundation/autoware_lanelet2_extension + version: main + autoware_lanelet2_extension: + type: git + url: https://github.com/autowarefoundation/autoware_lanelet2_extension + version: main + autoware_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_msgs + version: main diff --git a/common/autoware_test_utils/CMakeLists.txt b/common/autoware_test_utils/CMakeLists.txt new file mode 100644 index 0000000000..8bfa44f7bf --- /dev/null +++ b/common/autoware_test_utils/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_test_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_test_utils SHARED + src/autoware_test_utils.cpp + src/mock_data_parser.cpp +) + +ament_auto_add_executable(topic_snapshot_saver src/topic_snapshot_saver.cpp) + +target_link_libraries(topic_snapshot_saver autoware_test_utils) + +if(BUILD_TESTING) + ament_auto_add_gtest(test_autoware_test_utils + test/test_mock_data_parser.cpp + test/test_autoware_test_manager.cpp + ) +endif() + +ament_auto_package(INSTALL_TO_SHARE + config + test_map + test_data + launch + rviz +) diff --git a/common/autoware_test_utils/README.md b/common/autoware_test_utils/README.md new file mode 100644 index 0000000000..54b67d279e --- /dev/null +++ b/common/autoware_test_utils/README.md @@ -0,0 +1,106 @@ +# Test Utils + +## Background + +Several Autoware's components and modules have already adopted unit testing, so a common library to ease the process of writing unit tests is necessary. + +## Purpose + +The objective of the `test_utils` is to develop a unit testing library for the Autoware components. This library will include + +- commonly used functions +- input/mock data parser +- maps for testing +- common routes and mock data for testing. + +## Available Maps + +The following maps are available [here](https://github.com/autowarefoundation/autoware.universe/tree/main/common/autoware_test_utils/test_map) + +### Common + +The common map contains multiple types of usable inputs, including shoulder lanes, intersections, and some regulatory elements. The common map is named `lanelet2_map.osm` in the folder. + +![common](./images/common.png) + +### 2 km Straight + +The 2 km straight lanelet map consists of two lanes that run in the same direction. The map is named `2km_test.osm`. + +![two_km](./images/2km-test.png) + +The following illustrates the design of the map. + +![straight_diagram](./images/2km-test.svg) + +### road_shoulders + +The road_shoulders lanelet map consist of a variety of pick-up/drop-off site maps with road_shoulder tags including: + +- pick-up/drop-off sites on the side of street lanes +- pick-up/drop-off sites on the side of curved lanes +- pick-up/drop-off sites inside a private area + +![road_shoulder_test](./images/road_shoulder_test_map.png) + +You can easily launch planning_simulator by + +```bash +ros2 launch autoware_test_utils psim_road_shoulder.launch.xml vehicle_model:=<> sensor_model:=<> use_sim_time:=true +``` + +### intersection + +The intersections lanelet map consist of a variety of intersections including: + +- 4-way crossing with traffic light +- 4-way crossing without traffic light +- T-shape crossing without traffic light +- intersection with a loop +- complicated intersection + +![intersection_test](./images/intersection_test_map.png) + +You can easily launch planning_simulator by + +```bash +ros2 launch autoware_test_utils psim_intersection.launch.xml vehicle_model:=<> sensor_model:=<> use_sim_time:=true +``` + +## Example use cases + +### Autoware Planning Test Manager + +The goal of the [Autoware Planning Test Manager](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_planning_test_manager/) is to test planning module nodes. The `PlanningInterfaceTestManager` class ([source code](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp)) creates wrapper functions based on the `test_utils` functions. + +### Generate test data for unit testing + +As presented in this [PR description](https://github.com/autowarefoundation/autoware.universe/pull/9207), the user can save a snapshot of the scene to a yaml file while running Planning Simulation on the test map. + +```bash +ros2 launch autoware_test_utils psim_road_shoulder.launch.xml +ros2 launch autoware_test_utils psim_intersection.launch.xml +``` + +It uses the autoware `sample_vehicle_description` and `sample_sensor_kit` by default, and `autoware_test_utils/config/test_vehicle_info.param.yaml` is exactly the same as that of `sample_vehicle_description`. If specified, `vehicle_model`/`sensor_model` argument is available. + +```bash +ros2 service call /autoware_test_utils/topic_snapshot_saver std_srvs/srv/Empty \{\} +``` + +The list and field names of the topics to be saved are specified in `config/sample_topic_snapshot.yaml`. + +```yaml +# setting +fields: + - name: self_odometry # this is the field name for this topic + type: Odometry # the abbreviated type name of this topic + topic: /localization/kinematic_state # the name of this topic + +# output +self_odometry: + - header: ... + ... +``` + +Each field can be parsed to ROS message type using the functions defined in `autoware_test_utils/mock_data_parser.hpp` diff --git a/common/autoware_test_utils/config/path_with_lane_id_data.yaml b/common/autoware_test_utils/config/path_with_lane_id_data.yaml new file mode 100644 index 0000000000..3c97143247 --- /dev/null +++ b/common/autoware_test_utils/config/path_with_lane_id_data.yaml @@ -0,0 +1,973 @@ +header: + stamp: + sec: 1681359948 + nanosec: 547141221 + frame_id: map +points: + - point: + pose: + position: + x: 3715.5740795767524 + y: 73720.05420502694 + z: 19.32159271846531 + orientation: + x: -0.00012492665895144978 + y: 0.0005080452893778649 + z: 0.2387835635176333 + w: 0.9710726729123492 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9102 + - point: + pose: + position: + x: 3717.3460091535017 + y: 73720.98171005391 + z: 19.323685436930614 + orientation: + x: -0.0001249266589515537 + y: 0.0005080452893780565 + z: 0.2387835635177357 + w: 0.971072672912324 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9102 + - point: + pose: + position: + x: 3719.11793873025 + y: 73721.90921508087 + z: 19.325778155395916 + orientation: + x: -0.0001249266589531248 + y: 0.000508045289375651 + z: 0.23878356352163355 + w: 0.9710726729113656 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9102 + - point: + pose: + position: + x: 3720.8898683069915 + y: 73722.83672010785 + z: 19.32787087386121 + orientation: + x: -0.000310683506242536 + y: 0.0012634693896696495 + z: 0.23878379678994716 + w: 0.9710718848321361 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9102 + - point: + pose: + position: + x: 3722.6617971144556 + y: 73723.76422660447 + z: 19.333075314637686 + orientation: + x: -0.0003921929665141434 + y: 0.001594941534042793 + z: 0.2387844856659324 + w: 0.9710711980856173 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9102 + - point: + pose: + position: + x: 3724.433724376814 + y: 73724.69173605289 + z: 19.339645155612516 + orientation: + x: -0.00039219245860208326 + y: 0.0015949416581376968 + z: 0.23878417653940906 + w: 0.9710712740991666 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9102 + - point: + pose: + position: + x: 3725.24415 + y: 73725.11595 + z: 19.34265 + orientation: + x: -0.0002959351487701356 + y: 0.0012039266025741496 + z: 0.23870246198693626 + w: 0.9710919614663168 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9102 + - 9540 + - point: + pose: + position: + x: 3726.2057370620514 + y: 73725.61908227473 + z: 19.345340943452 + orientation: + x: -0.00029572536087377183 + y: 0.001203978076825147 + z: 0.2385332600613671 + w: 0.9711335370729391 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9540 + - point: + pose: + position: + x: 3727.9781442476115 + y: 73726.54567429313 + z: 19.35030001398721 + orientation: + x: -0.00021110267262458964 + y: 0.0008599917814071184 + z: 0.23839323765808382 + w: 0.971168306777303 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9540 + - point: + pose: + position: + x: 3729.7508187263084 + y: 73727.47175484717 + z: 19.35384210838946 + orientation: + x: -6.692141519012481e-05 + y: 0.0002732311146433046 + z: 0.23789445221110034 + w: 0.9712909710655518 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9540 + - point: + pose: + position: + x: 3731.5244436268235 + y: 73728.3960138543 + z: 19.35496733715985 + orientation: + x: -1.6242011437904305e-05 + y: 6.641944354783482e-05 + z: 0.23753792578867083 + w: 0.9713782626436884 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9540 + - point: + pose: + position: + x: 3733.2987465610177 + y: 73729.31897056928 + z: 19.355240843151954 + orientation: + x: 0.00018936146763009307 + y: -0.0007793818555571481 + z: 0.236094991504571 + w: 0.9717296494872504 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9540 + - point: + pose: + position: + x: 3735.07578303753 + y: 73730.23665317298 + z: 19.352032616018658 + orientation: + x: 0.0001900917198224421 + y: -0.0007792079910932596 + z: 0.23700434041186835 + w: 0.9715082600400162 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9540 + - point: + pose: + position: + x: 3736.8092500000002 + y: 73731.13595 + z: 19.3489 + orientation: + x: 7.886260021628063e-05 + y: -0.00032055994712992694 + z: 0.23889203668732487 + w: 0.9710460781185373 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9540 + - 9546 + - point: + pose: + position: + x: 3738.6227363153803 + y: 73732.08572376282 + z: 19.34754840033396 + orientation: + x: 7.893340496191734e-05 + y: -0.00032054260392099735 + z: 0.23910646055901144 + w: 0.9709933014912608 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9546 + - point: + pose: + position: + x: 3740.3940486925344 + y: 73733.01440695021 + z: 19.346227927212833 + orientation: + x: 0.00022440522723324796 + y: -0.0009107235004995869 + z: 0.23924725826360474 + w: 0.970958222395973 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9546 + - point: + pose: + position: + x: 3742.1650914887546 + y: 73733.94360413808 + z: 19.342476069385278 + orientation: + x: 0.0002332790846132277 + y: -0.000947163467295005 + z: 0.23914570261323878 + w: 0.9709832034509429 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9546 + - point: + pose: + position: + x: 3743.936328602766 + y: 73734.87243086356 + z: 19.338574191926664 + orientation: + x: 0.000226112916788845 + y: -0.0009199350095595325 + z: 0.23868786320777624 + w: 0.9710958791745709 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9546 + - point: + pose: + position: + x: 3745.7084408140895 + y: 73735.79958689708 + z: 19.334784923102248 + orientation: + x: 0.00022481230257080656 + y: -0.000915542859410791 + z: 0.23846673302914229 + w: 0.9711502090197526 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9546 + - point: + pose: + position: + x: 3747.480975080877 + y: 73736.72593579088 + z: 19.331013956820804 + orientation: + x: 0.00022096547458045463 + y: -0.0009000029138000313 + z: 0.23843520658566603 + w: 0.9711579652298924 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9546 + - point: + pose: + position: + x: 3749.25356949462 + y: 73737.65216958662 + z: 19.327307026665355 + orientation: + x: 0.00021984685012575375 + y: -0.0008955876604564825 + z: 0.23839981306281235 + w: 0.971166658571906 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9546 + - point: + pose: + position: + x: 3751.026231417815 + y: 73738.57827417362 + z: 19.32361831509698 + orientation: + x: 0.00021985229462805398 + y: -0.0008955862317758568 + z: 0.2384057401672238 + w: 0.9711652035805176 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9546 + - point: + pose: + position: + x: 3751.56365 + y: 73738.85905 + z: 19.322499999999998 + orientation: + x: 0.00038211915951996884 + y: -0.0015577432267740116 + z: 0.2382395686330772 + w: 0.9712050943845552 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9546 + - 9178 + - point: + pose: + position: + x: 3752.7991025330516 + y: 73739.50397818141 + z: 19.318029342200663 + orientation: + x: 0.00038170668546860933 + y: -0.0015578234532538037 + z: 0.23798541524130806 + w: 0.971267403766088 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9178 + - point: + pose: + position: + x: 3754.572553718778 + y: 73740.42857046552 + z: 19.31161369413195 + orientation: + x: 0.0003816081805609274 + y: -0.0015577413453910945 + z: 0.2379393079079524 + w: 0.9712787002551749 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9178 + - point: + pose: + position: + x: 3755.1569 + y: 73740.73315 + z: 19.3095 + orientation: + x: 3.3651043495358337e-05 + y: -0.00014983865010364847 + z: 0.21912386053108962 + w: 0.9756970381024725 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 9178 + - 54 + - point: + pose: + position: + x: 3756.3691634450834 + y: 73741.30657670146 + z: 19.30908810917335 + orientation: + x: 2.756305989731722e-05 + y: -0.00015030185318417783 + z: 0.18037674543436244 + w: 0.9835975835452921 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 54 + - point: + pose: + position: + x: 3758.2390212156342 + y: 73742.0162495662 + z: 19.30847687576686 + orientation: + x: -0.0004360373565484762 + y: 0.004088265926675131 + z: 0.10605342127152705 + w: 0.994351933567658 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 54 + - point: + pose: + position: + x: 3760.194122023701 + y: 73742.43809400384 + z: 19.324923869837736 + orientation: + x: 8.681609276137115e-05 + y: 0.005224211276852721 + z: -0.016615507728450753 + w: 0.9998483009849177 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 54 + - point: + pose: + position: + x: 3762.1929591494604 + y: 73742.37164218727 + z: 19.345823843919984 + orientation: + x: 0.0001531786687225762 + y: 0.0015359491755148644 + z: -0.09923660060229327 + w: 0.9950626686281255 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 54 + - point: + pose: + position: + x: 3764.1535192009887 + y: 73741.9766644188 + z: 19.35199798787947 + orientation: + x: 0.00017226521914067523 + y: 0.000878641135546086 + z: -0.19239571655372437 + w: 0.9813170163439683 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 54 + - point: + pose: + position: + x: 3766.005183624521 + y: 73741.22156956742 + z: 19.355578943862316 + orientation: + x: -0.0017237648178610864 + y: -0.006155496494888137 + z: -0.26965717149460783 + w: 0.9629351734978226 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 54 + - point: + pose: + position: + x: 3767.7139130315963 + y: 73740.18311995531 + z: 19.33001414139043 + orientation: + x: -0.0020068542186831096 + y: -0.00475671285010973 + z: -0.38871439637880045 + w: 0.921343836071459 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 54 + - point: + pose: + position: + x: 3769.110057328236 + y: 73738.74995264222 + z: 19.309354169085896 + orientation: + x: 0.001897599368611848 + y: 0.00354829845105058 + z: -0.4715851065133287 + w: 0.8818113721253265 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 54 + - point: + pose: + position: + x: 3770.2207264862286 + y: 73737.08614935388 + z: 19.325453604588517 + orientation: + x: 0.002207949729174236 + y: 0.003771770275607204 + z: -0.5051887257323875 + w: 0.8629978274015921 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 54 + - point: + pose: + position: + x: 3770.9794 + y: 73735.73485000001 + z: 19.339 + orientation: + x: 0.0012987300905163237 + y: 0.002139902333252243 + z: -0.5188316266762862 + w: 0.8548727842659366 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 54 + - 112 + - point: + pose: + position: + x: 3771.187290146968 + y: 73735.33535985528 + z: 19.341254605687745 + orientation: + x: 0.0012987405080321385 + y: 0.002139897912591106 + z: -0.5188354513887092 + w: 0.8548704629897391 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 112 + - point: + pose: + position: + x: 3772.1105224970565 + y: 73733.56120032944 + z: 19.351267403708185 + orientation: + x: 0.001284637221584647 + y: 0.002116691463527833 + z: -0.5188299089220646 + w: 0.8548739058670461 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 112 + - point: + pose: + position: + x: 3773.0337779331385 + y: 73731.78705297508 + z: 19.361171574493966 + orientation: + x: 0.001142131909105283 + y: 0.0018819208009395528 + z: -0.5188232398247702 + w: 0.8548787046865363 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 112 + - point: + pose: + position: + x: 3773.9570625659217 + y: 73730.01292049112 + z: 19.36997717485808 + orientation: + x: 0.0011373590375455824 + y: 0.0018740521620074502 + z: -0.5188241107734224 + w: 0.8548781997589371 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 112 + - point: + pose: + position: + x: 3774.88034355803 + y: 73728.2387862816 + z: 19.378745961639066 + orientation: + x: 0.001085865951009278 + y: 0.001760577504405141 + z: -0.5249493938024202 + w: 0.851130927183626 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 112 + - point: + pose: + position: + x: 3775.7780381215507 + y: 73726.45160236803 + z: 19.387019936812187 + orientation: + x: 0.001097732717327821 + y: 0.0017477932337268103 + z: -0.5318646975336433 + w: 0.8468268321912925 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 112 + - point: + pose: + position: + x: 3776.646678677668 + y: 73724.64966197452 + z: 19.39527727531804 + orientation: + x: 0.001092720497131852 + y: 0.0017513843126196664 + z: -0.5293376353561939 + w: 0.8484087496074152 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 112 + - point: + pose: + position: + x: 3777.52588447988 + y: 73722.85326761093 + z: 19.403534613823894 + orientation: + x: 0.0011058496923820054 + y: 0.001836501024357326 + z: -0.515848443146248 + w: 0.8566771784424034 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 112 + - point: + pose: + position: + x: 3778.362060546875 + y: 73721.2734375 + z: 19.411198429353448 + orientation: + x: 0.0011058496923820054 + y: 0.001836501024357326 + z: -0.515848443146248 + w: 0.8566771784424034 + longitudinal_velocity_mps: 0.0 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 112 +left_bound: + - x: 3714.4453347666704 + y: 73721.13700770421 + z: 19.284887409033047 + - x: 3722.9174 + y: 73725.5562 + z: 19.297 + - x: 3724.5488 + y: 73726.3942 + z: 19.2833 + - x: 3728.6133 + y: 73728.5323 + z: 19.308 + - x: 3729.2336 + y: 73728.8553 + z: 19.306 + - x: 3733.699 + y: 73731.1885 + z: 19.317 + - x: 3736.1127 + y: 73732.4548 + z: 19.3088 + - x: 3738.1619 + y: 73733.5217 + z: 19.317 + - x: 3738.7847 + y: 73733.8477 + z: 19.313 + - x: 3744.0873 + y: 73736.6186 + z: 19.3 + - x: 3750.9282 + y: 73740.1584 + z: 19.2908 + - x: 3754.4655 + y: 73742.0426 + z: 19.278 + - x: 3756.4876 + y: 73743.096 + z: 19.255 + - x: 3757.8421 + y: 73743.3924 + z: 19.275 + - x: 3758.958 + y: 73743.6452 + z: 19.294 + - x: 3760.0928 + y: 73743.7931 + z: 19.315 + - x: 3761.2364 + y: 73743.8299 + z: 19.333 + - x: 3762.3789 + y: 73743.765 + z: 19.353 + - x: 3763.5102 + y: 73743.5894 + z: 19.373 + - x: 3764.6203 + y: 73743.3092 + z: 19.373 + - x: 3765.6991 + y: 73742.9276 + z: 19.371 + - x: 3766.7368 + y: 73742.4448 + z: 19.369 + - x: 3767.7258 + y: 73741.8701 + z: 19.347 + - x: 3768.6561 + y: 73741.2005 + z: 19.331 + - x: 3769.5179 + y: 73740.4516 + z: 19.318 + - x: 3770.3086 + y: 73739.6233 + z: 19.307 + - x: 3771.0112 + y: 73738.759 + z: 19.293 + - x: 3771.2311 + y: 73738.47 + z: 19.29 + - x: 3772.1544 + y: 73736.6942 + z: 19.304 + - x: 3772.2992 + y: 73736.4122 + z: 19.305 + - x: 3774.9144 + y: 73731.3856 + z: 19.338 + - x: 3777.6768 + y: 73726.0769 + z: 19.363 + - x: 3777.9518 + y: 73725.547 + z: 19.364 + - x: 3781.0066 + y: 73719.6805 + z: 19.39 + - x: 3782.04730357663 + y: 73717.67915221369 + z: 19.400229050291234 +right_bound: + - x: 3715.820601233884 + y: 73718.50963718728 + z: 19.357252159643334 + - x: 3723.0499 + y: 73722.3069 + z: 19.364 + - x: 3725.9395 + y: 73723.8377 + z: 19.402 + - x: 3736.3301 + y: 73729.2369 + z: 19.397 + - x: 3737.5058 + y: 73729.8171 + z: 19.389 + - x: 3745.1879 + y: 73733.861 + z: 19.374 + - x: 3752.1991 + y: 73737.5597 + z: 19.3542 + - x: 3755.8483 + y: 73739.4237 + z: 19.341 + - x: 3757.8428 + y: 73740.465 + z: 19.337 + - x: 3758.2933 + y: 73740.6265 + z: 19.335 + - x: 3759.1305 + y: 73740.8454 + z: 19.331 + - x: 3759.9869 + y: 73740.9839 + z: 19.336 + - x: 3760.8525 + y: 73741.0423 + z: 19.338 + - x: 3761.7197 + y: 73741.0205 + z: 19.339 + - x: 3762.581 + y: 73740.9156 + z: 19.339 + - x: 3763.4288 + y: 73740.7307 + z: 19.337 + - x: 3764.2558 + y: 73740.4659 + z: 19.34 + - x: 3765.0543 + y: 73740.1274 + z: 19.343 + - x: 3765.8145 + y: 73739.7124 + z: 19.344 + - x: 3766.5362 + y: 73739.23 + z: 19.335 + - x: 3767.2071 + y: 73738.6772 + z: 19.32 + - x: 3767.8247 + y: 73738.0696 + z: 19.305 + - x: 3768.3815 + y: 73737.4041 + z: 19.289 + - x: 3768.5939 + y: 73737.106 + z: 19.287 + - x: 3769.6596 + y: 73735.0575 + z: 19.373 + - x: 3772.4147 + y: 73729.7643 + z: 19.398 + - x: 3775.1698 + y: 73724.4711 + z: 19.422 + - x: 3776.7831 + y: 73721.3721 + z: 19.438 + - x: 3779.2263 + y: 73716.6739 + z: 19.466 + - x: 3779.4158436325565 + y: 73716.30980852067 + z: 19.467859942424305 diff --git a/common/autoware_test_utils/config/sample_topic_snapshot.yaml b/common/autoware_test_utils/config/sample_topic_snapshot.yaml new file mode 100644 index 0000000000..8122d6e5f4 --- /dev/null +++ b/common/autoware_test_utils/config/sample_topic_snapshot.yaml @@ -0,0 +1,32 @@ +format_version: 1 + +fields: + - name: self_odometry + type: Odometry # nav_msgs::msg::Odometry + topic: /localization/kinematic_state + + - name: self_acceleration + type: AccelWithCovarianceStamped # geometry_msgs::msg::AccelWithCovarianceStamped + topic: /localization/acceleration + + - name: operation_mode + type: OperationModeState # autoware_adapi_v1_msgs::msg::OperationModeState + topic: /system/operation_mode/state + + - name: route + type: LaneletRoute # autoware_planning_msgs::msg::LaneletRoute + topic: /planning/mission_planning/route + + - name: traffic_signal + type: TrafficLightGroupArray # autoware_perception_msgs::msg::TrafficLightGroupArray + topic: /perception/traffic_light_recognition/traffic_signals + + - name: dynamic_object + type: PredictedObjects # autoware_perception_msgs::msg::PredictedObjects + topic: /perception/object_recognition/objects + # - name: tracked_object + # type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects + # topic: /perception/object_recognition/tracking/objects + # - name: path_with_lane_id + # type: PathWithLaneId # autoware_internal_planning_msgs::msg::PathWithLaneId + # topic: /planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id diff --git a/common/autoware_test_utils/config/test_common.param.yaml b/common/autoware_test_utils/config/test_common.param.yaml new file mode 100644 index 0000000000..6c547c8cd8 --- /dev/null +++ b/common/autoware_test_utils/config/test_common.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + # constraints param for normal driving + max_vel: 11.1 # max velocity limit [m/s] + + normal: + min_acc: -1.0 # min deceleration [m/ss] + max_acc: 1.0 # max acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + # constraints to be observed + limit: + min_acc: -2.5 # min deceleration limit [m/ss] + max_acc: 1.0 # max acceleration limit [m/ss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/common/autoware_test_utils/config/test_nearest_search.param.yaml b/common/autoware_test_utils/config/test_nearest_search.param.yaml new file mode 100644 index 0000000000..eb6709764c --- /dev/null +++ b/common/autoware_test_utils/config/test_nearest_search.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + # ego + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/common/autoware_test_utils/config/test_vehicle_info.param.yaml b/common/autoware_test_utils/config/test_vehicle_info.param.yaml new file mode 100644 index 0000000000..185ecb9e50 --- /dev/null +++ b/common/autoware_test_utils/config/test_vehicle_info.param.yaml @@ -0,0 +1,13 @@ +# NOTE: this configuration is exactly the same as that of sample_vehicle_description +/**: + ros__parameters: + wheel_radius: 0.383 # The radius of the wheel, primarily used for dead reckoning. + wheel_width: 0.235 # The lateral width of a wheel tire, primarily used for dead reckoning. + wheel_base: 2.79 # between front wheel center and rear wheel center + wheel_tread: 1.64 # between left wheel center and right wheel center + front_overhang: 1.0 # between front wheel center and vehicle front + rear_overhang: 1.1 # between rear wheel center and vehicle rear + left_overhang: 0.128 # between left wheel center and vehicle left + right_overhang: 0.128 # between right wheel center and vehicle right + vehicle_height: 2.5 + max_steer_angle: 0.70 # [rad] diff --git a/common/autoware_test_utils/images/2km-test.png b/common/autoware_test_utils/images/2km-test.png new file mode 100644 index 0000000000..297dc5a438 Binary files /dev/null and b/common/autoware_test_utils/images/2km-test.png differ diff --git a/common/autoware_test_utils/images/2km-test.svg b/common/autoware_test_utils/images/2km-test.svg new file mode 100644 index 0000000000..26632cdde9 --- /dev/null +++ b/common/autoware_test_utils/images/2km-test.svg @@ -0,0 +1,505 @@ + + + + + + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 3.5 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2000.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 1.75 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (0.0, 0.0) +
+
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ 1000.0 meter +
+
+
+ + + + + + + + + + + + + + + + + +
+
+
+ 7.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + +
+
+
+ pose +
+ (1000.0, -3.5) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (1000.0, 3.5) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (-1000.0, 3.5) +
+
+
+
+
+ + + + + + + diff --git a/common/autoware_test_utils/images/common.png b/common/autoware_test_utils/images/common.png new file mode 100644 index 0000000000..340437b53c Binary files /dev/null and b/common/autoware_test_utils/images/common.png differ diff --git a/common/autoware_test_utils/images/intersection_test_map.png b/common/autoware_test_utils/images/intersection_test_map.png new file mode 100644 index 0000000000..a2f3db6ad2 Binary files /dev/null and b/common/autoware_test_utils/images/intersection_test_map.png differ diff --git a/common/autoware_test_utils/images/road_shoulder_test_map.png b/common/autoware_test_utils/images/road_shoulder_test_map.png new file mode 100644 index 0000000000..0815bd9353 Binary files /dev/null and b/common/autoware_test_utils/images/road_shoulder_test_map.png differ diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp new file mode 100644 index 0000000000..a15fb1fc77 --- /dev/null +++ b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp @@ -0,0 +1,651 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_ +#define AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::test_utils +{ +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::LaneletPrimitive; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::Trajectory; +using RouteSections = std::vector; +using autoware_internal_planning_msgs::msg::Scenario; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using nav_msgs::msg::OccupancyGrid; +using nav_msgs::msg::Odometry; +using tf2_msgs::msg::TFMessage; + +/** + * @brief Creates a Pose message with the specified position and orientation. + * + * This function initializes a geometry_msgs::msg::Pose message with the + * given position (x, y, z) and orientation (roll, pitch, yaw). + * + * @param x The x-coordinate of the position. + * @param y The y-coordinate of the position. + * @param z The z-coordinate of the position. + * @param roll The roll component of the orientation (in radians). + * @param pitch The pitch component of the orientation (in radians). + * @param yaw The yaw component of the orientation (in radians). + * @return A geometry_msgs::msg::Pose message with the specified position + * and orientation. + */ +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw); + +/** + * @brief Creates a Pose message from a 4-element array representing a 3D pose. + * + * This function initializes a geometry_msgs::msg::Pose message using a + * std::array of four doubles, where the first three elements represent the + * position (x, y, z) and the fourth element represents the yaw orientation. + * The roll and pitch orientations are assumed to be zero. + * + * @param pose3d A std::array of four doubles representing the 3D pose. The + * first element is the x-coordinate, the second is the y-coordinate, the + * third is the z-coordinate, and the fourth is the yaw orientation. + * @return A geometry_msgs::msg::Pose message with the specified position + * and yaw orientation, with roll and pitch set to zero. + */ +geometry_msgs::msg::Pose createPose(const std::array & pose3d); + +/** + * @brief Creates a LaneletSegment with the specified ID. + * + * Initializes a LaneletSegment containing a single LaneletPrimitive with the + * given ID and a primitive type of "lane". + * + * @param id The ID for the LaneletPrimitive and preferred primitive. + * @return A LaneletSegment with the specified ID. + */ +LaneletSegment createLaneletSegment(int id); + +/** + * @brief Loads a Lanelet map from a specified file. + * + * This function loads a Lanelet2 map using the given filename. It uses the MGRS + * projector and checks for any errors during the loading process. If errors + * are found, they are logged, and the function returns nullptr. + * + * @param lanelet2_filename The filename of the Lanelet2 map to load. + * @return A LaneletMapPtr to the loaded map, or nullptr if there were errors. + */ +lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename); + +/** + * @brief Converts a Lanelet map to a LaneletMapBin message. + * + * This function converts a given Lanelet map to a LaneletMapBin message. It also + * parses the format and map versions from the specified filename and includes + * them in the message. The timestamp for the message is set to the provided time. + * + * @param map The Lanelet map to convert. + * @param lanelet2_filename The filename of the Lanelet2 map, used to parse format and map versions. + * @param now The current time to set in the message header. + * @return A LaneletMapBin message containing the converted map data. + */ +LaneletMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, + const rclcpp::Time & now); + +/** + * @brief Creates a normal Lanelet route with predefined start and goal poses. + * + * This function initializes a LaneletRoute with a predefined start and goal pose. + * + * @return A LaneletRoute with the specified start and goal poses. + */ +LaneletRoute makeNormalRoute(); + +/** + * @brief Creates an OccupancyGrid message with the specified dimensions and resolution. + * + * This function initializes an OccupancyGrid message with given width, height, and resolution. + * All cells in the grid are initialized to zero. + * + * @param width The width of the occupancy grid. + * @param height The height of the occupancy grid. + * @param resolution The resolution of the occupancy grid. + * @return An OccupancyGrid message with the specified dimensions and resolution. + */ +OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2); + +/** + * @brief Get the absolute path to the lanelet map file. + * + * This function retrieves the absolute path to a lanelet map file located in the + * package's share directory under the "test_map" folder. + * + * @param package_name The name of the package containing the map file. + * @param map_filename The name of the map file. + * @return A string representing the absolute path to the lanelet map file. + */ +std::string get_absolute_path_to_lanelet_map( + const std::string & package_name, const std::string & map_filename); + +/** + * @brief Get the absolute path to the route file. + * + * This function retrieves the absolute path to a route file located in the + * package's share directory under the "test_route" folder. + * + * @param package_name The name of the package containing the route file. + * @param route_filename The name of the route file. + * @return A string representing the absolute path to the route file. + */ +std::string get_absolute_path_to_route( + const std::string & package_name, const std::string & route_filename); + +/** + * @brief Get the absolute path to the config file. + * + * This function retrieves the absolute path to a route file located in the + * package's share directory under the "config" folder. + * + * @param package_name The name of the package containing the route file. + * @param route_filename The name of the config file. + * @return A string representing the absolute path to the config file. + */ +std::string get_absolute_path_to_config( + const std::string & package_name, const std::string & config_filename); + +/** + * @brief Creates a LaneletMapBin message from a Lanelet map file. + * + * This function loads a Lanelet map from the given file, overwrites the + * centerline with the specified resolution, and converts the map to a LaneletMapBin message. + * + * @param absolute_path The absolute path to the Lanelet2 map file. + * @param center_line_resolution The resolution for the centerline. + * @return A LaneletMapBin message containing the map data. + */ +LaneletMapBin make_map_bin_msg( + const std::string & absolute_path, const double center_line_resolution = 5.0); + +/** + * @brief Creates a LaneletMapBin message using a Lanelet2 map file. + * + * This function loads a specified map file from the test_map folder in the + * specified package (or autoware_test_utils if no package is specified), + * overwrites the centerline with a resolution of 5.0, + * and converts the map to a LaneletMapBin message. + * + * @param package_name The name of the package containing the map file. If empty, defaults to + * "autoware_test_utils". + * @param map_filename The name of the map file (e.g. "lanelet2_map.osm") + * @return A LaneletMapBin message containing the map data. + */ +LaneletMapBin makeMapBinMsg( + const std::string & package_name = "autoware_test_utils", + const std::string & map_filename = "lanelet2_map.osm"); + +/** + * @brief Creates an Odometry message with a specified shift. + * + * This function initializes an Odometry message with a pose shifted by the given amount at y + * direction. x pose, z pose, and yaw angle remains zero. + * + * @param shift The amount by which to shift the pose. + * @return An Odometry message with the specified shift. + */ +Odometry makeOdometry(const double shift = 0.0); + +/** + * @brief Creates an initial Odometry message with a specified shift. + * + * This function initializes an Odometry message with a pose shifted by the given amount, + * accounting for a specific yaw angle. + * + * @param shift The amount by which to shift the pose. + * @return An Odometry message with the specified shift. + */ +Odometry makeInitialPose(const double shift = 0.0); + +/** + * @brief Creates a TFMessage with the specified frame IDs. + * + * This function initializes a TFMessage with a transform containing the given frame IDs. + * The transform includes a specific translation and rotation. + * + * @param target_node The node to use for obtaining the current time. + * @param frame_id The ID of the parent frame. + * @param child_frame_id The ID of the child frame. + * @return A TFMessage containing the transform. + */ +TFMessage makeTFMsg( + rclcpp::Node::SharedPtr target_node, std::string && frame_id = "", + std::string && child_frame_id = ""); + +/** + * @brief Creates a Scenario message with the specified scenario. + * + * This function initializes a Scenario message with the current scenario and a list of activating + * scenarios. + * + * @param scenario The name of the current scenario. + * @return A Scenario message with the specified scenario. + */ +Scenario makeScenarioMsg(const std::string & scenario); + +/** + * @brief Combines two sets of consecutive RouteSections. + * + * This function combines two sets of RouteSections, removing the overlapping end section from the + * first set. + * + * @param route_sections1 The first set of RouteSections. + * @param route_sections2 The second set of RouteSections. + * @return A combined set of RouteSections. + */ +RouteSections combineConsecutiveRouteSections( + const RouteSections & route_sections1, const RouteSections & route_sections2); + +/** + * @brief Creates a predefined behavior Lanelet route. + * + * This function initializes a LaneletRoute with predefined start and goal poses, + * a list of lanelet segment IDs, and a fixed UUID. + * this is for the test lanelet2_map.osm + * file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 + * + * @return A LaneletRoute with the specified attributes. + */ +LaneletRoute makeBehaviorNormalRoute(); + +/** + * @brief Spins multiple ROS nodes a specified number of times. + * + * This function spins the given test and target nodes for the specified number of iterations. + * + * @param test_node The test node to spin. + * @param target_node The target node to spin. + * @param repeat_count The number of times to spin the nodes. + */ +void spinSomeNodes( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, + const int repeat_count = 1); + +/** + * @brief Updates node options with parameter files. + * + * This function updates the given node options to include the specified parameter files. + * + * @param node_options The node options to update. + * @param params_files A vector of parameter file paths to add to the node options. + */ +void updateNodeOptions( + rclcpp::NodeOptions & node_options, const std::vector & params_files); + +/** + * @brief Loads a PathWithLaneId message from a YAML file. + * + * This function loads a PathWithLaneId message from a YAML file located in the + * autoware_test_utils package. + * + * @return A PathWithLaneId message containing the loaded data. + */ +PathWithLaneId loadPathWithLaneIdInYaml(); + +/** + * @brief create a straight lanelet from 2 segments defined by 4 points + * @param [in] left0 start of the left segment + * @param [in] left1 end of the left segment + * @param [in] right0 start of the right segment + * @param [in] right1 end of the right segment + * @return a ConstLanelet with the given left and right bounds and a unique lanelet id + * + */ +lanelet::ConstLanelet make_lanelet( + const lanelet::BasicPoint2d & left0, const lanelet::BasicPoint2d & left1, + const lanelet::BasicPoint2d & right0, const lanelet::BasicPoint2d & right1); + +/** + * @brief Generates a trajectory with specified parameters. + * + * This function generates a trajectory of type T with a given number of points, + * point interval, velocity, initial theta, delta theta, and optionally an + * overlapping point index. + * + * @tparam T The type of the trajectory. + * @param num_points The number of points in the trajectory. + * @param point_interval The distance between consecutive points. + * @param velocity The longitudinal velocity for each point. + * @param init_theta The initial theta angle. + * @param delta_theta The change in theta per point. + * @param overlapping_point_index The index of the point to overlap (default is max size_t value). + * @return A trajectory of type T with the specified parameters. + */ +template +T generateTrajectory( + const size_t num_points, const double point_interval, const double velocity = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0, + const size_t overlapping_point_index = std::numeric_limits::max()) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + static_cast(i) * delta_theta; + const double x = static_cast(i) * point_interval * std::cos(theta); + const double y = static_cast(i) * point_interval * std::sin(theta); + + Point p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = velocity; + traj.points.push_back(p); + + if (i == overlapping_point_index) { + Point value_to_insert = traj.points[overlapping_point_index]; + traj.points.insert(traj.points.begin() + overlapping_point_index + 1, value_to_insert); + } + } + + return traj; +} + +template <> +inline PathWithLaneId generateTrajectory( + const size_t num_points, const double point_interval, const double velocity, + const double init_theta, const double delta_theta, const size_t overlapping_point_index) +{ + PathWithLaneId traj; + + for (size_t i = 0; i < num_points; i++) { + const double theta = init_theta + static_cast(i) * delta_theta; + const double x = static_cast(i) * point_interval * std::cos(theta); + const double y = static_cast(i) * point_interval * std::sin(theta); + + PathPointWithLaneId p; + p.point.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.point.longitudinal_velocity_mps = static_cast(velocity); + p.lane_ids.push_back(static_cast(i)); + traj.points.push_back(p); + + if (i == overlapping_point_index) { + auto value_to_insert = traj.points.at(overlapping_point_index); + traj.points.insert( + traj.points.begin() + static_cast(overlapping_point_index) + 1, value_to_insert); + } + } + return traj; +} + +/** + * @brief Creates a publisher with appropriate QoS settings. + * + * This function creates a publisher for a given topic name and message type with appropriate + * QoS settings, depending on the message type. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param topic_name The name of the topic to publish to. + * @param publisher A reference to the publisher to be created. + */ +template +void createPublisherWithQoS( + rclcpp::Node::SharedPtr test_node, std::string topic_name, + std::shared_ptr> & publisher) +{ + // override QoS settings for specific message types + if constexpr ( + std::is_same_v || std::is_same_v || + std::is_same_v) { + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.reliable(); + qos.transient_local(); + publisher = rclcpp::create_publisher(test_node, topic_name, qos); + } else { + publisher = rclcpp::create_publisher(test_node, topic_name, 1); + } +} + +/** + * @brief Sets up a publisher for a given topic. + * + * This function sets up a publisher for a given topic using createPublisherWithQoS. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param topic_name The name of the topic to publish to. + * @param publisher A reference to the publisher to be set. + */ +template +void setPublisher( + rclcpp::Node::SharedPtr test_node, std::string topic_name, + std::shared_ptr> & publisher) +{ + createPublisherWithQoS(test_node, topic_name, publisher); +} + +/** + * @brief Creates a subscription with appropriate QoS settings. + * + * This function creates a subscription for a given topic name and message type with appropriate + * QoS settings, depending on the message type. + * + * @tparam T The type of the message to subscribe to. + * @param test_node The node to create the subscription on. + * @param topic_name The name of the topic to subscribe to. + * @param callback The callback function to call when a message is received. + * @param subscriber A reference to the subscription to be created. + * @param qos The QoS settings for the subscription (optional). + */ +template +void createSubscription( + rclcpp::Node::SharedPtr test_node, const std::string & topic_name, + std::function callback, + std::shared_ptr> & subscriber, + std::optional qos = std::nullopt) +{ + if (!qos.has_value()) { + if constexpr (std::is_same_v) { + qos = rclcpp::QoS{1}; + } else { + qos = rclcpp::QoS{10}; + } + } + subscriber = test_node->create_subscription(topic_name, *qos, callback); +} + +/** + * @brief Sets up a subscriber for a given topic. + * + * This function sets up a subscriber for a given topic using createSubscription. + * + * @tparam T The type of the message to subscribe to. + * @param test_node The node to create the subscription on. + * @param topic_name The name of the topic to subscribe to. + * @param subscriber A reference to the subscription to be set. + * @param count A reference to a counter that increments on message receipt. + * @param qos The QoS settings for the subscription (optional). + */ +template +void setSubscriber( + rclcpp::Node::SharedPtr test_node, std::string topic_name, + std::shared_ptr> & subscriber, size_t & count, + std::optional qos = std::nullopt) +{ + createSubscription( + test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber, + qos); +} + +/** + * @brief Publishes data to a target node. + * + * This function publishes data to a target node on a given topic, ensuring that the topic has + * subscribers and retrying a specified number of times. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param target_node The target node to publish the message to. + * @param topic_name The name of the topic to publish to. + * @param publisher A shared pointer to the publisher. + * @param data The data to publish. + * @param repeat_count The number of times to retry publishing. + */ +template +void publishToTargetNode( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, std::string topic_name, + typename rclcpp::Publisher::SharedPtr publisher, T data, const int repeat_count = 3) +{ + if (topic_name.empty()) { + int status = 1; + char * demangled_name = abi::__cxa_demangle(typeid(data).name(), nullptr, nullptr, &status); + if (status == 0) { + throw std::runtime_error(std::string("Topic name for ") + demangled_name + " is empty"); + } + throw std::runtime_error(std::string("Topic name for ") + typeid(data).name() + " is empty"); + } + + autoware::test_utils::setPublisher(test_node, topic_name, publisher); + publisher->publish(data); + + if (target_node->count_subscribers(topic_name) == 0) { + throw std::runtime_error("No subscriber for " + topic_name); + } + autoware::test_utils::spinSomeNodes(test_node, target_node, repeat_count); +} + +/** + * @brief Manages publishing and subscribing to ROS topics for testing Autoware. + * + * The AutowareTestManager class provides utility functions to facilitate + * the publishing of messages to specified topics and the setting up of + * subscribers to listen for messages on specified topics. This class + * simplifies the setup of test environments in Autoware. + */ +class AutowareTestManager +{ +public: + AutowareTestManager() + { + test_node_ = std::make_shared("autoware_test_manager_node"); + pub_clock_ = test_node_->create_publisher("/clock", 1); + } + + template + void test_pub_msg( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name, MessageType & msg) + { + rclcpp::QoS qos(rclcpp::KeepLast(10)); + test_pub_msg(target_node, topic_name, msg, qos); + } + + template + void test_pub_msg( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name, MessageType & msg, + rclcpp::QoS qos) + { + if (publishers_.find(topic_name) == publishers_.end()) { + auto publisher = test_node_->create_publisher(topic_name, qos); + publishers_[topic_name] = std::static_pointer_cast(publisher); + } + + auto publisher = + std::dynamic_pointer_cast>(publishers_[topic_name]); + + publisher->publish(msg); + const int repeat_count = 3; + autoware::test_utils::spinSomeNodes(test_node_, target_node, repeat_count); + RCLCPP_INFO(test_node_->get_logger(), "Published message on topic '%s'", topic_name.c_str()); + } + + template + void set_subscriber( + const std::string & topic_name, + std::function callback, + std::optional qos = std::nullopt) + { + if (subscribers_.find(topic_name) == subscribers_.end()) { + std::shared_ptr> subscriber; + autoware::test_utils::createSubscription( + test_node_, topic_name, callback, subscriber, qos); + subscribers_[topic_name] = std::static_pointer_cast(subscriber); + } else { + RCLCPP_WARN(test_node_->get_logger(), "Subscriber %s already set.", topic_name.c_str()); + } + } + + /** + * @brief Publishes a ROS Clock message with the specified time. + * + * This function publishes a ROS Clock message with the specified time. + * Be careful when using this function, as it can affect the behavior of + * the system under test. Consider using ament_add_ros_isolated_gtest to + * isolate the system under test from the ROS clock. + * + * @param time The time to publish. + */ + void jump_clock(const rclcpp::Time & time) + { + rosgraph_msgs::msg::Clock clock; + clock.clock = time; + pub_clock_->publish(clock); + } + +protected: + // Publisher + std::unordered_map> publishers_; + std::unordered_map> subscribers_; + rclcpp::Publisher::SharedPtr pub_clock_; + + // Node + rclcpp::Node::SharedPtr test_node_; +}; // class AutowareTestManager + +std::optional resolve_pkg_share_uri(const std::string & uri_path); + +} // namespace autoware::test_utils + +#endif // AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_ diff --git a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp new file mode 100644 index 0000000000..e4cba1f1a8 --- /dev/null +++ b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp @@ -0,0 +1,228 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_ +#define AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::test_utils +{ +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjectKinematics; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjectKinematics; +using autoware_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_perception_msgs::msg::TrafficLightGroup; +using autoware_perception_msgs::msg::TrafficLightGroupArray; + +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::LaneletPrimitive; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::PathPoint; +using builtin_interfaces::msg::Duration; +using builtin_interfaces::msg::Time; +using geometry_msgs::msg::Accel; +using geometry_msgs::msg::AccelWithCovariance; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseWithCovariance; +using geometry_msgs::msg::Twist; +using geometry_msgs::msg::TwistWithCovariance; +using nav_msgs::msg::Odometry; +using std_msgs::msg::Header; +using unique_identifier_msgs::msg::UUID; + +/** + * @brief Parses a YAML node and converts it into an object of type T. + * + * This function extracts data from the given YAML node and converts it into an object of type T. + * If no specialization exists for T, it will result in a compile-time error. + * + * @tparam T The type of object to parse the node contents into. + * @param node The YAML node to be parsed. + * @return T An object of type T containing the parsed data. + */ +template +T parse(const YAML::Node & node); + +template <> +Header parse(const YAML::Node & node); + +template <> +Duration parse(const YAML::Node & node); + +template <> +Time parse(const YAML::Node & node); + +template <> +Point parse(const YAML::Node & node); + +template <> +std::vector parse(const YAML::Node & node); + +template <> +std::array parse(const YAML::Node & node); + +template <> +Pose parse(const YAML::Node & node); + +template <> +PoseWithCovariance parse(const YAML::Node & node); + +template <> +Twist parse(const YAML::Node & node); + +template <> +TwistWithCovariance parse(const YAML::Node & node); + +template <> +Odometry parse(const YAML::Node & node); + +template <> +Accel parse(const YAML::Node & node); + +template <> +AccelWithCovariance parse(const YAML::Node & node); + +template <> +AccelWithCovarianceStamped parse(const YAML::Node & node); + +template <> +LaneletPrimitive parse(const YAML::Node & node); + +template <> +std::vector parse(const YAML::Node & node); + +template <> +std::vector parse(const YAML::Node & node); + +template <> +LaneletRoute parse(const YAML::Node & node); + +template <> +std::vector parse(const YAML::Node & node); + +template <> +UUID parse(const YAML::Node & node); + +template <> +PathPoint parse(const YAML::Node & node); + +template <> +PathPointWithLaneId parse(const YAML::Node & node); + +template <> +PathWithLaneId parse(const YAML::Node & node); + +template <> +PredictedPath parse(const YAML::Node & node); + +template <> +ObjectClassification parse(const YAML::Node & node); + +template <> +Shape parse(const YAML::Node & node); + +template <> +PredictedObjectKinematics parse(const YAML::Node & node); + +template <> +PredictedObject parse(const YAML::Node & node); + +template <> +PredictedObjects parse(const YAML::Node & node); + +template <> +TrackedObjectKinematics parse(const YAML::Node & node); + +template <> +TrackedObject parse(const YAML::Node & node); + +template <> +TrackedObjects parse(const YAML::Node & node); + +template <> +TrafficLightGroupArray parse(const YAML::Node & node); + +template <> +TrafficLightGroup parse(const YAML::Node & node); + +template <> +TrafficLightElement parse(const YAML::Node & node); + +template <> +OperationModeState parse(const YAML::Node & node); + +/** + * @brief Parses a YAML file and converts it into an object of type T. + * + * This function reads the specified YAML file and converts its contents into an object of the given + * type T. If no specialization exists for T, it will result in a compile-time error. + * + * @tparam T The type of object to parse the file contents into. + * @param filename The path to the YAML file to be parsed. + * @return T An object of type T containing the parsed data. + */ +template +T parse(const std::string & filename); + +template <> +std::optional parse(const std::string & filename); + +template <> +std::optional parse(const std::string & filename); + +template +auto create_const_shared_ptr(MessageType && payload) +{ + using UnqualifiedType = typename std::decay_t; + return std::make_shared(std::forward(payload)); +} + +} // namespace autoware::test_utils + +#endif // AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_ diff --git a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp new file mode 100644 index 0000000000..0c596673e4 --- /dev/null +++ b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp @@ -0,0 +1,328 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// NOTE(soblin): this file is intentionally inline to deal with link issue + +#ifndef AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_ +#define AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::test_utils +{ + +struct PointConfig +{ + std::optional color{}; + std::optional marker{}; + std::optional marker_size{}; +}; + +struct LineConfig +{ + static constexpr const char * default_color = "blue"; + static LineConfig defaults() + { + return LineConfig{std::string(default_color), 1.0, "solid", std::nullopt}; + } + std::optional color{}; + std::optional linewidth{}; + std::optional linestyle{}; + std::optional label{}; +}; + +struct LaneConfig +{ + static LaneConfig defaults() { return LaneConfig{std::nullopt, LineConfig::defaults(), true}; } + + std::optional label{}; + std::optional line_config{}; + bool plot_centerline = true; // alpha{}; + std::optional color{}; + bool fill{true}; + std::optional linewidth{}; + std::optional point_config{}; +}; + +/** + * @brief plot the linestring by `axes.plot()` + * @param [in] config_opt argument for plotting the linestring. if valid, each field is used as the + * kwargs + */ +inline void plot_lanelet2_object( + const lanelet::ConstLineString3d & line, autoware::pyplot::Axes & axes, + const std::optional & config_opt = std::nullopt) +{ + py::dict kwargs{}; + if (config_opt) { + const auto & config = config_opt.value(); + if (config.color) { + kwargs["color"] = config.color.value(); + } + if (config.linewidth) { + kwargs["linewidth"] = config.linewidth.value(); + } + if (config.linestyle) { + kwargs["linestyle"] = config.linestyle.value(); + } + if (config.label) { + kwargs["label"] = config.label.value(); + } + } + std::vector xs; + for (const auto & p : line) { + xs.emplace_back(p.x()); + } + std::vector ys; + for (const auto & p : line) { + ys.emplace_back(p.y()); + } + axes.plot(Args(xs, ys), kwargs); +} + +/** + * @brief plot the left/right bounds and optionally centerline + * @param [in] args used for plotting the left/right bounds as + */ +inline void plot_lanelet2_object( + const lanelet::ConstLanelet & lanelet, autoware::pyplot::Axes & axes, + const std::optional & config_opt = std::nullopt) +{ + const auto left = lanelet.leftBound(); + const auto right = lanelet.rightBound(); + + const auto line_config = [&]() -> std::optional { + if (!config_opt) { + return LineConfig{std::string(LineConfig::default_color)}; + } + return config_opt.value().line_config; + }(); + + if (config_opt) { + const auto & config = config_opt.value(); + + // plot lanelet centerline + if (config.plot_centerline) { + auto centerline_config = [&]() -> LineConfig { + if (!config.line_config) { + return LineConfig{"k", std::nullopt, "dashed"}; + } + auto cfg = config.line_config.value(); + cfg.color = "k"; + cfg.linestyle = "dashed"; + return cfg; + }(); + plot_lanelet2_object( + lanelet.centerline(), axes, std::make_optional(std::move(centerline_config))); + } + + // plot lanelet-id + const auto center = (left.front().basicPoint2d() + left.back().basicPoint2d() + + right.front().basicPoint2d() + right.back().basicPoint2d()) / + 4.0; + axes.text( + Args(center.x(), center.y(), std::to_string(lanelet.id())), Kwargs("clip_on"_a = true)); + } + + if (config_opt && config_opt.value().label) { + auto left_line_config_for_legend = line_config ? line_config.value() : LineConfig::defaults(); + left_line_config_for_legend.label = config_opt.value().label.value(); + + // plot left + plot_lanelet2_object(lanelet.leftBound(), axes, left_line_config_for_legend); + + // plot right + plot_lanelet2_object(lanelet.rightBound(), axes, line_config); + } else { + // plot left + plot_lanelet2_object(lanelet.leftBound(), axes, line_config); + + // plot right + plot_lanelet2_object(lanelet.rightBound(), axes, line_config); + } + + // plot centerline direction + const auto centerline_size = lanelet.centerline().size(); + const auto mid_index = centerline_size / 2; + const auto before = static_cast(std::max(0, mid_index - 1)); + const auto after = static_cast(std::min(centerline_size - 1, mid_index + 1)); + const auto p_before = lanelet.centerline()[before]; + const auto p_after = lanelet.centerline()[after]; + const double azimuth = std::atan2(p_after.y() - p_before.y(), p_after.x() - p_before.x()); + const auto & mid = lanelet.centerline()[mid_index]; + axes.quiver( + Args(mid.x(), mid.y(), std::cos(azimuth), std::sin(azimuth)), Kwargs("units"_a = "width")); +} + +/** + * @brief plot the polygon as matplotlib.patches.Polygon + * @param [in] config_opt argument for plotting the polygon. if valid, each field is used as the + * kwargs + */ +inline void plot_lanelet2_object( + const lanelet::ConstPolygon3d & polygon, autoware::pyplot::Axes & axes, + const std::optional & config_opt = std::nullopt) +{ + std::vector> xy(polygon.size()); + for (unsigned i = 0; i < polygon.size(); ++i) { + xy.at(i) = std::vector({polygon[i].x(), polygon[i].y()}); + } + py::dict kwargs; + if (config_opt) { + const auto & config = config_opt.value(); + if (config.alpha) { + kwargs["alpha"] = config.alpha.value(); + } + if (config.color) { + kwargs["color"] = config.color.value(); + } + kwargs["fill"] = config.fill; + if (config.linewidth) { + kwargs["linewidth"] = config.linewidth.value(); + } + } + auto poly = autoware::pyplot::Polygon(Args(xy), kwargs); + axes.add_patch(Args(poly.unwrap())); +} + +struct DrivableAreaConfig +{ + static DrivableAreaConfig defaults() { return {"turquoise", 2.0}; } + std::optional color{}; + std::optional linewidth{}; +}; + +struct PathWithLaneIdConfig +{ + static PathWithLaneIdConfig defaults() + { + return {std::nullopt, "k", 1.0, std::nullopt, false, 1.0}; + } + std::optional label{}; + std::optional color{}; + std::optional linewidth{}; + std::optional da{}; + bool lane_id{}; // & config_opt = std::nullopt) +{ + py::dict kwargs{}; + if (config_opt) { + const auto & config = config_opt.value(); + if (config.label) { + kwargs["label"] = config.label.value(); + } + if (config.color) { + kwargs["color"] = config.color.value(); + } + if (config.linewidth) { + kwargs["linewidth"] = config.linewidth.value(); + } + } + std::vector xs; + std::vector ys; + std::vector yaw_cos; + std::vector yaw_sin; + std::vector> ids; + const bool plot_lane_id = config_opt ? config_opt.value().lane_id : false; + for (const auto & point : path.points) { + xs.push_back(point.point.pose.position.x); + ys.push_back(point.point.pose.position.y); + const auto th = autoware_utils::get_rpy(point.point.pose.orientation).z; + yaw_cos.push_back(std::cos(th)); + yaw_sin.push_back(std::sin(th)); + if (plot_lane_id) { + ids.emplace_back(); + for (const auto & id : point.lane_ids) { + ids.back().push_back(id); + } + } + } + // plot centerline + axes.plot(Args(xs, ys), kwargs); + const auto quiver_scale = + config_opt ? config_opt.value().quiver_size : PathWithLaneIdConfig::defaults().quiver_size; + const auto quiver_color = + config_opt ? (config_opt.value().color ? config_opt.value().color.value() : "k") : "k"; + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), Kwargs( + "angles"_a = "xy", "scale_units"_a = "xy", + "scale"_a = quiver_scale, "color"_a = quiver_color)); + if (plot_lane_id) { + for (size_t i = 0; i < xs.size(); ++i) { + std::stringstream ss; + const char * delimiter = ""; + for (const auto id : ids[i]) { + ss << std::exchange(delimiter, ",") << id; + } + axes.text(Args(xs[i], ys[i], ss.str()), Kwargs("clip_on"_a = true)); + } + } + // plot drivable area + if (config_opt && config_opt.value().da) { + auto plot_boundary = [&](const decltype(path.left_bound) & points) { + std::vector xs; + std::vector ys; + for (const auto & point : points) { + xs.push_back(point.x); + ys.push_back(point.y); + } + const auto & cfg = config_opt.value().da.value(); + py::dict kwargs{}; + if (cfg.color) { + kwargs["color"] = cfg.color.value(); + } + if (cfg.linewidth) { + kwargs["linewidth"] = cfg.linewidth.value(); + } + axes.plot(Args(xs, ys), kwargs); + }; + plot_boundary(path.left_bound); + plot_boundary(path.right_bound); + } +} + +} // namespace autoware::test_utils + +#endif // AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_ diff --git a/common/autoware_test_utils/launch/psim_intersection.launch.xml b/common/autoware_test_utils/launch/psim_intersection.launch.xml new file mode 100644 index 0000000000..723dd73522 --- /dev/null +++ b/common/autoware_test_utils/launch/psim_intersection.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml b/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml new file mode 100644 index 0000000000..7dd888a036 --- /dev/null +++ b/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml new file mode 100644 index 0000000000..16aa464050 --- /dev/null +++ b/common/autoware_test_utils/package.xml @@ -0,0 +1,44 @@ + + + + autoware_test_utils + 0.0.0 + ROS 2 node for testing interface of the nodes in planning module + Kyoichi Sugahara + Takamasa Horibe + Zulfaqar Azmi + Mamoru Sobue + Yukinari Hisaki + Jian Kang + Apache License 2.0 + + Kyoichi Sugahara + + ament_cmake_auto + autoware_cmake + + ament_index_cpp + autoware_adapi_v1_msgs + autoware_internal_planning_msgs + autoware_lanelet2_extension + autoware_map_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_utils + lanelet2_io + nav_msgs + rclcpp + std_srvs + tf2_msgs + tf2_ros + unique_identifier_msgs + yaml_cpp_vendor + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_test_utils/rviz/psim_test_map.rviz b/common/autoware_test_utils/rviz/psim_test_map.rviz new file mode 100644 index 0000000000..8475c94b86 --- /dev/null +++ b/common/autoware_test_utils/rviz/psim_test_map.rviz @@ -0,0 +1,4569 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + Splitter Ratio: 0.557669460773468 + Tree Height: 225 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: ~ + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: AutowareDateTimePanel + Name: AutowareDateTimePanel + - Class: rviz_plugins::AutowareStatePanel + Name: AutowareStatePanel + - Class: rviz_plugins::SimulatedClockPanel + Name: SimulatedClockPanel + - Class: rviz_plugins::TrafficLightPublishPanel + Name: TrafficLightPublishPanel +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + ars408_front_center: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera0/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera0/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera1/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera1/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera2/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera2/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera3/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera3/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera4/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera4/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera5/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera5/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera6/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera6/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera7/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera7/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gnss_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + livox_front_left: + Alpha: 1 + Show Axes: false + Show Trail: false + livox_front_left_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + livox_front_right: + Alpha: 1 + Show Axes: false + Show Trail: false + livox_front_right_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_kit_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + tamagawa/imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_left_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_rear: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_rear_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_right_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_top_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: VehicleModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_plugins/PolarGridDisplay + Color: 255; 255; 255 + Delta Range: 10 + Enabled: true + Max Alpha: 0.5 + Max Range: 100 + Max Wave Alpha: 0.5 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 + Name: PolarGridDisplay + Reference Frame: base_link + Value: true + Wave Color: 255; 255; 255 + Wave Velocity: 40 + - Background Alpha: 0.5 + Background Color: 23; 28; 31 + Class: autoware_overlay_rviz_plugin/SignalDisplay + Dark Traffic Color: 255; 51; 51 + Enabled: true + Gear Topic Test: /vehicle/status/gear_status + Handle Angle Scale: 17 + Hazard Lights Topic: /vehicle/status/hazard_lights_status + Height: 100 + Left: 0 + Light Traffic Color: 255; 153; 153 + Name: SignalDisplay + Primary Color: 174; 174; 174 + Signal Color: 0; 230; 120 + Speed Limit Topic: /planning/scenario_planning/current_max_velocity + Speed Topic: /vehicle/status/velocity_status + Steering Topic: /vehicle/status/steering_status + Top: 10 + Traffic Topic: /planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal + Turn Signals Topic: /vehicle/status/turn_indicators_status + Value: true + Width: 550 + Enabled: true + Name: Vehicle + - Class: rviz_plugins/MrmSummaryOverlayDisplay + Enabled: false + Font Size: 10 + Left: 512 + Max Letter Num: 100 + Name: MRM Summary + Text Color: 25; 255; 240 + Top: 64 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /system/emergency/hazard_status + Value: false + Value height offset: 0 + Enabled: true + Name: System + - Class: rviz_common/Group + Displays: + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 28.71826171875 + Min Value: -7.4224700927734375 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 237 + Min Color: 211; 215; 207 + Min Intensity: 0 + Name: PointCloudMap + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/pointcloud_map + Use Fixed Frame: true + Use rainbow: false + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Lanelet2VectorMap + Namespaces: + bus_stop_area: true + center_lane_line: false + center_line_arrows: false + curbstone: true + lane_start_bound: false + lanelet direction: true + lanelet_id: false + left_lane_bound: true + no_parking_area: true + parking_lots: true + parking_space: true + partitions: true + right_lane_bound: true + road_lanelets: false + shoulder_center_lane_line: false + shoulder_center_line_arrows: true + shoulder_lane_start_bound: true + shoulder_left_lane_bound: true + shoulder_right_lane_bound: true + shoulder_road_lanelets: false + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/vector_map_marker + Value: true + Enabled: true + Name: Map + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.4000000059604645 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: -1 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ConcatenatePointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + - Alpha: 0.9990000128746033 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: MeasurementRange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/lidar/crop_box_filter/crop_box_polygon + Value: false + Enabled: true + Name: LiDAR + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 233; 185; 110 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.20000000298023224 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.699999988079071 + Head Radius: 1.2000000476837158 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.5 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/gnss/pose_with_covariance + Value: true + Enabled: false + Name: GNSS + Enabled: true + Name: Sensing + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 170; 255 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovInitial + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/initial_pose_with_covariance + Value: false + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 255; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovAligned + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose_with_covariance + Value: false + - Buffer Size: 200 + Class: rviz_plugins::PoseHistory + Enabled: false + Line: + Alpha: 0.9990000128746033 + Color: 170; 255; 127 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Initial + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /localization/util/downsample/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 85; 255; 127 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Aligned + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/points_aligned + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MonteCarloInitialPose + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/monte_carlo_initial_pose_marker + Value: true + Enabled: true + Name: NDT + - Class: rviz_common/Group + Displays: + - Buffer Size: 1000 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 0; 255; 255 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_twist_fusion_filter/pose + Value: true + Enabled: true + Name: EKF + Enabled: true + Name: Localization + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 15 + Min Value: -2 + Value: false + Axis: Z + Channel Name: z + Class: rviz_default_plugins/PointCloud2 + Color: 200; 200; 200 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 15 + Min Color: 0; 0; 0 + Min Intensity: -5 + Name: NoGroundPointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/obstacle_segmentation/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Segmentation + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: DetectedObjects + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + Enabled: false + Name: Detection + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_perception_rviz_plugin/TrackedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Dynamic Status: All + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: TrackedObjects + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/tracking/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + Enabled: false + Name: Tracking + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_perception_rviz_plugin/PredictedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: PredictedObjects + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Maneuver + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/prediction/maneuver + Value: false + Enabled: true + Name: Prediction + Enabled: true + Name: ObjectRecognition + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RecognitionResultOnImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/traffic_light/debug/rois + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MapBasedDetectionResult + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers + Value: true + Enabled: true + Name: TrafficLight + - Class: rviz_common/Group + Displays: + - Alpha: 0.5 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map_updates + Use Timestamp: false + Value: true + Enabled: false + Name: OccupancyGrid + Enabled: true + Name: Perception + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: RouteArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/route_marker + Value: true + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.30000001192092896 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.5 + Name: GoalPose + Shaft Length: 3 + Shaft Radius: 0.20000000298023224 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/echo_back_goal_pose + Value: true + - Background Alpha: 0.5 + Background Color: 23; 28; 31 + Class: autoware_mission_details_overlay_rviz_plugin/MissionDetailsDisplay + Enabled: true + Height: 100 + Name: MissionDetailsDisplay + Remaining Distance and Time Topic: /planning/mission_remaining_distance_time + Right: 10 + Text Color: 194; 194; 194 + Top: 10 + Value: true + Width: 170 + Enabled: true + Name: MissionPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: ScenarioTrajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/trajectory + Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.9990000128746033 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: Path + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/path + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.4000000059604645 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_AvoidanceByLC + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/avoidance_by_lane_change + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_Avoidance + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/static_obstacle_avoidance + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_LaneChangeRight + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/lane_change_right + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_LaneChangeLeft + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/lane_change_left + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_GoalPlanner + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/goal_planner + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_StartPlanner + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/start_planner + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_AvoidanceByLC + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/avoidance_by_lane_change + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: (old)PathChangeCandidate_LaneChange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/lane_change + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_LaneChangeRight + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/lane_change_right + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_LaneChangeLeft + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/lane_change_left + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_ExternalRequestLaneChangeRight + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/external_request_lane_change_right + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_ExternalRequestLaneChangeLeft + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/external_request_lane_change_left + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_Avoidance + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/static_obstacle_avoidance + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_StartPlanner + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/start_planner + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_GoalPlanner + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/goal_planner + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Bound + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (StaticObstacleAvoidance) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/static_obstacle_avoidance + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (AvoidanceByLC) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/avoidance_by_lane_change + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (LaneChangeRight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_right + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (LaneChangeLeft) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_left + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ExtLaneChangeRight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_right + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ExtLaneChangeLeft) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_left + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (GoalPlanner) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/goal_planner + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (StartPlanner) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/start_planner + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (BlindSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Crosswalk) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Walkway) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/walkway + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (DetectionArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Intersection) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (MergeFromPrivateArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (NoStoppingArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (OcclusionSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (StopLine) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (TrafficLight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (VirtualTrafficLight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (RunOut) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (SpeedBump) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/speed_bump + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (NoDrivableLane) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_drivable_lane + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Arrow + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Crosswalk + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Intersection + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Blind Spot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: TrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: VirtualTrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: StopLine + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: DetectionArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: OcclusionSpot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: NoStoppingArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: RunOut + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: StaticObstacleAvoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/static_obstacle_avoidance + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LeftLaneChange + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_left + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: RightLaneChange + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_right + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LaneFollowing + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_following + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: GoalPlanner + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/goal_planner + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: StartPlanner + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: SideShift + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/side_shift + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: SpeedBump + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/speed_bump + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: DynamicObstacleAvoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/dynamic_obstacle_avoidance + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: NoDrivableLane + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_drivable_lane + Value: false + Enabled: false + Name: DebugMarker + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (StaticObstacleAvoidance) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/static_obstacle_avoidance + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (AvoidanceByLC) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/avoidance_by_lane_change + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (LaneChangeLeft) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_left + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (LaneChangeRight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_right + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (ExtLaneChangeLeft) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_left + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (ExtLaneChangeRight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_right + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (GoalPlanner) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/goal_planner + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (StartPlanner) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/start_planner + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (DynamicObstacleAvoidance) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/dynamic_obstacle_avoidance + Value: false + Enabled: false + Name: InfoMarker + Enabled: true + Name: BehaviorPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/trajectory + Value: false + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.9990000128746033 + Constant Width: false + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleStop) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (SurroundObstacle) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleAvoidance) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleCruise) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (MotionVelocitySmoother) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/velocity_smoother/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (OutOfLane) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/virtual_walls + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleVelocityLimiter) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_velocity_limiter/virtual_walls + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (DynamicObstacleStop) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/dynamic_obstacle_stop/virtual_walls + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SurroundObstacleCheck + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Footprint + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 239; 41; 41 + Enabled: false + Name: FootprintOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 10; 21; 255 + Enabled: false + Name: FootprintRecoverOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset + Value: false + Enabled: false + Name: SurroundObstacleChecker + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ObstacleStop + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: CruiseVirtualWall + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SlowDownVirtualWall + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/slow_down/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: DebugMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker + Value: true + Enabled: false + Name: ObstacleCruise + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ObstacleAvoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: OutOfLane + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/debug_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ObstacleVelocityLimiter + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_velocity_limiter/debug_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: DynamicObstacleStop + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/dynamic_obstacle_stop/debug_markers + Value: true + Enabled: false + Name: MotionVelocityPlanner + Enabled: false + Name: DebugMarker + Enabled: true + Name: MotionPlanning + Enabled: true + Name: LaneDriving + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates + Use Timestamp: false + Value: false + - Alpha: 0.9990000128746033 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PartialPoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (3D) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array + Value: true + - Alpha: 0.9990000128746033 + Arrow Length: 0.5 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 0; 255 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array + Value: true + Enabled: true + Name: Parking + - Class: rviz_plugins/PoseWithUuidStamped + Enabled: true + Length: 1.5 + Name: ModifiedGoal + Radius: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/modified_goal + UUID: + Scale: 0.30000001192092896 + Value: false + Value: true + Enabled: true + Name: ScenarioPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: PlanningErrorMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker + Value: true + Enabled: false + Name: Diagnostic + Enabled: true + Name: Planning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: Predicted Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/controller_node_exe/debug/predicted_trajectory_in_frenet_coordinate + Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Constant Width: true + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: true + Width: 0.05000000074505806 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: Resampled Reference Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/controller_node_exe/debug/resampled_reference_trajectory + Value: false + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Constant Width: true + Fade Out Distance: 0 + Max Velocity Color: 0; 230; 120 + Mid Velocity Color: 32; 138; 174 + Min Velocity Color: 63; 46; 227 + Value: false + Width: 0.20000000298023224 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: true + View Text Slope: + Scale: 0.30000001192092896 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (AEB) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/autonomous_emergency_braking/virtual_wall + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Stop Reason + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/longitudinal/stop_reason + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/MPC + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/mpc_follower/debug/markers + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/PurePursuit + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/controller_node_exe/debug/markers + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/AEB + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/autonomous_emergency_braking/debug/markers + Value: false + Enabled: true + Name: Control + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: ~ + Enabled: true + Name: Map + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: PointcloudOnCamera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/camera/camera6/image_raw + Value: true + Visibility: + Control: + Debug/AEB: true + Debug/MPC: true + Debug/PurePursuit: true + Predicted Trajectory: true + Resampled Reference Trajectory: true + Stop Reason: true + Value: false + VirtualWall: + Value: true + VirtualWall (AEB): true + Debug: + Control: true + Localization: + "": true + Value: true + Map: true + Perception: + "": true + Value: true + Planning: + "": + "": true + Value: true + Value: true + Sensing: + ConcatenatePointCloud: true + RadarRawObjects(white): true + Value: true + Value: true + Localization: + EKF: + PoseHistory: true + Value: true + NDT: + Aligned: true + Initial: true + MonteCarloInitialPose: true + PoseHistory: true + PoseWithCovAligned: true + PoseWithCovInitial: true + Value: true + Value: false + Map: + Lanelet2VectorMap: true + PointCloudMap: true + Value: false + Perception: + ObjectRecognition: + Detection: + DetectedObjects: true + Value: true + Prediction: + Maneuver: true + PredictedObjects: true + Value: true + Tracking: + TrackedObjects: true + Value: true + Value: true + OccupancyGrid: + Map: true + Value: true + Segmentation: + NoGroundPointCloud: true + Value: true + TrafficLight: + MapBasedDetectionResult: true + RecognitionResultOnImage: true + Value: true + Value: false + Planning: + Diagnostic: + PlanningErrorMarker: true + Value: true + MissionPlanning: + GoalPose: true + MissionDetailsDisplay: true + RouteArea: true + Value: true + ScenarioPlanning: + LaneDriving: + BehaviorPlanning: + (old)PathChangeCandidate_LaneChange: true + Bound: true + DebugMarker: + Arrow: true + Blind Spot: true + Crosswalk: true + DetectionArea: true + DynamicObstacleAvoidance: true + GoalPlanner: true + Intersection: true + LaneFollowing: true + LeftLaneChange: true + NoDrivableLane: true + NoStoppingArea: true + OcclusionSpot: true + RightLaneChange: true + RunOut: true + SideShift: true + SpeedBump: true + StartPlanner: true + StaticObstacleAvoidance: true + StopLine: true + TrafficLight: true + Value: true + VirtualTrafficLight: true + InfoMarker: + Info (AvoidanceByLC): true + Info (DynamicObstacleAvoidance): true + Info (ExtLaneChangeLeft): true + Info (ExtLaneChangeRight): true + Info (GoalPlanner): true + Info (LaneChangeLeft): true + Info (LaneChangeRight): true + Info (StartPlanner): true + Info (StaticObstacleAvoidance): true + Value: true + Path: true + PathChangeCandidate_Avoidance: true + PathChangeCandidate_AvoidanceByLC: true + PathChangeCandidate_ExternalRequestLaneChangeLeft: true + PathChangeCandidate_ExternalRequestLaneChangeRight: true + PathChangeCandidate_GoalPlanner: true + PathChangeCandidate_LaneChangeLeft: true + PathChangeCandidate_LaneChangeRight: true + PathChangeCandidate_StartPlanner: true + PathReference_Avoidance: true + PathReference_AvoidanceByLC: true + PathReference_GoalPlanner: true + PathReference_LaneChangeLeft: true + PathReference_LaneChangeRight: true + PathReference_StartPlanner: true + Value: true + VirtualWall: + Value: true + VirtualWall (AvoidanceByLC): true + VirtualWall (BlindSpot): true + VirtualWall (Crosswalk): true + VirtualWall (DetectionArea): true + VirtualWall (ExtLaneChangeLeft): true + VirtualWall (ExtLaneChangeRight): true + VirtualWall (GoalPlanner): true + VirtualWall (Intersection): true + VirtualWall (LaneChangeLeft): true + VirtualWall (LaneChangeRight): true + VirtualWall (MergeFromPrivateArea): true + VirtualWall (NoDrivableLane): true + VirtualWall (NoStoppingArea): true + VirtualWall (OcclusionSpot): true + VirtualWall (RunOut): true + VirtualWall (SpeedBump): true + VirtualWall (StartPlanner): true + VirtualWall (StaticObstacleAvoidance): true + VirtualWall (StopLine): true + VirtualWall (TrafficLight): true + VirtualWall (VirtualTrafficLight): true + VirtualWall (Walkway): true + MotionPlanning: + DebugMarker: + MotionVelocityPlanner: + DynamicObstacleStop: true + ObstacleVelocityLimiter: true + OutOfLane: true + Value: true + ObstacleAvoidance: true + ObstacleCruise: + CruiseVirtualWall: true + DebugMarker: true + SlowDownVirtualWall: true + Value: true + ObstacleStop: true + SurroundObstacleChecker: + Footprint: true + FootprintOffset: true + FootprintRecoverOffset: true + SurroundObstacleCheck: true + Value: true + Value: true + Trajectory: true + Value: true + VirtualWall: + Value: true + VirtualWall (DynamicObstacleStop): true + VirtualWall (MotionVelocitySmoother): true + VirtualWall (ObstacleAvoidance): true + VirtualWall (ObstacleCruise): true + VirtualWall (ObstacleStop): true + VirtualWall (ObstacleVelocityLimiter): true + VirtualWall (OutOfLane): true + VirtualWall (SurroundObstacle): true + Value: true + ModifiedGoal: true + Parking: + Costmap: true + PartialPoseArray: true + PoseArray: true + Value: true + ScenarioTrajectory: true + Value: true + Value: false + Sensing: + GNSS: + PoseWithCovariance: true + Value: true + LiDAR: + ConcatenatePointCloud: true + MeasurementRange: true + Value: true + Value: true + System: + Grid: true + MRM Summary: true + TF: true + Value: false + Vehicle: + PolarGridDisplay: true + SignalDisplay: true + Value: true + VehicleModel: true + Value: true + Zoom Factor: 1 + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: -1 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ConcatenatePointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + - BUS: + Alpha: 0.5 + Color: 255; 255; 255 + CAR: + Alpha: 0.5 + Color: 255; 255; 255 + CYCLIST: + Alpha: 0.5 + Color: 255; 255; 255 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.5 + Color: 255; 255; 255 + Name: RadarRawObjects(white) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.5 + Color: 255; 255; 255 + Polygon Type: 3d + TRAILER: + Alpha: 0.5 + Color: 255; 255; 255 + TRUCK: + Alpha: 0.5 + Color: 255; 255; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/radar/detected_objects + UNKNOWN: + Alpha: 0.5 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + Enabled: false + Name: Sensing + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 85; 255; 127 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: NDT pointclouds + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/points_aligned + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: NDTLoadedPCDMap + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/debug/loaded_pointcloud_map + Use Fixed Frame: true + Use rainbow: true + Value: true + - Buffer Size: 200 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 170; 255; 127 + Value: true + Width: 0.10000000149011612 + Name: NDTPoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose + Value: true + - Buffer Size: 1000 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 0; 255; 255 + Value: true + Width: 0.10000000149011612 + Name: EKFPoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_twist_fusion_filter/pose + Value: true + Enabled: true + Name: Localization + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + CAR: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + Name: Centerpoint(red1) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + TRUCK: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/centerpoint/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + Value: true + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + CAR: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + Name: CenterpointROIFusion(red2) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + TRUCK: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/centerpoint/roi_fusion/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + Value: true + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + CAR: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + Name: CenterpointValidator(red3) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + TRUCK: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/centerpoint/validation/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + Value: true + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + CAR: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + Name: PointPainting(light_green1) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + TRUCK: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/pointpainting/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + Value: true + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + CAR: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + Name: PointPaintingROIFusion(light_green2) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + TRUCK: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/pointpainting/roi_fusion/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + Value: true + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + CAR: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + Name: PointPaintingValidator(light_green3) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + TRUCK: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/pointpainting/validation/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + Value: true + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + CAR: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + Name: DetectionByTracker(orange) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + TRUCK: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/detection_by_tracker/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + Value: true + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + CAR: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + Name: CameraLidarFusion(purple) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + TRUCK: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/clustering/camera_lidar_fusion/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + Value: true + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Name: RadarFarObjects(white) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/radar/far_objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + CAR: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + Class: autoware_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + Name: Detection(yellow) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + TRUCK: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + Value: true + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + CAR: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + Class: autoware_perception_rviz_plugin/TrackedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Dynamic Status: All + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + Name: Tracking(green) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + TRUCK: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/tracking/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + Value: true + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + Class: autoware_perception_rviz_plugin/PredictedObjects + Confidence Interval: 95% + Display Acceleration: true + Display Existence Probability: false + Display Label: true + Display Pose Covariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display Twist Covariance: false + Display UUID: true + Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Enabled: true + Line Width: 0.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + Name: Prediction(light_blue) + Namespaces: + {} + Object Fill Type: skeleton + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + Value: true + Visualization Type: Normal + Enabled: true + Name: Perception + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: LaneChangeLeft + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/lane_change_left + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: LaneChangeRight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/lane_change_right + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: AvoidanceLeft + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/static_obstacle_avoidance_left + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: AvoidanceRight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/static_obstacle_avoidance_right + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: AvoidanceByLCLeft + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/avoidance_by_lc_left + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: AvoidanceByLCRight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/avoidance_by_lc_right + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: StartPlanner + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/start_planner + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: GoalPlanner + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/goal_planner + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Crosswalk + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/crosswalk + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Intersection + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/intersection + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: BlindSpot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/blind_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: TrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: DetectionArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/detection_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: NoStoppingArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/no_stopping_area + Value: true + Enabled: true + Name: Objects Of Interest + Enabled: true + Name: Planning + - Class: rviz_common/Group + Displays: ~ + Enabled: true + Name: Control + Enabled: false + Name: Debug + Enabled: true + Global Options: + Background Color: 15; 20; 23 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/goal + - Class: tier4_adapi_rviz_plugins::RouteTool + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz/routing/rough_goal + - Acceleration: 0 + Class: rviz_plugins/PedestrianInitialPoseTool + Interactive: false + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 1 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/CarInitialPoseTool + H vehicle height: 2 + Interactive: false + L vehicle length: 4 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 3 + W vehicle width: 1.7999999523162842 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/BusInitialPoseTool + H vehicle height: 3.5 + Interactive: false + L vehicle length: 10.5 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + W vehicle width: 2.5 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/MissionCheckpointTool + Pose Topic: /planning/mission_planning/checkpoint + Theta std deviation: 0.2617993950843811 + X std deviation: 0.5 + Y std deviation: 0.5 + Z position: 0 + - Class: rviz_plugins/DeleteAllObjectsTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: + - Class: rviz_default_plugins/ThirdPersonFollower + Distance: 18 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.20000000298023224 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 3.141592025756836 + - Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 +Window Geometry: + AutowareDateTimePanel: + collapsed: false + AutowareStatePanel: + collapsed: false + Displays: + collapsed: false + Height: 939 + Hide Left Dock: false + Hide Right Dock: false + PointcloudOnCamera: + collapsed: false + QMainWindow State: 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 + RecognitionResultOnImage: + collapsed: false + Selection: + collapsed: false + SimulatedClockPanel: + collapsed: false + Tool Properties: + collapsed: false + TrafficLightPublishPanel: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 34 diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp new file mode 100644 index 0000000000..0ab2430f7f --- /dev/null +++ b/common/autoware_test_utils/src/autoware_test_utils.cpp @@ -0,0 +1,348 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace autoware::test_utils +{ + +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using geometry_msgs::msg::TransformStamped; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = create_point(x, y, z); + p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); + return p; +} + +geometry_msgs::msg::Pose createPose(const std::array & pose3d) +{ + return createPose(pose3d[0], pose3d[1], pose3d[2], 0.0, 0.0, pose3d[3]); +} + +LaneletSegment createLaneletSegment(int id) +{ + LaneletPrimitive primitive; + primitive.id = id; + primitive.primitive_type = "lane"; + LaneletSegment segment; + segment.preferred_primitive.id = id; + segment.primitives.push_back(primitive); + return segment; +} + +lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (!errors.empty()) { + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + return nullptr; + } + + for (lanelet::Point3d point : map->pointLayer) { + if (point.hasAttribute("local_x")) { + point.x() = point.attribute("local_x").asDouble().value(); + } + if (point.hasAttribute("local_y")) { + point.y() = point.attribute("local_y").asDouble().value(); + } + } + + // realign lanelet borders using updated points + for (lanelet::Lanelet lanelet : map->laneletLayer) { + auto left = lanelet.leftBound(); + auto right = lanelet.rightBound(); + std::tie(left, right) = lanelet::geometry::align(left, right); + lanelet.setLeftBound(left); + lanelet.setRightBound(right); + } + + return map; +} + +LaneletMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) +{ + std::string format_version{}; + std::string map_version{}; + lanelet::io_handlers::AutowareOsmParser::parseVersions( + lanelet2_filename, &format_version, &map_version); + + LaneletMapBin map_bin_msg; + map_bin_msg.header.stamp = now; + map_bin_msg.header.frame_id = "map"; + map_bin_msg.version_map_format = format_version; + map_bin_msg.version_map = map_version; + lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); + + return map_bin_msg; +} + +LaneletRoute makeNormalRoute() +{ + const std::array start_pose{5.5, 4., 0., M_PI_2}; + const std::array goal_pose{8.0, 26.3, 0, 0}; + LaneletRoute route; + route.header.frame_id = "map"; + route.start_pose = createPose(start_pose); + route.goal_pose = createPose(goal_pose); + return route; +} + +OccupancyGrid makeCostMapMsg(size_t width, size_t height, double resolution) +{ + nav_msgs::msg::OccupancyGrid costmap_msg; + + // create info + costmap_msg.header.frame_id = "map"; + costmap_msg.info.width = width; + costmap_msg.info.height = height; + costmap_msg.info.resolution = static_cast(resolution); + + // create data + const size_t n_elem = width * height; + for (size_t i = 0; i < n_elem; ++i) { + costmap_msg.data.push_back(0.0); + } + return costmap_msg; +} + +std::string get_absolute_path_to_lanelet_map( + const std::string & package_name, const std::string & map_filename) +{ + const auto dir = ament_index_cpp::get_package_share_directory(package_name); + return dir + "/test_map/" + map_filename; +} + +std::string get_absolute_path_to_route( + const std::string & package_name, const std::string & route_filename) +{ + const auto dir = ament_index_cpp::get_package_share_directory(package_name); + return dir + "/test_route/" + route_filename; +} + +std::string get_absolute_path_to_config( + const std::string & package_name, const std::string & config_filename) +{ + const auto dir = ament_index_cpp::get_package_share_directory(package_name); + return dir + "/config/" + config_filename; +} + +LaneletMapBin make_map_bin_msg( + const std::string & absolute_path, const double center_line_resolution) +{ + const auto map = loadMap(absolute_path); + if (!map) { + return autoware_map_msgs::msg::LaneletMapBin_>{}; + } + + // overwrite centerline + lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + + // create map bin msg + const auto map_bin_msg = + convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now()); + return map_bin_msg; +} + +LaneletMapBin makeMapBinMsg(const std::string & package_name, const std::string & map_filename) +{ + return make_map_bin_msg(get_absolute_path_to_lanelet_map(package_name, map_filename)); +} + +Odometry makeOdometry(const double shift) +{ + Odometry odometry; + const std::array start_pose{0.0, shift, 0.0, 0.0}; + odometry.pose.pose = createPose(start_pose); + odometry.header.frame_id = "map"; + return odometry; +} + +Odometry makeInitialPose(const double shift) +{ + Odometry current_odometry; + const auto yaw = 0.9724497591854532; + const auto shift_x = shift * std::sin(yaw); + const auto shift_y = shift * std::cos(yaw); + const std::array start_pose{ + 3722.16015625 + shift_x, 73723.515625 + shift_y, 0.233112560494183, yaw}; + current_odometry.pose.pose = autoware::test_utils::createPose(start_pose); + current_odometry.header.frame_id = "map"; + return current_odometry; +} + +TFMessage makeTFMsg( + rclcpp::Node::SharedPtr target_node, std::string && frame_id, std::string && child_frame_id) +{ + TFMessage tf_msg; + geometry_msgs::msg::Quaternion quaternion; + quaternion.x = 0.; + quaternion.y = 0.; + quaternion.z = 0.23311256049418302; + quaternion.w = 0.9724497591854532; + + TransformStamped tf; + tf.header.stamp = target_node->get_clock()->now(); + tf.header.frame_id = frame_id; + tf.child_frame_id = child_frame_id; + tf.transform.translation.x = 3722.16015625; + tf.transform.translation.y = 73723.515625; + tf.transform.translation.z = 0; + tf.transform.rotation = quaternion; + + tf_msg.transforms.emplace_back(std::move(tf)); + return tf_msg; +} + +Scenario makeScenarioMsg(const std::string & scenario) +{ + Scenario scenario_msg; + scenario_msg.current_scenario = scenario; + scenario_msg.activating_scenarios = {scenario}; + return scenario_msg; +} + +// cppcheck-suppress unusedFunction +RouteSections combineConsecutiveRouteSections( + const RouteSections & route_sections1, const RouteSections & route_sections2) +{ + RouteSections route_sections; + route_sections.reserve(route_sections1.size() + route_sections2.size()); + if (!route_sections1.empty()) { + // remove end route section because it is overlapping with first one in next route_section + route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1); + } + if (!route_sections2.empty()) { + route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end()); + } + return route_sections; +} + +// this is for the test lanelet2_map.osm +// file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 +LaneletRoute makeBehaviorNormalRoute() +{ + LaneletRoute route; + route.header.frame_id = "map"; + route.start_pose = + createPose({3722.16015625, 73723.515625, 0.233112560494183, 0.9724497591854532}); + route.goal_pose = + createPose({3778.362060546875, 73721.2734375, -0.5107480274693206, 0.8597304533609347}); + + std::vector primitive_ids = {9102, 9178, 54, 112}; + for (int id : primitive_ids) { + route.segments.push_back(createLaneletSegment(id)); + } + + std::array uuid_bytes{210, 87, 16, 126, 98, 151, 58, 28, + 252, 221, 230, 92, 122, 170, 46, 6}; + route.uuid.uuid = uuid_bytes; + + route.allow_modification = false; + return route; +} + +// cppcheck-suppress unusedFunction +void spinSomeNodes( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, const int repeat_count) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node); + executor.add_node(target_node); + for (int i = 0; i < repeat_count; i++) { + executor.spin_some(std::chrono::milliseconds(100)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } +} + +void updateNodeOptions( + rclcpp::NodeOptions & node_options, const std::vector & params_files) +{ + std::vector arguments; + arguments.push_back("--ros-args"); + arguments.push_back("--params-file"); + for (const auto & param_file : params_files) { + arguments.push_back(param_file.c_str()); + arguments.push_back("--params-file"); + } + arguments.pop_back(); + + node_options.arguments(std::vector{arguments.begin(), arguments.end()}); +} + +PathWithLaneId loadPathWithLaneIdInYaml() +{ + const auto yaml_path = + get_absolute_path_to_config("autoware_test_utils", "path_with_lane_id_data.yaml"); + + if (const auto path = parse>(yaml_path)) { + return *path; + } + + throw std::runtime_error( + "Failed to parse YAML file: " + yaml_path + ". The file might be corrupted."); +} + +lanelet::ConstLanelet make_lanelet( + const lanelet::BasicPoint2d & left0, const lanelet::BasicPoint2d & left1, + const lanelet::BasicPoint2d & right0, const lanelet::BasicPoint2d & right1) +{ + lanelet::LineString3d left_bound; + left_bound.push_back(lanelet::Point3d(lanelet::InvalId, left0.x(), left0.y(), 0.0)); + left_bound.push_back(lanelet::Point3d(lanelet::InvalId, left1.x(), left1.y(), 0.0)); + lanelet::LineString3d right_bound; + right_bound.push_back(lanelet::Point3d(lanelet::InvalId, right0.x(), right0.y(), 0.0)); + right_bound.push_back(lanelet::Point3d(lanelet::InvalId, right1.x(), right1.y(), 0.0)); + return {lanelet::utils::getId(), left_bound, right_bound}; +} + +std::optional resolve_pkg_share_uri(const std::string & uri_path) +{ + std::smatch match; + std::regex pattern(R"(package://([^/]+)/(.+))"); + if (std::regex_match(uri_path, match, pattern)) { + const std::string pkg_name = ament_index_cpp::get_package_share_directory(match[1].str()); + const std::string resource_path = match[2].str(); + const auto path = std::filesystem::path(pkg_name) / std::filesystem::path(resource_path); + return std::filesystem::exists(path) ? std::make_optional(path) : std::nullopt; + } + return std::nullopt; +} + +} // namespace autoware::test_utils diff --git a/common/autoware_test_utils/src/mock_data_parser.cpp b/common/autoware_test_utils/src/mock_data_parser.cpp new file mode 100644 index 0000000000..29aed75357 --- /dev/null +++ b/common/autoware_test_utils/src/mock_data_parser.cpp @@ -0,0 +1,504 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_test_utils/mock_data_parser.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::test_utils +{ + +template <> +Header parse(const YAML::Node & node) +{ + Header header; + + header.stamp.sec = node["stamp"]["sec"].as(); + header.stamp.nanosec = node["stamp"]["nanosec"].as(); + header.frame_id = node["frame_id"].as(); + + return header; +} + +template <> +Duration parse(const YAML::Node & node) +{ + Duration msg; + msg.sec = node["sec"].as(); + msg.nanosec = node["nanosec"].as(); + return msg; +} + +template <> +Time parse(const YAML::Node & node) +{ + Time msg; + msg.sec = node["sec"].as(); + msg.nanosec = node["nanosec"].as(); + return msg; +} + +template <> +std::array parse(const YAML::Node & node) +{ + std::array msg{}; + const auto cov = node.as>(); + for (size_t i = 0; i < 36; ++i) { + msg[i] = cov.at(i); + } + return msg; +} + +template <> +Point parse(const YAML::Node & node) +{ + Point geom_point; + geom_point.x = node["x"].as(); + geom_point.y = node["y"].as(); + geom_point.z = node["z"].as(); + return geom_point; +} + +template <> +std::vector parse(const YAML::Node & node) +{ + std::vector geom_points; + + std::transform( + node.begin(), node.end(), std::back_inserter(geom_points), [&](const YAML::Node & input) { + Point point; + point.x = input["x"].as(); + point.y = input["y"].as(); + point.z = input["z"].as(); + return point; + }); + + return geom_points; +} + +template <> +Pose parse(const YAML::Node & node) +{ + Pose pose; + pose.position.x = node["position"]["x"].as(); + pose.position.y = node["position"]["y"].as(); + pose.position.z = node["position"]["z"].as(); + pose.orientation.x = node["orientation"]["x"].as(); + pose.orientation.y = node["orientation"]["y"].as(); + pose.orientation.z = node["orientation"]["z"].as(); + pose.orientation.w = node["orientation"]["w"].as(); + return pose; +} + +template <> +PoseWithCovariance parse(const YAML::Node & node) +{ + PoseWithCovariance msg; + msg.pose = parse(node["pose"]); + msg.covariance = parse>(node["covariance"]); + return msg; +} + +template <> +Twist parse(const YAML::Node & node) +{ + Twist msg; + msg.linear.x = node["linear"]["x"].as(); + msg.linear.y = node["linear"]["y"].as(); + msg.linear.z = node["linear"]["z"].as(); + msg.angular.x = node["angular"]["x"].as(); + msg.angular.y = node["angular"]["y"].as(); + msg.angular.z = node["angular"]["z"].as(); + return msg; +} + +template <> +TwistWithCovariance parse(const YAML::Node & node) +{ + TwistWithCovariance msg; + msg.twist = parse(node["twist"]); + msg.covariance = parse>(node["covariance"]); + return msg; +} + +template <> +Odometry parse(const YAML::Node & node) +{ + Odometry msg; + msg.header = parse
(node["header"]); + msg.child_frame_id = node["child_frame_id"].as(); + msg.pose = parse(node["pose"]); + msg.twist = parse(node["twist"]); + return msg; +} + +template <> +Accel parse(const YAML::Node & node) +{ + Accel msg; + msg.linear.x = node["linear"]["x"].as(); + msg.linear.y = node["linear"]["y"].as(); + msg.linear.z = node["linear"]["z"].as(); + msg.angular.x = node["angular"]["x"].as(); + msg.angular.y = node["angular"]["y"].as(); + msg.angular.z = node["angular"]["z"].as(); + return msg; +} + +template <> +AccelWithCovariance parse(const YAML::Node & node) +{ + AccelWithCovariance msg; + msg.accel = parse(node["accel"]); + msg.covariance = parse>(node["covariance"]); + return msg; +} + +template <> +AccelWithCovarianceStamped parse(const YAML::Node & node) +{ + AccelWithCovarianceStamped msg; + msg.header = parse
(node["header"]); + msg.accel = parse(node["accel"]); + return msg; +} + +template <> +LaneletPrimitive parse(const YAML::Node & node) +{ + LaneletPrimitive primitive; + primitive.id = node["id"].as(); + primitive.primitive_type = node["primitive_type"].as(); + + return primitive; +} + +template <> +std::vector parse(const YAML::Node & node) +{ + std::vector primitives; + primitives.reserve(node.size()); + std::transform(node.begin(), node.end(), std::back_inserter(primitives), [&](const auto & p) { + return parse(p); + }); + + return primitives; +} + +template <> +std::vector parse(const YAML::Node & node) +{ + std::vector segments; + std::transform(node.begin(), node.end(), std::back_inserter(segments), [&](const auto & input) { + LaneletSegment segment; + segment.preferred_primitive = parse(input["preferred_primitive"]); + segment.primitives = parse>(input["primitives"]); + return segment; + }); + + return segments; +} + +template <> +LaneletRoute parse(const YAML::Node & node) +{ + LaneletRoute route; + + route.start_pose = parse(node["start_pose"]); + route.goal_pose = parse(node["goal_pose"]); + route.segments = parse>(node["segments"]); + return route; +} + +template <> +std::vector parse>(const YAML::Node & node) +{ + std::vector path_points; + + const auto & points = node; + path_points.reserve(points.size()); + std::transform( + points.begin(), points.end(), std::back_inserter(path_points), [&](const YAML::Node & input) { + PathPointWithLaneId point; + if (!input["point"]) { + return point; + } + const auto & point_node = input["point"]; + point.point.pose = parse(point_node["pose"]); + + point.point.longitudinal_velocity_mps = point_node["longitudinal_velocity_mps"].as(); + point.point.lateral_velocity_mps = point_node["lateral_velocity_mps"].as(); + point.point.heading_rate_rps = point_node["heading_rate_rps"].as(); + point.point.is_final = point_node["is_final"].as(); + // Fill the lane_ids + for (const auto & lane_id_node : input["lane_ids"]) { + point.lane_ids.push_back(lane_id_node.as()); + } + + return point; + }); + + return path_points; +} + +template <> +UUID parse(const YAML::Node & node) +{ + UUID msg; + const auto uuid = node["uuid"].as>(); + for (unsigned i = 0; i < 16; ++i) { + msg.uuid.at(i) = uuid.at(i); + } + return msg; +} + +template <> +ObjectClassification parse(const YAML::Node & node) +{ + ObjectClassification msg; + msg.label = node["label"].as(); + msg.probability = node["probability"].as(); + return msg; +} + +template <> +Shape parse(const YAML::Node & node) +{ + Shape msg; + msg.type = node["type"].as(); + for (const auto & footprint_point_node : node["footprint"]["points"]) { + geometry_msgs::msg::Point32 point; + point.x = footprint_point_node["x"].as(); + point.y = footprint_point_node["y"].as(); + point.z = footprint_point_node["z"].as(); + msg.footprint.points.push_back(point); + } + msg.dimensions.x = node["dimensions"]["x"].as(); + msg.dimensions.y = node["dimensions"]["y"].as(); + msg.dimensions.z = node["dimensions"]["z"].as(); + return msg; +} + +template <> +PathPoint parse(const YAML::Node & node) +{ + PathPoint point; + point.pose = parse(node["pose"]); + point.longitudinal_velocity_mps = node["longitudinal_velocity_mps"].as(); + point.lateral_velocity_mps = node["lateral_velocity_mps"].as(); + point.heading_rate_rps = node["heading_rate_rps"].as(); + point.is_final = node["is_final"].as(); + return point; +} + +template <> +PathPointWithLaneId parse(const YAML::Node & node) +{ + PathPointWithLaneId point; + point.point = parse(node["point"]); + for (const auto & lane_id_node : node["lane_ids"]) { + point.lane_ids.push_back(lane_id_node.as()); + } + return point; +} + +template <> +PathWithLaneId parse(const YAML::Node & node) +{ + PathWithLaneId path; + path.header = parse
(node["header"]); + for (const auto & point_node : node["points"]) { + path.points.push_back(parse(point_node)); + } + for (const auto & left_bound_node : node["left_bound"]) { + path.left_bound.push_back(parse(left_bound_node)); + } + for (const auto & right_bound_node : node["right_bound"]) { + path.right_bound.push_back(parse(right_bound_node)); + } + return path; +} + +template <> +PredictedPath parse(const YAML::Node & node) +{ + PredictedPath path; + for (const auto & path_pose_node : node["path"]) { + path.path.push_back(parse(path_pose_node)); + } + path.time_step = parse(node["time_step"]); + path.confidence = node["confidence"].as(); + return path; +} + +template <> +PredictedObjectKinematics parse(const YAML::Node & node) +{ + PredictedObjectKinematics msg; + msg.initial_pose_with_covariance = + parse(node["initial_pose_with_covariance"]); + msg.initial_twist_with_covariance = + parse(node["initial_twist_with_covariance"]); + msg.initial_acceleration_with_covariance = + parse(node["initial_acceleration_with_covariance"]); + for (const auto & predicted_path_node : node["predicted_paths"]) { + msg.predicted_paths.push_back(parse(predicted_path_node)); + } + return msg; +} + +template <> +PredictedObject parse(const YAML::Node & node) +{ + PredictedObject msg; + msg.object_id = parse(node["object_id"]); + msg.existence_probability = node["existence_probability"].as(); + for (const auto & classification_node : node["classification"]) { + msg.classification.push_back(parse(classification_node)); + } + msg.kinematics = parse(node["kinematics"]); + msg.shape = parse(node["shape"]); + return msg; +} + +template <> +PredictedObjects parse(const YAML::Node & node) +{ + PredictedObjects msg; + msg.header = parse
(node["header"]); + for (const auto & object_node : node["objects"]) { + msg.objects.push_back(parse(object_node)); + } + return msg; +} + +template <> +TrackedObjectKinematics parse(const YAML::Node & node) +{ + TrackedObjectKinematics msg; + msg.pose_with_covariance = parse(node["pose_with_covariance"]); + msg.twist_with_covariance = parse(node["twist_with_covariance"]); + msg.acceleration_with_covariance = + parse(node["acceleration_with_covariance"]); + msg.orientation_availability = node["orientation_availability"].as(); + msg.is_stationary = node["is_stationary"].as(); + return msg; +} + +template <> +TrackedObject parse(const YAML::Node & node) +{ + TrackedObject msg; + msg.object_id = parse(node["object_id"]); + msg.existence_probability = node["existence_probability"].as(); + for (const auto & classification_node : node["classification"]) { + msg.classification.push_back(parse(classification_node)); + } + msg.kinematics = parse(node["kinematics"]); + msg.shape = parse(node["shape"]); + return msg; +} + +template <> +TrackedObjects parse(const YAML::Node & node) +{ + TrackedObjects msg; + msg.header = parse
(node["header"]); + for (const auto & object_node : node["objects"]) { + msg.objects.push_back(parse(object_node)); + } + return msg; +} + +template <> +TrafficLightElement parse(const YAML::Node & node) +{ + TrafficLightElement msg; + msg.color = node["color"].as(); + msg.shape = node["shape"].as(); + msg.status = node["status"].as(); + msg.confidence = node["confidence"].as(); + return msg; +} + +template <> +TrafficLightGroup parse(const YAML::Node & node) +{ + TrafficLightGroup msg; + msg.traffic_light_group_id = node["traffic_light_group_id"].as(); + for (const auto & element_node : node["elements"]) { + msg.elements.push_back(parse(element_node)); + } + return msg; +} + +template <> +TrafficLightGroupArray parse(const YAML::Node & node) +{ + TrafficLightGroupArray msg; + msg.stamp = parse