diff --git a/common/autoware_test_utils/CHANGELOG.rst b/common/autoware_test_utils/CHANGELOG.rst deleted file mode 100644 index 0eddee7112ec2..0000000000000 --- a/common/autoware_test_utils/CHANGELOG.rst +++ /dev/null @@ -1,204 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_test_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.41.0 (2025-01-29) -------------------- -* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base -* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) -* feat(autoware_planning_test_manager): remove dependency of tier4_planning_msgs::msg::LateralOffset (`#9967 `_) - * feat(autoware_planning_test_manager): remove dependency of tier4_planning_msgs::msg::LateralOffset - * fix - --------- -* chore(autoware_test_utils): move rviz config to autoware_launch (`#9925 `_) -* feat(autoware_test_utils): add visualization and yaml dumper for PathWithLaneId (`#9841 `_) -* chore(autoware_test_utils): update test map (`#9664 `_) -* feat(autoware_test_utils): add visualization (`#9603 `_) -* refactor(autoware_test_utils): enhance makeMapBinMsg to accept package name and map filename parameters (`#9617 `_) - * feat: enhance makeMapBinMsg to accept package name and map filename parameters - * feat: set default package name to 'autoware_test_utils' in makeMapBinMsg and related functions - --------- -* Contributors: Fumiya Watanabe, Kyoichi Sugahara, Mamoru Sobue, Ryohsuke Mitsudome, Takayuki Murooka - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* ci(pre-commit): update cpplint to 2.0.0 (`#9557 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* refactor(test_utils): return parser as optional (`#9391 `_) - Co-authored-by: Mamoru Sobue -* 0.39.0 -* update changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* feat(autoware_test_utils): add parser for LaneletRoute, TrackedObject (`#9317 `_) -* test(autoware_test_utils): add TrafficLight to map, fix launcher (`#9318 `_) -* test(bpp_common): add tests for the static drivable area (`#9324 `_) -* test(costmap_generator): unit test implementation for costmap generator (`#9149 `_) - * modify costmap generator directory structure - * rename class CostmapGenerator to CostmapGeneratorNode - * unit test for object_map_utils - * catch error from lookupTransform - * use polling subscriber in costmap generator node - * add test for costmap generator node - * add test for isActive() - * revert unnecessary changes - * remove commented out line - * minor fix - * Update planning/autoware_costmap_generator/src/costmap_generator.cpp - Co-authored-by: Kosuke Takeuchi - --------- - Co-authored-by: Kosuke Takeuchi -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* feat(autoware_test_utils): use sample_vehicle/sample_sensor_kit (`#9290 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* feat(autoware_test_utils): add general topic dumper (`#9207 `_) -* refactor(component_interface_utils): prefix package and namespace with autoware (`#9092 `_) -* feat(autoware_test_utils): add traffic light msgs parser (`#9177 `_) -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Mamoru Sobue, Maxime CLEMENT, Ryohsuke Mitsudome, Yutaka Kondo, Zulfaqar Azmi, awf-autoware-bot[bot], mkquda - -0.39.0 (2024-11-25) -------------------- -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* test(costmap_generator): unit test implementation for costmap generator (`#9149 `_) - * modify costmap generator directory structure - * rename class CostmapGenerator to CostmapGeneratorNode - * unit test for object_map_utils - * catch error from lookupTransform - * use polling subscriber in costmap generator node - * add test for costmap generator node - * add test for isActive() - * revert unnecessary changes - * remove commented out line - * minor fix - * Update planning/autoware_costmap_generator/src/costmap_generator.cpp - Co-authored-by: Kosuke Takeuchi - --------- - Co-authored-by: Kosuke Takeuchi -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* feat(autoware_test_utils): use sample_vehicle/sample_sensor_kit (`#9290 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* feat(autoware_test_utils): add general topic dumper (`#9207 `_) -* refactor(component_interface_utils): prefix package and namespace with autoware (`#9092 `_) -* feat(autoware_test_utils): add traffic light msgs parser (`#9177 `_) -* Contributors: Esteve Fernandez, Mamoru Sobue, Yutaka Kondo, mkquda - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* feat(autoware_test_utils): add parser for PredictedObjects (`#9176 `_) - feat(autoware_test_utils): add parser for predicted objects -* refactor(autoware_test_utils): sanitizer header (`#9174 `_) -* refactor(component_interface_specs): prefix package and namespace with autoware (`#9094 `_) -* fix(autoware_test_utils): remove unnecessary cppcheck suppression (`#9165 `_) -* feat(autoware_test_utils): add parser for geometry_msgs and others (`#9167 `_) -* feat(autoware_test_utils): add path with lane id parser (`#9098 `_) - * add path with lane id parser - * refactor parse to use template - --------- -* feat(autoware_test_utils): move test_map, add launcher for test_map (`#9045 `_) -* feat(test_utils): add simple path with lane id generator (`#9113 `_) - * add simple path with lane id generator - * chnage to explicit template - * fix - * add static cast - * remove header file - --------- -* fix(autoware_test_utils): missing ament_index_cpp dependency (`#8618 `_) -* fix(autoware_test_utils): fix unusedFunction (`#8857 `_) - fix:unusedFunction -* fix(autoware_test_utils): fix unusedFunction (`#8816 `_) - fix: unusedFunction -* test(autoware_route_handler): add unit test for autoware route handler function (`#8271 `_) - * remove unused functions in route handler - * add docstring for function getShoulderLaneletsAtPose - * update test map to include shoulder lanelet - * add unit test for function getShoulderLaneletsAtPose - * add test case for getCenterLinePath to improve branch coverage - --------- -* feat(autoware_test_utils): add qos handler in pub/sub (`#7856 `_) - * feat: add qos handler in pub/sub - * style(pre-commit): autofix - * feat: update test_pub_msg function to not use setpublisher function - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* feat: add `autoware\_` prefix to `lanelet2_extension` (`#7640 `_) -* feat(auoware_test_utils): add jump_clock interface (`#7638 `_) - * feat(auoware_test_utils): add jump_clock interface - * add comment - --------- -* feat(route_handler): add unit test for lane change related functions (`#7504 `_) - * RT1-6230 feat(route_handler): add unit test for lane change related functions - * fix spell check - * fix spellcheck - --------- -* feat(autoware_test_utils): add autoware test manager (`#7597 `_) - * feat(detected_object_validation): add test - * move to autoware_test_utils - * remove perception - * update cmake - * style(pre-commit): autofix - * remove perception change - * add include - * refactored - * avoid using void and static_pointer_cast - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* refactor(universe_utils/motion_utils)!: add autoware namespace (`#7594 `_) -* feat(autoware_universe_utils)!: rename from tier4_autoware_utils (`#7538 `_) - Co-authored-by: kosuke55 -* refactor(autoware_test_utils): function to load paths from folder (`#7474 `_) -* fix(route_handler): route handler overlap removal is too conservative (`#7156 `_) - * add flag to enable/disable loop check in getLaneletSequence functions - * implement function to get closest route lanelet based on previous closest lanelet - * refactor DefaultPlanner::plan function - * modify loop check logic in getLaneletSequenceUpTo function - * improve logic in isEgoOutOfRoute function - * fix format - * check if prev lanelet is a goal lanelet in getLaneletSequenceUpTo function - * separate function to update current route lanelet in planner manager - * rename function and add docstring - * modify functions extendNextLane and extendPrevLane to account for overlap - * refactor function getClosestRouteLaneletFromLanelet - * add route handler unit tests for overlapping route case - * fix function getClosestRouteLaneletFromLanelet - * format fix - * move test map to autoware_test_utils - --------- -* refactor(test_utils): move to common folder (`#7158 `_) - * Move autoware planning test manager to autoware namespace - * fix package share directory for behavior path planner - * renaming files and directory - * rename variables that has planning_test_utils in its name. - * use autoware namespace for test utils - * move folder to common - * update .pages file - * fix test error - * removed obstacle velocity limiter test artifact - * remove namespace from planning validator, it has using keyword - --------- -* Contributors: Esteve Fernandez, Go Sakayori, Kosuke Takeuchi, Mamoru Sobue, Nagi70, Ryuta Kambe, Takayuki Murooka, Tim Clephas, Yoshi Ri, Yutaka Kondo, Zulfaqar Azmi, kminoda, kobayu858, mkquda - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_test_utils/CMakeLists.txt b/common/autoware_test_utils/CMakeLists.txt deleted file mode 100644 index 20b8f964f879f..0000000000000 --- a/common/autoware_test_utils/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -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 -) diff --git a/common/autoware_test_utils/README.md b/common/autoware_test_utils/README.md deleted file mode 100644 index 54b67d279eaac..0000000000000 --- a/common/autoware_test_utils/README.md +++ /dev/null @@ -1,106 +0,0 @@ -# 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 deleted file mode 100644 index 3c97143247e96..0000000000000 --- a/common/autoware_test_utils/config/path_with_lane_id_data.yaml +++ /dev/null @@ -1,973 +0,0 @@ -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 deleted file mode 100644 index 8122d6e5f4a4d..0000000000000 --- a/common/autoware_test_utils/config/sample_topic_snapshot.yaml +++ /dev/null @@ -1,32 +0,0 @@ -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 deleted file mode 100644 index 6c547c8cd83dc..0000000000000 --- a/common/autoware_test_utils/config/test_common.param.yaml +++ /dev/null @@ -1,17 +0,0 @@ -/**: - 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 deleted file mode 100644 index eb6709764ce3e..0000000000000 --- a/common/autoware_test_utils/config/test_nearest_search.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - 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 deleted file mode 100644 index 185ecb9e50a7e..0000000000000 --- a/common/autoware_test_utils/config/test_vehicle_info.param.yaml +++ /dev/null @@ -1,13 +0,0 @@ -# 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 deleted file mode 100644 index 297dc5a43865e..0000000000000 Binary files a/common/autoware_test_utils/images/2km-test.png and /dev/null differ diff --git a/common/autoware_test_utils/images/2km-test.svg b/common/autoware_test_utils/images/2km-test.svg deleted file mode 100644 index 26632cdde9118..0000000000000 --- a/common/autoware_test_utils/images/2km-test.svg +++ /dev/null @@ -1,505 +0,0 @@ - - - - - - - - - - - - - - - - - - -
-
-
- 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 deleted file mode 100644 index 340437b53c0f5..0000000000000 Binary files a/common/autoware_test_utils/images/common.png and /dev/null differ diff --git a/common/autoware_test_utils/images/intersection_test_map.png b/common/autoware_test_utils/images/intersection_test_map.png deleted file mode 100644 index a2f3db6ad2a31..0000000000000 Binary files a/common/autoware_test_utils/images/intersection_test_map.png and /dev/null 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 deleted file mode 100644 index 0815bd93538a5..0000000000000 Binary files a/common/autoware_test_utils/images/road_shoulder_test_map.png and /dev/null 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 deleted file mode 100644 index 70be3880fe7e4..0000000000000 --- a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp +++ /dev/null @@ -1,651 +0,0 @@ -// 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 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; -using tier4_planning_msgs::msg::Scenario; - -/** - * @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 deleted file mode 100644 index e4cba1f1a82a5..0000000000000 --- a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp +++ /dev/null @@ -1,228 +0,0 @@ -// 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 deleted file mode 100644 index 9186785ba6145..0000000000000 --- a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp +++ /dev/null @@ -1,328 +0,0 @@ -// 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::universe_utils::getRPY(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 deleted file mode 100644 index dc3e75cca1d96..0000000000000 --- a/common/autoware_test_utils/launch/psim_intersection.launch.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml b/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml deleted file mode 100644 index 58b71524e72b8..0000000000000 --- a/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml deleted file mode 100644 index 03846e620bedc..0000000000000 --- a/common/autoware_test_utils/package.xml +++ /dev/null @@ -1,47 +0,0 @@ - - - - autoware_test_utils - 0.41.0 - ROS 2 node for testing interface of the nodes in planning module - Kyoichi Sugahara - Takamasa Horibe - Zulfaqar Azmi - Mamoru Sobue - Yukinari Hisaki - Apache License 2.0 - - Kyoichi Sugahara - - ament_cmake_auto - autoware_cmake - - ament_index_cpp - autoware_component_interface_specs_universe - autoware_component_interface_utils - autoware_control_msgs - autoware_lanelet2_extension - autoware_map_msgs - autoware_perception_msgs - autoware_planning_msgs - autoware_pyplot - autoware_universe_utils - autoware_vehicle_msgs - lanelet2_io - nav_msgs - rclcpp - std_srvs - tf2_msgs - tf2_ros - tier4_planning_msgs - unique_identifier_msgs - yaml_cpp_vendor - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp deleted file mode 100644 index 08c4a3b5e9264..0000000000000 --- a/common/autoware_test_utils/src/autoware_test_utils.cpp +++ /dev/null @@ -1,348 +0,0 @@ -// 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::universe_utils::createPoint; -using autoware::universe_utils::createQuaternionFromRPY; -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 = createPoint(x, y, z); - p.orientation = createQuaternionFromRPY(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 deleted file mode 100644 index 29aed75357441..0000000000000 --- a/common/autoware_test_utils/src/mock_data_parser.cpp +++ /dev/null @@ -1,504 +0,0 @@ -// 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