diff --git a/planning/autoware_static_centerline_generator/CHANGELOG.rst b/planning/autoware_static_centerline_generator/CHANGELOG.rst new file mode 100644 index 00000000..0338cdc8 --- /dev/null +++ b/planning/autoware_static_centerline_generator/CHANGELOG.rst @@ -0,0 +1,287 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_static_centerline_generator +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +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(static_centerline_generator): several bug fixes (`#9426 `_) + * fix: dependent packages + * feat: use steer angle, use warn for steer angle failure, calc curvature dicontinuously + * fix cppcheck + --------- +* 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 `_) +* fix(cpplint): include what you use - planning (`#9570 `_) +* refactor(global_parameter_loader): prefix package and namespace with autoware (`#9303 `_) +* feat!: replace tier4_map_msgs with autoware_map_msgs for MapProjectorInfo (`#9392 `_) +* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) + * make lanelet2_map_visualization independent + * remove unused files + * remove unused package + * fix package name + * add autoware\_ prefix + * add autoware to exec name + * add autoware prefix + * removed unnecessary dependency + --------- +* 0.39.0 +* update changelog +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* fix(static_centerline_generator): map_tf_generator package name needs update (`#9383 `_) + fix map_tf_generator name in autoware_static_centerline_generator.launch +* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) + * make lanelet2_map_visualization independent + * remove unused files + * remove unused package + * fix package name + * add autoware\_ prefix + * add autoware to exec name + * add autoware prefix + * removed unnecessary dependency + --------- +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* 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 + --------- +* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Masaki Baba, Ryohsuke Mitsudome, Takayuki Murooka, Yutaka Kondo, Zhanhong Yan + +0.39.0 (2024-11-25) +------------------- +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* 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 + --------- +* Contributors: Esteve Fernandez, Yutaka Kondo + +0.38.0 (2024-11-08) +------------------- +* unify package.xml version to 0.37.0 +* refactor(osqp_interface): added autoware prefix to osqp_interface (`#8958 `_) +* refactor(autoware_interpolation): prefix package and namespace with autoware (`#8088 `_) + Co-authored-by: kosuke55 +* fix(other_planning_packages): align the parameters with launcher (`#8793 `_) + * parameters in planning/others aligned + * update json + --------- +* refactor(map_projection_loader)!: prefix package and namespace with autoware (`#8420 `_) + * add autoware\_ prefix + * add autoware\_ prefix + --------- + Co-authored-by: SakodaShintaro +* fix(autoware_static_centerline_generator): fix unusedFunction (`#8647 `_) + * fix:unusedFunction + * fix:unusedFunction + * fix:compile error + --------- +* refactor(geography_utils): prefix package and namespace with autoware (`#7790 `_) + * refactor(geography_utils): prefix package and namespace with autoware + * move headers to include/autoware/ + --------- +* fix(autoware_static_centerline_generator): fix funcArgNamesDifferent (`#8019 `_) + fix:funcArgNamesDifferent +* fix(static_centerline_generator): save_map only once (`#7770 `_) +* refactor(static_centerline_optimizer): clean up the code (`#7756 `_) +* feat: add `autoware\_` prefix to `lanelet2_extension` (`#7640 `_) +* feat(static_centerline_generator): organize AUTO/GUI/VMB modes (`#7432 `_) +* refactor(universe_utils/motion_utils)!: add autoware namespace (`#7594 `_) +* refactor(motion_utils)!: add autoware prefix and include dir (`#7539 `_) + refactor(motion_utils): add autoware prefix and include dir +* feat(autoware_universe_utils)!: rename from tier4_autoware_utils (`#7538 `_) + Co-authored-by: kosuke55 +* refactor(route_handler)!: rename to include/autoware/{package_name} (`#7530 `_) + refactor(route_handler)!: rename to include/autoware/{package_name} +* feat(map_loader): add waypoints flag (`#7480 `_) + * feat(map_loader): handle centelrine and waypoints + * update README + * fix doc + * update schema + * fix + * fix + --------- +* feat(path_optimizer): rename to include/autoware/{package_name} (`#7529 `_) +* feat(path_smoother): rename to include/autoware/{package_name} (`#7527 `_) + * feat(path_smoother): rename to include/autoware/{package_name} + * fix + --------- +* refactor(behaivor_path_planner)!: rename to include/autoware/{package_name} (`#7522 `_) + * refactor(behavior_path_planner)!: make autoware dir in include + * refactor(start_planner): make autoware include dir + * refactor(goal_planner): make autoware include dir + * sampling planner module + * fix sampling planner build + * dynamic_avoidance + * lc + * side shift + * autoware_behavior_path_static_obstacle_avoidance_module + * autoware_behavior_path_planner_common + * make behavior_path dir + * pre-commit + * fix pre-commit + * fix build + --------- +* feat(mission_planner): rename to include/autoware/{package_name} (`#7513 `_) + * feat(mission_planner): rename to include/autoware/{package_name} + * feat(mission_planner): rename to include/autoware/{package_name} + * feat(mission_planner): rename to include/autoware/{package_name} + --------- +* fix(static_centerline_generator): fix dependency (`#7442 `_) + * fix: deps + * fix: package name + * fix: package name + --------- +* refactor(route_handler): route handler add autoware prefix (`#7341 `_) + * rename route handler package + * update packages dependencies + * update include guards + * update includes + * put in autoware namespace + * fix formats + * keep header and source file name as before + --------- +* refactor(mission_planner)!: add autoware prefix and namespace (`#7414 `_) + * refactor(mission_planner)!: add autoware prefix and namespace + * fix svg + --------- +* refactor(vehicle_info_utils)!: prefix package and namespace with autoware (`#7353 `_) + * chore(autoware_vehicle_info_utils): rename header + * chore(bpp-common): vehicle info + * chore(path_optimizer): vehicle info + * chore(velocity_smoother): vehicle info + * chore(bvp-common): vehicle info + * chore(static_centerline_generator): vehicle info + * chore(obstacle_cruise_planner): vehicle info + * chore(obstacle_velocity_limiter): vehicle info + * chore(mission_planner): vehicle info + * chore(obstacle_stop_planner): vehicle info + * chore(planning_validator): vehicle info + * chore(surround_obstacle_checker): vehicle info + * chore(goal_planner): vehicle info + * chore(start_planner): vehicle info + * chore(control_performance_analysis): vehicle info + * chore(lane_departure_checker): vehicle info + * chore(predicted_path_checker): vehicle info + * chore(vehicle_cmd_gate): vehicle info + * chore(obstacle_collision_checker): vehicle info + * chore(operation_mode_transition_manager): vehicle info + * chore(mpc): vehicle info + * chore(control): vehicle info + * chore(common): vehicle info + * chore(perception): vehicle info + * chore(evaluator): vehicle info + * chore(freespace): vehicle info + * chore(planning): vehicle info + * chore(vehicle): vehicle info + * chore(simulator): vehicle info + * chore(launch): vehicle info + * chore(system): vehicle info + * chore(sensing): vehicle info + * fix(autoware_joy_controller): remove unused deps + --------- +* refactor(path_smoother)!: prefix package and namespace with autoware (`#7381 `_) + * git mv + * fix + * fix launch + * rever a part of prefix + * fix test + * fix + * fix static_centerline_optimizer + * fix + --------- +* refactor(path_optimizer, velocity_smoother)!: prefix package and namespace with autoware (`#7354 `_) + * chore(autoware_velocity_smoother): update namespace + * chore(autoware_path_optimizer): update namespace + --------- +* chore(bpp): add prefix `autoware\_` (`#7288 `_) + * chore(common): rename package + * fix(static_obstacle_avoidance): fix header + * fix(dynamic_obstacle_avoidance): fix header + * fix(side_shift): fix header + * fix(sampling_planner): fix header + * fix(start_planner): fix header + * fix(goal_planner): fix header + * fix(lane_change): fix header + * fix(external_lane_change): fix header + * fix(AbLC): fix header + * fix(bpp-node): fix header + * fix(static_centerline_generator): fix header + * fix(.pages): update link + --------- +* feat!: replace autoware_auto_msgs with autoware_msgs for planning modules (`#7246 `_) + Co-authored-by: Cynthia Liu + Co-authored-by: NorahXiong + Co-authored-by: beginningfan +* chore(autoware_velocity_smoother, autoware_path_optimizer): rename packages (`#7202 `_) + * chore(autoware_path_optimizer): rename package and namespace + * chore(autoware_static_centerline_generator): rename package and namespace + * chore: update module name + * chore(autoware_velocity_smoother): rename package and namespace + * chore(tier4_planning_launch): update module name + * chore: update module name + * fix: test + * fix: test + * fix: test + --------- +* refactor(behavior_velocity_planner)!: prefix package and namespace with autoware\_ (`#6693 `_) +* fix(autoware_static_centerline_generator): update the centerline correctly with map projector (`#6825 `_) + * fix(static_centerline_generator): fixed the bug of offset lat/lon values + * fix typo + --------- +* fix(autoware_static_centerline_generator): remove prefix from topics and node names (`#7028 `_) +* build(static_centerline_generator): prefix package and namespace with autoware\_ (`#6817 `_) + * build(static_centerline_generator): prefix package and namespace with autoware\_ + * style(pre-commit): autofix + * build: fix CMake target + * build(autoware_static_centerline_generator): more renames + * style(pre-commit): autofix + * build(autoware_static_centerline_generator): fix namespace + * fix(autoware_static_centerline_generator): fix clang-tidy issues + * style(pre-commit): autofix + * style(pre-commit): autofix + * fix(autoware_static_centerline_generator): fix clang-tidy issues + * fix(autoware_static_centerline_generator): fix build issues + * fix(autoware_static_centerline_generator): fix build issues + * style(pre-commit): autofix + * fix(autoware_static_centerline_optimizer): fix clang-tidy issues + * style(pre-commit): autofix + * build: fix build errors + * fix: remove else statements after return + * fix(autoware_static_centerline_generator): fix clang-tidy issues + * style(pre-commit): autofix + * revert changes for static_centerline_generator + * fix(autoware_static_centerline_generator): add autoware\_ prefix + * style(pre-commit): autofix + * fix(autoware_static_centerline_generator): fix filenames + * fix(autoware_static_centerline_generator): fix namespaces + * style(pre-commit): autofix + * fix: added prefix to missing strings + * refactor(autoware_static_centerline_generator): move header files to src + * refactor(autoware_static_centerline_generator): fix include paths + * style(pre-commit): autofix + * refactor(autoware_static_centerline_generator): rename base folder + * Update planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml + Co-authored-by: M. Fatih Cırıt + * build(autoware_static_centerline_generator): fix include in CMake + * build(autoware_static_centerline_generator): fix missing includes + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: M. Fatih Cırıt +* Contributors: Esteve Fernandez, Kosuke Takeuchi, Masaki Baba, Ryohsuke Mitsudome, Satoshi OTA, Takayuki Murooka, Yutaka Kondo, Zhe Shen, kobayu858, mkquda + +0.26.0 (2024-04-03) +------------------- diff --git a/planning/autoware_static_centerline_generator/CMakeLists.txt b/planning/autoware_static_centerline_generator/CMakeLists.txt new file mode 100644 index 00000000..261e8beb --- /dev/null +++ b/planning/autoware_static_centerline_generator/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_static_centerline_generator) + +find_package(autoware_cmake REQUIRED) + +find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces( + autoware_static_centerline_generator + "srv/LoadMap.srv" + "srv/PlanRoute.srv" + "srv/PlanPath.srv" + "msg/PointsWithLaneId.msg" + DEPENDENCIES builtin_interfaces geometry_msgs +) + +autoware_package() + +ament_auto_add_executable(main + src/main.cpp + src/static_centerline_generator_node.cpp + src/centerline_source/optimization_trajectory_based_centerline.cpp + src/centerline_source/bag_ego_trajectory_based_centerline.cpp + src/utils.cpp +) + +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(main + autoware_static_centerline_generator "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target autoware_static_centerline_generator "rosidl_typesupport_cpp") + target_link_libraries(main "${cpp_typesupport_target}") +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config + rviz +) + +target_include_directories(main PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src +) + +if(BUILD_TESTING) + add_launch_test( + test/test_static_centerline_generator.test.py + TIMEOUT "30" + ) + install(DIRECTORY + test/data/ + DESTINATION share/${PROJECT_NAME}/test/data/ + ) +endif() + +install(PROGRAMS + scripts/app.py + scripts/centerline_updater_helper.py + scripts/show_lanelet2_map_diff.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/autoware_static_centerline_generator/README.md b/planning/autoware_static_centerline_generator/README.md new file mode 100644 index 00000000..426d5487 --- /dev/null +++ b/planning/autoware_static_centerline_generator/README.md @@ -0,0 +1,83 @@ +# Static Centerline Generator + +## Purpose + +This package statically calculates the centerline satisfying path footprints inside the drivable area. + +On narrow-road driving, the default centerline, which is the middle line between lanelets' right and left boundaries, often causes path footprints outside the drivable area. +To make path footprints inside the drivable area, we use online path shape optimization by [the autoware_path_optimizer package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/autoware_path_optimizer/). + +Instead of online path shape optimization, we introduce static centerline optimization. +With this static centerline optimization, we have following advantages. + +- We can see the optimized centerline shape in advance. + - With the default autoware, path shape is not determined until the vehicle drives there. + - This enables offline path shape evaluation. +- We do not have to calculate a heavy and sometimes unstable path optimization since the path footprints are already inside the drivable area. + +## Use cases + +There are two interfaces to communicate with the centerline optimizer. + +### Vector Map Builder Interface + +Note: This function of Vector Map Builder has not been released. Please wait for a while. +Currently there is no documentation about Vector Map Builder's operation for this function. + +The optimized centerline can be generated from Vector Map Builder's operation. + +We can run + +- path planning server +- http server to connect path planning server and Vector Map Builder + +with the following command by designating `` + +```sh +ros2 launch autoware_static_centerline_generator run_planning_server.launch.xml vehicle_model:= +``` + +FYI, port ID of the http server is 4010 by default. + +### Command Line Interface + +The optimized centerline can be generated from the command line interface by designating + +- `` +- `` (not mandatory) +- `` +- `` +- `` + +```sh +ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= +``` + +The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`. +If you want to change the output map path, you can remap the path by designating ``. + +## Visualization + +When launching the path planning server, rviz is launched as well as follows. +![rviz](./media/rviz.png) + +- The yellow footprints are the original ones from the osm map file. + - FYI: Footprints are generated based on the centerline and vehicle size. +- The red footprints are the optimized ones. +- The gray area is the drivable area. +- You can see that the red footprints are inside the drivable area although the yellow ones are outside. + +### Unsafe footprints + +Sometimes the optimized centerline footprints are close to the lanes' boundaries. +We can check how close they are with `unsafe footprints` marker as follows. + +Footprints' color depends on its distance to the boundaries, and text expresses its distance. + +![rviz](./media/unsafe_footprints.png) + +By default, footprints' color is + +- when the distance is less than 0.1 [m] : red +- when the distance is less than 0.2 [m] : green +- when the distance is less than 0.3 [m] : blue diff --git a/planning/autoware_static_centerline_generator/config/common.param.yaml b/planning/autoware_static_centerline_generator/config/common.param.yaml new file mode 100644 index 00000000..6c547c8c --- /dev/null +++ b/planning/autoware_static_centerline_generator/config/common.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + # constraints param for normal driving + max_vel: 11.1 # max velocity limit [m/s] + + normal: + min_acc: -1.0 # min deceleration [m/ss] + max_acc: 1.0 # max acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + # constraints to be observed + limit: + min_acc: -2.5 # min deceleration limit [m/ss] + max_acc: 1.0 # max acceleration limit [m/ss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml b/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml new file mode 100644 index 00000000..eb670976 --- /dev/null +++ b/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + # ego + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml b/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml new file mode 100644 index 00000000..06059080 --- /dev/null +++ b/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + marker_color: ["FF0000", "FF5A00", "FFFF00"] + marker_color_dist_thresh : [0.1, 0.2, 0.3] + output_trajectory_interval: 1.0 + + validation: + dist_threshold_to_road_border: 0.0 + max_steer_angle_margin: 0.0 # [rad] NOTE: Positive value makes max steer angle threshold to decrease. diff --git a/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml b/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml new file mode 100644 index 00000000..8941b92b --- /dev/null +++ b/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + wheel_radius: 0.39 + wheel_width: 0.42 + wheel_base: 2.74 # between front wheel center and rear wheel center + wheel_tread: 1.63 # between left wheel center and right wheel center + front_overhang: 1.0 # between front wheel center and vehicle front + rear_overhang: 1.03 # between rear wheel center and vehicle rear + left_overhang: 0.1 # between left wheel center and vehicle left + right_overhang: 0.1 # between right wheel center and vehicle right + vehicle_height: 2.5 + max_steer_angle: 0.70 # [rad] diff --git a/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml b/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml new file mode 100644 index 00000000..cb368ca3 --- /dev/null +++ b/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml new file mode 100644 index 00000000..0590009a --- /dev/null +++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/autoware_static_centerline_generator/media/rviz.png b/planning/autoware_static_centerline_generator/media/rviz.png new file mode 100644 index 00000000..e76ede78 Binary files /dev/null and b/planning/autoware_static_centerline_generator/media/rviz.png differ diff --git a/planning/autoware_static_centerline_generator/media/unsafe_footprints.png b/planning/autoware_static_centerline_generator/media/unsafe_footprints.png new file mode 100644 index 00000000..b4404f4c Binary files /dev/null and b/planning/autoware_static_centerline_generator/media/unsafe_footprints.png differ diff --git a/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg b/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg new file mode 100644 index 00000000..97cd0355 --- /dev/null +++ b/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg @@ -0,0 +1,2 @@ +int64 lane_id +geometry_msgs/Point[] points diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml new file mode 100644 index 00000000..cea18e56 --- /dev/null +++ b/planning/autoware_static_centerline_generator/package.xml @@ -0,0 +1,57 @@ + + + + autoware_static_centerline_generator + 0.40.0 + The autoware_static_centerline_generator package + Takayuki Murooka + Kosuke Takeuchi + Apache License 2.0 + + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + rosidl_default_generators + + autoware_behavior_path_planner_common + autoware_geography_utils + autoware_global_parameter_loader + autoware_interpolation + autoware_lanelet2_extension + autoware_lanelet2_map_visualizer + autoware_map_loader + autoware_map_msgs + autoware_map_projection_loader + autoware_map_tf_generator + autoware_mission_planner + autoware_motion_utils + autoware_osqp_interface + autoware_path_optimizer + autoware_path_smoother + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_universe_utils + autoware_vehicle_info_utils + geometry_msgs + rclcpp + rclcpp_components + + python3-flask-cors + rosidl_default_runtime + + ament_cmake_gmock + ament_lint_auto + autoware_behavior_path_planner + autoware_behavior_velocity_planner + autoware_lint_common + ros_testing + + rosidl_interface_packages + + + ament_cmake + + diff --git a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz new file mode 100644 index 00000000..f1bd1107 --- /dev/null +++ b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz @@ -0,0 +1,459 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.550000011920929 + Tree Height: 386 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Dummy Car1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: rviz_plugins::AutowareStatePanel + Name: AutowareStatePanel +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + Enabled: false + Name: System + - Class: rviz_common/Group + Displays: + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 28.71826171875 + Min Value: -7.4224700927734375 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 237 + Min Color: 211; 215; 207 + Min Intensity: 0 + Name: PointCloudMap + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/pointcloud_map + Use Fixed Frame: true + Use rainbow: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Lanelet2VectorMap + Namespaces: + center_lane_line: true + center_line_arrows: true + lane_start_bound: false + lanelet_id: false + left_lane_bound: true + right_lane_bound: true + road_lanelets: false + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/vector_map_marker + Value: true + Enabled: true + Name: Map + - Class: rviz_plugins/PathWithLaneId + Color Border Vel Max: 3 + Enabled: false + Name: Raw Centerline + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /static_centerline_generator/input_centerline + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: true + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View LaneId: + Scale: 0.10000000149011612 + Value: false + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: false + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: Optimized Centerline + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /static_centerline_generator/output/centerline + Value: true + View Footprint: + Alpha: 0.5 + Color: 0; 255; 0 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: true + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: false + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: Raw Centerline (Path type) + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /static_centerline_generator/debug/raw_centerline + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: true + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /static_centerline_generator/output/validation_results + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Debug Markers + Namespaces: + curvature: false + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /static_centerline_generator/debug/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /static_centerline_generator/debug/ego_footprint_bounds + Value: true + Enabled: true + Name: debug + Enabled: true + Global Options: + Background Color: 10; 10; 10 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/goal + - Acceleration: 0 + Class: rviz_plugins/PedestrianInitialPoseTool + Interactive: false + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 1 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/CarInitialPoseTool + H vehicle height: 2 + Interactive: false + L vehicle length: 4 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 8 + W vehicle width: 1.7999999523162842 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/BusInitialPoseTool + H vehicle height: 3.5 + Interactive: false + L vehicle length: 10.5 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + W vehicle width: 2.5 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/MissionCheckpointTool + Pose Topic: /planning/mission_planning/checkpoint + Theta std deviation: 0.2617993950843811 + X std deviation: 0.5 + Y std deviation: 0.5 + Z position: 0 + - Class: rviz_plugins/DeleteAllObjectsTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: + - Class: rviz_default_plugins/ThirdPersonFollower + Distance: 18 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.20000000298023224 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 3.141592025756836 + - Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 +Window Geometry: + AutowareStatePanel: + collapsed: false + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 0 diff --git a/planning/autoware_static_centerline_generator/scripts/app.py b/planning/autoware_static_centerline_generator/scripts/app.py new file mode 100755 index 00000000..08bff8b8 --- /dev/null +++ b/planning/autoware_static_centerline_generator/scripts/app.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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. + +import json +import uuid + +from autoware_static_centerline_generator.srv import LoadMap +from autoware_static_centerline_generator.srv import PlanPath +from autoware_static_centerline_generator.srv import PlanRoute +from flask import Flask +from flask import jsonify +from flask import request +from flask_cors import CORS +import rclpy +from rclpy.node import Node + +rclpy.init() +node = Node("static_centerline_generator_http_server") + +app = Flask(__name__) +CORS(app) + + +def create_client(service_type, server_name): + # create client + cli = node.create_client(service_type, server_name) + while not cli.wait_for_service(timeout_sec=1.0): + print("{} service not available, waiting again...".format(server_name)) + return cli + + +@app.route("/map", methods=["POST"]) +def get_map(): + data = request.get_json() + + map_uuid = str(uuid.uuid4()) + global map_id + map_id = map_uuid + + # create client + cli = create_client(LoadMap, "/planning/static_centerline_generator/load_map") + + # request map loading + req = LoadMap.Request(map=data["map"]) + future = cli.call_async(req) + + # get result + rclpy.spin_until_future_complete(node, future) + res = future.result() + + # error handling + if res.message == "InvalidMapFormat": + return jsonify(code=res.message, message="Map format is invalid."), 400 + elif res.message != "": + return ( + jsonify( + code="InternalServerError", + message="Error occurred on the server. Please check the terminal.", + ), + 500, + ) + + return map_uuid + + +@app.route("/planned_route", methods=["GET"]) +def post_planned_route(): + args = request.args.to_dict() + global map_id + if map_id != args.get("map_id"): + # TODO(murooka) error handling for map_id mismatch + print("map_id is not correct.") + + # create client + cli = create_client(PlanRoute, "/planning/static_centerline_generator/plan_route") + + # request route planning + req = PlanRoute.Request( + start_lane_id=int(args.get("start_lane_id")), end_lane_id=int(args.get("end_lane_id")) + ) + future = cli.call_async(req) + + # get result + rclpy.spin_until_future_complete(node, future) + res = future.result() + + # error handling + if res.message == "MapNotFound": + return jsonify(code=res.message, message="Map is missing."), 404 + elif res.message == "RouteNotFound": + return jsonify(code=res.message, message="Planning route failed."), 404 + elif res.message != "": + return ( + jsonify( + code="InternalServerError", + message="Error occurred on the server. Please check the terminal.", + ), + 500, + ) + + return json.dumps(tuple(res.lane_ids)) + + +@app.route("/planned_path", methods=["GET"]) +def post_planned_path(): + args = request.args.to_dict() + global map_id + if map_id != args.get("map_id"): + # TODO(murooka) error handling for map_id mismatch + print("map_id is not correct.") + + # create client + cli = create_client(PlanPath, "/planning/static_centerline_generator/plan_path") + + # request path planning + route_lane_ids = [eval(i) for i in request.args.getlist("route[]")] + req = PlanPath.Request(route=route_lane_ids) + future = cli.call_async(req) + + # get result + rclpy.spin_until_future_complete(node, future) + res = future.result() + + # error handling + if res.message == "MapNotFound": + return jsonify(code=res.message, message="Map is missing."), 404 + elif res.message == "LaneletsNotConnected": + return ( + jsonify( + code=res.message, + message="Lanelets are not connected.", + object_ids=tuple(res.unconnected_lane_ids), + ), + 400, + ) + elif res.message != "": + return ( + jsonify( + code="InternalServerError", + message="Error occurred on the server. Please check the terminal.", + ), + 500, + ) + + # create output json + result_json = [] + for points_with_lane_id in res.points_with_lane_ids: + current_lane_points = [] + for geom_point in points_with_lane_id.points: + point = {"x": geom_point.x, "y": geom_point.y, "z": geom_point.z} + current_lane_points.append(point) + + current_result_json = {} + current_result_json["lane_id"] = int(points_with_lane_id.lane_id) + current_result_json["points"] = current_lane_points + + result_json.append(current_result_json) + + return json.dumps(tuple(result_json)) + + +if __name__ == "__main__": + app.debug = True + app.secret_key = "tmp_secret_key" + app.run(host="localhost", port=4010) diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py new file mode 100755 index 00000000..f3d90871 --- /dev/null +++ b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py @@ -0,0 +1,207 @@ +#!/bin/env python3 + +# 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. + + +import sys +import time + +from PyQt5 import QtCore +from PyQt5.QtWidgets import QApplication +from PyQt5.QtWidgets import QGroupBox +from PyQt5.QtWidgets import QMainWindow +from PyQt5.QtWidgets import QPushButton +from PyQt5.QtWidgets import QSizePolicy +from PyQt5.QtWidgets import QSlider +from PyQt5.QtWidgets import QVBoxLayout +from PyQt5.QtWidgets import QWidget +from autoware_planning_msgs.msg import Trajectory +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile +from std_msgs.msg import Empty +from std_msgs.msg import Float32 +from std_msgs.msg import Int32 + + +class CenterlineUpdaterWidget(QMainWindow): + def __init__(self): + super(self.__class__, self).__init__() + self.setupUI() + + def setupUI(self): + self.setObjectName("MainWindow") + self.resize(480, 120) + self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint) + + central_widget = QWidget(self) + self.grid_layout = QVBoxLayout(central_widget) + self.grid_layout.setContentsMargins(10, 10, 10, 10) + + # slide of the trajectory's start and end index + self.traj_start_index_slider = QSlider(QtCore.Qt.Horizontal) + self.traj_end_index_slider = QSlider(QtCore.Qt.Horizontal) + self.min_traj_start_index = 0 + self.max_traj_end_index = None + + # Layout: Range of Centerline + centerline_vertical_box = QVBoxLayout(self) + centerline_vertical_box.addWidget(self.traj_start_index_slider) + centerline_vertical_box.addWidget(self.traj_end_index_slider) + centerline_group = QGroupBox("Centerline") + centerline_group.setLayout(centerline_vertical_box) + self.grid_layout.addWidget(centerline_group) + + """ + # 2. Road Boundary + road_boundary_group = QGroupBox("Road Boundary") + road_boundary_vertical_box = QVBoxLayout(self) + road_boundary_group.setLayout(road_boundary_vertical_box) + self.grid_layout.addWidget(road_boundary_group) + + # 2.1. Slider + self.road_boundary_lateral_margin_slider = QSlider(QtCore.Qt.Horizontal) + road_boundary_vertical_box.addWidget(self.road_boundary_lateral_margin_slider) + self.road_boundary_lateral_margin_ratio = 10 + self.road_boundary_lateral_margin_slider.setMinimum(0) + self.road_boundary_lateral_margin_slider.setMaximum( + 5 * self.road_boundary_lateral_margin_ratio + ) + road_boundary_vertical_box.addWidget(QPushButton("reset")) + """ + + # 3. General + general_group = QGroupBox("General") + general_vertical_box = QVBoxLayout(self) + general_group.setLayout(general_vertical_box) + self.grid_layout.addWidget(general_group) + + # 3.1. Validate Centerline + self.validate_button = QPushButton("validate") + self.validate_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + general_vertical_box.addWidget(self.validate_button) + + # 3.2. Save Map + self.save_map_button = QPushButton("save map") + self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + general_vertical_box.addWidget(self.save_map_button) + + self.setCentralWidget(central_widget) + + def initWithEndIndex(self, max_traj_end_index): + self.max_traj_end_index = max_traj_end_index + + # initialize sliders + self.traj_start_index_slider.setMinimum(self.min_traj_start_index) + self.traj_start_index_slider.setMaximum(self.max_traj_end_index) + self.traj_start_index_slider.setValue(self.min_traj_start_index) + + self.traj_end_index_slider.setMinimum(self.min_traj_start_index) + self.traj_end_index_slider.setMaximum(self.max_traj_end_index) + self.traj_end_index_slider.setValue(self.max_traj_end_index) + + +class CenterlineUpdaterHelper(Node): + def __init__(self): + super().__init__("centerline_updater_helper") + # Qt + self.widget = CenterlineUpdaterWidget() + self.widget.show() + self.widget.save_map_button.clicked.connect(self.onSaveMapButtonPushed) + self.widget.validate_button.clicked.connect(self.onValidateButtonPushed) + self.widget.traj_start_index_slider.valueChanged.connect(self.onStartIndexChanged) + self.widget.traj_end_index_slider.valueChanged.connect(self.onEndIndexChanged) + """ + self.widget.road_boundary_lateral_margin_slider.valueChanged.connect( + self.onRoadBoundaryLateralMargin + ) + """ + + # ROS + self.pub_save_map = self.create_publisher(Empty, "/static_centerline_generator/save_map", 1) + self.pub_validate = self.create_publisher(Empty, "/static_centerline_generator/validate", 1) + self.pub_traj_start_index = self.create_publisher( + Int32, "/static_centerline_generator/traj_start_index", 1 + ) + self.pub_traj_end_index = self.create_publisher( + Int32, "/static_centerline_generator/traj_end_index", 1 + ) + self.pub_road_boundary_lateral_margin = self.create_publisher( + Float32, "/static_centerline_generator/road_boundary_lateral_margin", 1 + ) + + transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + self.sub_whole_centerline = self.create_subscription( + Trajectory, + "/static_centerline_generator/output/whole_centerline", + self.onWholeCenterline, + transient_local_qos, + ) + + while self.widget.max_traj_end_index is None: + rclpy.spin_once(self, timeout_sec=0.01) + time.sleep(0.1) + + def onWholeCenterline(self, whole_centerline): + self.widget.initWithEndIndex(len(whole_centerline.points) - 1) + + def onSaveMapButtonPushed(self, event): + msg = Empty() + self.pub_save_map.publish(msg) + + # NOTE: After saving the map, the generated centerline is written + # in original_map_ptr_ in static_centerline_generator_node. + # When saving the map again, another centerline is written without + # removing the previous centerline. + # Therefore, saving the map can be called only once. + self.widget.save_map_button.setEnabled(False) + + def onValidateButtonPushed(self, event): + msg = Empty() + self.pub_validate.publish(msg) + + def onStartIndexChanged(self, event): + msg = Int32() + msg.data = self.widget.traj_start_index_slider.value() + self.pub_traj_start_index.publish(msg) + + def onEndIndexChanged(self, event): + msg = Int32() + msg.data = self.widget.traj_end_index_slider.value() + self.pub_traj_end_index.publish(msg) + + def onRoadBoundaryLateralMargin(self, event): + msg = Float32() + msg.data = ( + self.widget.road_boundary_lateral_margin_slider.value() + / self.widget.road_boundary_lateral_margin_ratio + ) + self.pub_road_boundary_lateral_margin.publish(msg) + + +def main(args=None): + app = QApplication(sys.argv) + + rclpy.init(args=args) + node = CenterlineUpdaterHelper() + + while True: + app.processEvents() + rclpy.spin_once(node, timeout_sec=0.01) + + +if __name__ == "__main__": + main() diff --git a/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py b/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py new file mode 100755 index 00000000..00d06ca2 --- /dev/null +++ b/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py @@ -0,0 +1,139 @@ +#!/bin/env python3 + +# 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. + +import argparse +from decimal import Decimal +import os +import subprocess +import xml.etree.ElementTree as ET + + +def sort_attributes(root): + for shallow_element in root: + # sort nodes + attrib = shallow_element.attrib + if len(attrib) > 1: + attributes = sorted(attrib.items()) + attrib.clear() + attrib.update(attributes) + if shallow_element.tag == "relation": + pass + + # sort the element in the tag + for deep_element in shallow_element: + attrib = deep_element.attrib + if len(attrib) > 1: + # adjust attribute order, e.g. by sorting + attributes = sorted(attrib.items()) + attrib.clear() + attrib.update(attributes) + + # sort tags + sorted_shallow_element = sorted(shallow_element, key=lambda x: x.items()) + if len(shallow_element) != 0: + # NOTE: This "tail" is related to the indent of the next tag + first_tail = shallow_element[0].tail + last_tail = shallow_element[-1].tail + for idx, val_shallow_element in enumerate(sorted_shallow_element): + shallow_element[idx] = val_shallow_element + if idx == len(shallow_element) - 1: + shallow_element[idx].tail = last_tail + else: + shallow_element[idx].tail = first_tail + + +def remove_diff_to_ignore(osm_root): + decimal_precision = 11 # for lat/lon values + + # remove attributes of the osm tag + osm_root.attrib.clear() + + # remove the MetaInfo tag generated by Vector Map Builder + if osm_root[0].tag == "MetaInfo": + osm_root.remove(osm_root[0]) + + # remove unnecessary attributes for diff + for osm_child in osm_root: + if osm_child.tag == "osm": + osm_child.attrib.pop("osm") + if "visible" in osm_child.attrib and osm_child.attrib["visible"]: + osm_child.attrib.pop("visible") + if "version" in osm_child.attrib and osm_child.attrib["version"]: + osm_child.attrib.pop("version") + if "action" in osm_child.attrib and osm_child.attrib["action"] == "modify": + osm_child.attrib.pop("action") + if "lat" in osm_child.attrib: + osm_child.attrib["lat"] = str( + Decimal(float(osm_child.attrib["lat"])).quantize(Decimal(f"1e-{decimal_precision}")) + ) + if "lon" in osm_child.attrib: + osm_child.attrib["lon"] = str( + Decimal(float(osm_child.attrib["lon"])).quantize(Decimal(f"1e-{decimal_precision}")) + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "-s", "--sort-attributes", action="store_true", help="Sort attributes of LL2 maps" + ) + parser.add_argument( + "-i", + "--ignore-minor-attributes", + action="store_true", + help="Ignore minor attributes of LL2 maps which does not change the map's behavior", + ) + args = parser.parse_args() + + original_osm_file_name = "/tmp/autoware_static_centerline_generator/input/lanelet2_map.osm" + modified_osm_file_name = "/tmp/autoware_static_centerline_generator/output/lanelet2_map.osm" + + # load LL2 maps + original_osm_tree = ET.parse(original_osm_file_name) + original_osm_root = original_osm_tree.getroot() + modified_osm_tree = ET.parse(modified_osm_file_name) + modified_osm_root = modified_osm_tree.getroot() + + # sort attributes + if args.sort_attributes: + sort_attributes(modified_osm_root) + sort_attributes(original_osm_root) + + # ignore minor attributes + if args.ignore_minor_attributes: + remove_diff_to_ignore(original_osm_root) + remove_diff_to_ignore(modified_osm_root) + + # write LL2 maps + output_dir_path = "/tmp/autoware_static_centerline_generator/show_lanelet2_map_diff/" + os.makedirs(output_dir_path + "original/", exist_ok=True) + os.makedirs(output_dir_path + "modified/", exist_ok=True) + + original_osm_tree.write(output_dir_path + "original/lanelet2_map.osm") + modified_osm_tree.write(output_dir_path + "modified/lanelet2_map.osm") + + # show diff + print("[INFO] Show diff of following LL2 maps") + print(f" {output_dir_path + 'original/lanelet2_map.osm'}") + print(f" {output_dir_path + 'modified/lanelet2_map.osm'}") + subprocess.run( + [ + "diff", + output_dir_path + "original/lanelet2_map.osm", + output_dir_path + "modified/lanelet2_map.osm", + "--color", + ] + ) diff --git a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh new file mode 100755 index 00000000..c170e24e --- /dev/null +++ b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-right.db3" vehicle_model:=lexus + +# ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-left-right.db3" vehicle_model:=lexus diff --git a/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh new file mode 100755 index 00000000..48809198 --- /dev/null +++ b/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="optimization_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp new file mode 100644 index 00000000..dc950f19 --- /dev/null +++ b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -0,0 +1,86 @@ +// 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 "centerline_source/bag_ego_trajectory_based_centerline.hpp" + +#include "rclcpp/serialization.hpp" +#include "rosbag2_cpp/reader.hpp" +#include "static_centerline_generator_node.hpp" + +#include + +#include +#include +#include + +namespace autoware::static_centerline_generator +{ +std::vector generate_centerline_with_bag(rclcpp::Node & node) +{ + const auto bag_filename = node.declare_parameter("bag_filename"); + + // open rosbag + rosbag2_cpp::Reader bag_reader; + bag_reader.open(bag_filename); + + // extract 2D position of ego's trajectory from rosbag + rclcpp::Serialization bag_serialization; + std::vector centerline_traj_points; + while (bag_reader.has_next()) { + const rosbag2_storage::SerializedBagMessageSharedPtr msg = bag_reader.read_next(); + + if (msg->topic_name != "/localization/kinematic_state") { + continue; + } + + rclcpp::SerializedMessage serialized_msg(*msg->serialized_data); + const auto ros_msg = std::make_shared(); + + bag_serialization.deserialize_message(&serialized_msg, ros_msg.get()); + + if (!centerline_traj_points.empty()) { + constexpr double epsilon = 1e-1; + if ( + std::abs(centerline_traj_points.back().pose.position.x - ros_msg->pose.pose.position.x) < + epsilon && + std::abs(centerline_traj_points.back().pose.position.y - ros_msg->pose.pose.position.y) < + epsilon) { + continue; + } + } + TrajectoryPoint centerline_traj_point; + centerline_traj_point.pose.position = ros_msg->pose.pose.position; + centerline_traj_points.push_back(centerline_traj_point); + } + + RCLCPP_INFO(node.get_logger(), "Extracted centerline from the bag."); + + // calculate rough orientation of centerline trajectory points + for (size_t i = 0; i < centerline_traj_points.size(); ++i) { + if (i == centerline_traj_points.size() - 1) { + if (i != 0) { + centerline_traj_points.at(i).pose.orientation = + centerline_traj_points.at(i - 1).pose.orientation; + } + } else { + const double yaw_angle = autoware::universe_utils::calcAzimuthAngle( + centerline_traj_points.at(i).pose.position, centerline_traj_points.at(i + 1).pose.position); + centerline_traj_points.at(i).pose.orientation = + autoware::universe_utils::createQuaternionFromYaw(yaw_angle); + } + } + + return centerline_traj_points; +} +} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp new file mode 100644 index 00000000..2275f881 --- /dev/null +++ b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp @@ -0,0 +1,27 @@ +// 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. + +#ifndef CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ +#define CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "type_alias.hpp" + +#include + +namespace autoware::static_centerline_generator +{ +std::vector generate_centerline_with_bag(rclcpp::Node & node); +} // namespace autoware::static_centerline_generator +#endif // CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp new file mode 100644 index 00000000..51725fb8 --- /dev/null +++ b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -0,0 +1,187 @@ +// 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 "centerline_source/optimization_trajectory_based_centerline.hpp" + +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/path_optimizer/node.hpp" +#include "autoware/path_smoother/elastic_band_smoother.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "static_centerline_generator_node.hpp" +#include "utils.hpp" + +#include +#include + +namespace autoware::static_centerline_generator +{ +namespace +{ +rclcpp::NodeOptions create_node_options() +{ + return rclcpp::NodeOptions{}; +} + +Path convert_to_path(const PathWithLaneId & path_with_lane_id) +{ + Path path; + path.header = path_with_lane_id.header; + path.left_bound = path_with_lane_id.left_bound; + path.right_bound = path_with_lane_id.right_bound; + for (const auto & point : path_with_lane_id.points) { + path.points.push_back(point.point); + } + + return path; +} + +Trajectory convert_to_trajectory(const Path & path) +{ + Trajectory traj; + for (const auto & point : path.points) { + TrajectoryPoint traj_point; + traj_point.pose = point.pose; + traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + traj_point.lateral_velocity_mps = point.lateral_velocity_mps; + traj_point.heading_rate_rps = point.heading_rate_rps; + + traj.points.push_back(traj_point); + } + return traj; +} +} // namespace + +OptimizationTrajectoryBasedCenterline::OptimizationTrajectoryBasedCenterline(rclcpp::Node & node) +{ + pub_raw_path_with_lane_id_ = + node.create_publisher("input_centerline", utils::create_transient_local_qos()); + pub_raw_path_ = + node.create_publisher("debug/raw_centerline", utils::create_transient_local_qos()); +} + +std::vector +OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( + rclcpp::Node & node, const RouteHandler & route_handler, + const std::vector & route_lane_ids) +{ + const auto route_lanelets = utils::get_lanelets_from_ids(route_handler, route_lane_ids); + + // optimize centerline inside the lane + const auto start_center_pose = utils::get_center_pose(route_handler, route_lane_ids.front()); + + // get ego nearest search parameters and resample interval in behavior_path_planner + const double ego_nearest_dist_threshold = + autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); + const double ego_nearest_yaw_threshold = + autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); + const double behavior_path_interval = + autoware::universe_utils::getOrDeclareParameter(node, "output_path_interval"); + const double behavior_vel_interval = + autoware::universe_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); + + // extract path with lane id from lanelets + const auto raw_path_with_lane_id = [&]() { + const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( + route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + return autoware::motion_utils::resamplePath( + non_resampled_path_with_lane_id, behavior_path_interval); + }(); + pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); + RCLCPP_INFO(node.get_logger(), "Calculated raw path with lane id and published."); + + // convert path with lane id to path + const auto raw_path = [&]() { + const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); + return autoware::motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); + }(); + pub_raw_path_->publish(raw_path); + RCLCPP_INFO(node.get_logger(), "Converted to path and published."); + + // smooth trajectory and road collision avoidance + const auto optimized_traj_points = optimize_trajectory(raw_path); + RCLCPP_INFO( + node.get_logger(), + "Smoothed trajectory and made it collision free with the road and published."); + + return optimized_traj_points; +} + +std::vector OptimizationTrajectoryBasedCenterline::optimize_trajectory( + const Path & raw_path) const +{ + // convert to trajectory points + const auto raw_traj_points = [&]() { + const auto raw_traj = convert_to_trajectory(raw_path); + return autoware::motion_utils::convertToTrajectoryPointArray(raw_traj); + }(); + + // create an instance of elastic band and model predictive trajectory. + const auto eb_path_smoother_ptr = + autoware::path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); + const auto mpt_optimizer_ptr = + autoware::path_optimizer::PathOptimizer(create_node_options()).getMPTOptimizer(); + + // NOTE: The optimization is executed every valid_optimized_traj_points_num points. + constexpr int valid_optimized_traj_points_num = 10; + const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; + + // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use + // warm start. + constexpr int num_initial_optimization = 2; + + std::vector whole_optimized_traj_points; + for (int virtual_ego_pose_idx = -num_initial_optimization; + virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { + // calculate virtual ego pose for the optimization + constexpr int virtual_ego_pose_offset_idx = 1; + const auto virtual_ego_pose = + raw_traj_points + .at( + valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + + virtual_ego_pose_offset_idx) + .pose; + + // smooth trajectory by elastic band in the autoware_path_smoother package + const auto smoothed_traj_points = + eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); + + // road collision avoidance by model predictive trajectory in the autoware_path_optimizer + // package + const autoware::path_optimizer::PlannerData planner_data{ + raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, + virtual_ego_pose}; + const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); + + // connect the previously and currently optimized trajectory points + for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { + const double dist = autoware::universe_utils::calcDistance2d( + whole_optimized_traj_points.at(j), optimized_traj_points.front()); + if (dist < 0.5) { + const std::vector extracted_whole_optimized_traj_points{ + whole_optimized_traj_points.begin(), + whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; + whole_optimized_traj_points = extracted_whole_optimized_traj_points; + break; + } + } + for (size_t j = 0; j < optimized_traj_points.size(); ++j) { + whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); + } + } + + return whole_optimized_traj_points; +} +} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp new file mode 100644 index 00000000..88def6fa --- /dev/null +++ b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp @@ -0,0 +1,43 @@ +// 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. + +#ifndef CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#define CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT + +#include "rclcpp/rclcpp.hpp" +#include "type_alias.hpp" + +#include + +namespace autoware::static_centerline_generator +{ +class OptimizationTrajectoryBasedCenterline +{ +public: + OptimizationTrajectoryBasedCenterline() = default; + explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node); + std::vector generate_centerline_with_optimization( + rclcpp::Node & node, const RouteHandler & route_handler, + const std::vector & route_lane_ids); + +private: + std::vector optimize_trajectory(const Path & raw_path) const; + + rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; + rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; +}; +} // namespace autoware::static_centerline_generator +// clang-format off +#endif // CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +// clang-format on diff --git a/planning/autoware_static_centerline_generator/src/main.cpp b/planning/autoware_static_centerline_generator/src/main.cpp new file mode 100644 index 00000000..71a076ee --- /dev/null +++ b/planning/autoware_static_centerline_generator/src/main.cpp @@ -0,0 +1,59 @@ +// Copyright 2022 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 "static_centerline_generator_node.hpp" + +#include +#include +#include + +int main(int argc, char * argv[]) +{ + try { + rclcpp::init(argc, argv); + + // initialize node + rclcpp::NodeOptions node_options; + auto node = + std::make_shared( + node_options); + + // get ros parameter + const auto mode = node->declare_parameter("mode"); + + // process + if (mode == "AUTO") { + node->generate_centerline(); + node->validate(); + node->save_map(); + } else if (mode == "GUI") { + node->generate_centerline(); + } else if (mode == "VMB") { + // Do nothing + } else { + throw std::invalid_argument("The `mode` is invalid."); + } + + // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local + rclcpp::spin(node); + rclcpp::shutdown(); + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return {}; + } + return 0; +} diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp new file mode 100644 index 00000000..f172e3e0 --- /dev/null +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -0,0 +1,774 @@ +// Copyright 2022 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 "static_centerline_generator_node.hpp" + +#include "autoware/map_loader/lanelet2_map_loader_node.hpp" +#include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp" +#include "autoware/map_projection_loader/map_projection_loader.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware_lanelet2_extension/utility/message_conversion.hpp" +#include "autoware_lanelet2_extension/utility/query.hpp" +#include "autoware_lanelet2_extension/utility/utilities.hpp" +#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" +#include "centerline_source/bag_ego_trajectory_based_centerline.hpp" +#include "type_alias.hpp" +#include "utils.hpp" + +#include +#include +#include +#include + +#include "std_msgs/msg/empty.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/int32.hpp" + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define RESET_TEXT "\x1B[0m" +#define RED_TEXT "\x1B[31m" +#define YELLOW_TEXT "\x1b[33m" +#define BOLD_TEXT "\x1B[1m" + +namespace autoware::static_centerline_generator +{ +namespace +{ +std::vector get_lane_ids_from_route(const LaneletRoute & route) +{ + std::vector lane_ids; + for (const auto & segment : route.segments) { + const auto & target_lanelet_id = segment.preferred_primitive.id; + lane_ids.push_back(target_lanelet_id); + } + + return lane_ids; +} + +lanelet::BasicPoint2d convert_to_lanelet_point(const geometry_msgs::msg::Point & geom_point) +{ + lanelet::BasicPoint2d point(geom_point.x, geom_point.y); + return point; +} + +LinearRing2d create_vehicle_footprint( + const geometry_msgs::msg::Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) +{ + const auto & i = vehicle_info; + + const double x_front = i.front_overhang_m + i.wheel_base_m + margin; + const double x_rear = -(i.rear_overhang_m + margin); + const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin; + const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); + + std::vector geom_points; + geom_points.push_back( + autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0).position); + geom_points.push_back( + autoware::universe_utils::calcOffsetPose(pose, x_front, y_right, 0.0).position); + geom_points.push_back( + autoware::universe_utils::calcOffsetPose(pose, x_rear, y_right, 0.0).position); + geom_points.push_back( + autoware::universe_utils::calcOffsetPose(pose, x_rear, y_left, 0.0).position); + + LinearRing2d footprint; + for (const auto & geom_point : geom_points) { + footprint.push_back(Point2d{geom_point.x, geom_point.y}); + } + footprint.push_back(footprint.back()); + + boost::geometry::correct(footprint); + + return footprint; +} + +geometry_msgs::msg::Pose get_text_pose( + const geometry_msgs::msg::Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +{ + const auto & i = vehicle_info; + + const double x_front = i.front_overhang_m + i.wheel_base_m; + const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 0.5; + + return autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0); +} + +std::array convert_hex_string_to_decimal(const std::string & hex_str_color) +{ + unsigned int hex_int_color; + std::istringstream iss(hex_str_color); + iss >> std::hex >> hex_int_color; + + unsigned int unit = 16 * 16; + unsigned int b = hex_int_color % unit; + unsigned int g = (hex_int_color - b) / unit % unit; + unsigned int r = (hex_int_color - g * unit - b) / unit / unit; + + return std::array{r / 255.0, g / 255.0, b / 255.0}; +} + +std::vector check_lanelet_connection( + const RouteHandler & route_handler, const lanelet::ConstLanelets & route_lanelets) +{ + std::vector unconnected_lane_ids; + + for (size_t i = 0; i < route_lanelets.size() - 1; ++i) { + const auto next_lanelets = route_handler.getNextLanelets(route_lanelets.at(i)); + + const bool is_connected = + std::find_if(next_lanelets.begin(), next_lanelets.end(), [&](const auto & next_lanelet) { + return next_lanelet.id() == route_lanelets.at(i + 1).id(); + }) != next_lanelets.end(); + if (!is_connected) { + unconnected_lane_ids.push_back(route_lanelets.at(i).id()); + } + } + + return unconnected_lane_ids; +} + +std_msgs::msg::Header create_header(const rclcpp::Time & now) +{ + std_msgs::msg::Header header; + header.frame_id = "map"; + header.stamp = now; + return header; +} + +std::vector resample_trajectory_points( + const std::vector & input_traj_points, const double resample_interval) +{ + // resample and calculate trajectory points' orientation + const auto input_traj = autoware::motion_utils::convertToTrajectory(input_traj_points); + auto resampled_input_traj = + autoware::motion_utils::resampleTrajectory(input_traj, resample_interval); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_input_traj); +} + +std::vector convertToGeometryPoints(const LineString2d & lanelet_points) +{ + std::vector points; + for (const auto & lanelet_point : lanelet_points) { + geometry_msgs::msg::Point point; + point.x = lanelet_point.x(); + point.y = lanelet_point.y(); + points.push_back(point); + } + return points; +} +} // namespace + +StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( + const rclcpp::NodeOptions & node_options) +: Node("static_centerline_generator", node_options) +{ + // publishers + pub_map_bin_ = + create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); + pub_whole_centerline_ = + create_publisher("~/output/whole_centerline", utils::create_transient_local_qos()); + pub_centerline_ = + create_publisher("~/output/centerline", utils::create_transient_local_qos()); + + // debug publishers + pub_validation_results_ = + create_publisher("~/validation_results", utils::create_transient_local_qos()); + pub_debug_markers_ = + create_publisher("~/debug/markers", utils::create_transient_local_qos()); + + pub_debug_ego_footprint_bounds_ = create_publisher( + "~/debug/ego_footprint_bounds", utils::create_transient_local_qos()); + + // subscribers + sub_footprint_margin_for_road_bound_ = create_subscription( + "/static_centerline_generator/road_boundary_lateral_margin", rclcpp::QoS{1}, + [this](const std_msgs::msg::Float32 & msg) { footprint_margin_for_road_bound_ = msg.data; }); + sub_traj_start_index_ = create_subscription( + "/static_centerline_generator/traj_start_index", rclcpp::QoS{1}, + [this](const std_msgs::msg::Int32 & msg) { + if (centerline_handler_.update_start_index(msg.data)) { + visualize_selected_centerline(); + } + }); + sub_traj_end_index_ = create_subscription( + "/static_centerline_generator/traj_end_index", rclcpp::QoS{1}, + [this](const std_msgs::msg::Int32 & msg) { + if (centerline_handler_.update_end_index(msg.data)) { + visualize_selected_centerline(); + } + }); + sub_save_map_ = create_subscription( + "/static_centerline_generator/save_map", rclcpp::QoS{1}, + [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { + if (!centerline_handler_.is_valid()) { + return; + } + save_map(); + }); + sub_validate_ = create_subscription( + "/static_centerline_generator/validate", rclcpp::QoS{1}, + [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { validate(); }); + + // services + callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + srv_load_map_ = create_service( + "/planning/static_centerline_generator/load_map", + std::bind( + &StaticCenterlineGeneratorNode::on_load_map, this, std::placeholders::_1, + std::placeholders::_2), + rmw_qos_profile_services_default, callback_group_); + srv_plan_route_ = create_service( + "/planning/static_centerline_generator/plan_route", + std::bind( + &StaticCenterlineGeneratorNode::on_plan_route, this, std::placeholders::_1, + std::placeholders::_2), + rmw_qos_profile_services_default, callback_group_); + srv_plan_path_ = create_service( + "/planning/static_centerline_generator/plan_path", + std::bind( + &StaticCenterlineGeneratorNode::on_plan_path, this, std::placeholders::_1, + std::placeholders::_2), + rmw_qos_profile_services_default, callback_group_); + + // vehicle info + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); + + centerline_source_ = [&]() { + const auto centerline_source_param = declare_parameter("centerline_source"); + if (centerline_source_param == "optimization_trajectory_base") { + optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); + return CenterlineSource::OptimizationTrajectoryBase; + } else if (centerline_source_param == "bag_ego_trajectory_base") { + return CenterlineSource::BagEgoTrajectoryBase; + } + throw std::logic_error( + "The centerline source is not supported in autoware_static_centerline_generator."); + }(); +} + +void StaticCenterlineGeneratorNode::visualize_selected_centerline() +{ + // publish selected centerline + const auto selected_centerline = centerline_handler_.get_selected_centerline(); + pub_centerline_->publish( + autoware::motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); + + // delete markers for validation + pub_validation_results_->publish(utils::create_delete_all_marker_array({}, now())); + pub_debug_markers_->publish(utils::create_delete_all_marker_array( + {"unsafe_footprints", "unsafe_footprints_distance"}, now())); + pub_debug_ego_footprint_bounds_->publish( + utils::create_delete_all_marker_array({"road_bounds"}, now())); +} + +void StaticCenterlineGeneratorNode::generate_centerline() +{ + // declare planning setting parameters + const auto lanelet2_input_file_path = declare_parameter("lanelet2_input_file_path"); + + // process + load_map(lanelet2_input_file_path); + const auto whole_centerline_with_route = generate_whole_centerline_with_route(); + centerline_handler_ = CenterlineHandler(whole_centerline_with_route); + + visualize_selected_centerline(); +} + +CenterlineWithRoute StaticCenterlineGeneratorNode::generate_whole_centerline_with_route() +{ + if (!route_handler_ptr_) { + RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory."); + return CenterlineWithRoute{}; + } + + // generate centerline with route + auto centerline_with_route = [&]() { + if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { + const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); + const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); + const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); + const auto optimized_centerline = + optimization_trajectory_based_centerline_.generate_centerline_with_optimization( + *this, *route_handler_ptr_, route_lane_ids); + return CenterlineWithRoute{optimized_centerline, route_lane_ids}; + } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { + const auto bag_centerline = generate_centerline_with_bag(*this); + const auto route_lane_ids = + plan_route(bag_centerline.front().pose, bag_centerline.back().pose); + return CenterlineWithRoute{bag_centerline, route_lane_ids}; + } + throw std::logic_error( + "The centerline source is not supported in autoware_static_centerline_generator."); + }(); + + // resample + const double output_trajectory_interval = declare_parameter("output_trajectory_interval"); + centerline_with_route.centerline = + resample_trajectory_points(centerline_with_route.centerline, output_trajectory_interval); + + pub_whole_centerline_->publish(autoware::motion_utils::convertToTrajectory( + centerline_with_route.centerline, create_header(this->now()))); + + return centerline_with_route; +} + +void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) +{ + // copy the input LL2 map to the temporary file for debugging + const std::string debug_input_file_dir{"/tmp/autoware_static_centerline_generator/input/"}; + std::filesystem::create_directories(debug_input_file_dir); + std::filesystem::copy( + lanelet2_input_file_path, debug_input_file_dir + "lanelet2_map.osm", + std::filesystem::copy_options::overwrite_existing); + + // load map by the map_loader package + map_bin_ptr_ = [&]() -> LaneletMapBin::ConstSharedPtr { + // load map + map_projector_info_ = std::make_unique( + autoware::map_projection_loader::load_info_from_lanelet2_map(lanelet2_input_file_path)); + const auto map_ptr = autoware::map_loader::Lanelet2MapLoaderNode::load_map( + lanelet2_input_file_path, *map_projector_info_); + if (!map_ptr) { + return nullptr; + } + + // NOTE: The original map is stored here since the centerline will be added to all the + // lanelet when lanelet::utils::overwriteLaneletCenterline is called. + original_map_ptr_ = autoware::map_loader::Lanelet2MapLoaderNode::load_map( + lanelet2_input_file_path, *map_projector_info_); + + // overwrite more dense centerline + // NOTE: overwriteLaneletsCenterlineWithWaypoints is used only in real time calculation. + lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); + + // create map bin msg + const auto map_bin_msg = autoware::map_loader::Lanelet2MapLoaderNode::create_map_bin_msg( + map_ptr, lanelet2_input_file_path, now()); + + return std::make_shared(map_bin_msg); + }(); + + // check if map_bin_ptr_ is not null pointer + if (!map_bin_ptr_) { + RCLCPP_ERROR(get_logger(), "Loading map failed"); + return; + } + RCLCPP_INFO(get_logger(), "Loaded map."); + + // publish map bin msg + pub_map_bin_->publish(*map_bin_ptr_); + RCLCPP_INFO(get_logger(), "Published map."); + + // create route_handler + route_handler_ptr_ = std::make_shared(); + route_handler_ptr_->setMap(*map_bin_ptr_); +} + +void StaticCenterlineGeneratorNode::on_load_map( + const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response) +{ + const std::string tmp_lanelet2_input_file_path = "/tmp/input_lanelet2_map.osm"; + + // save map file temporarily since load map's input must be a file + std::ofstream map_writer; + map_writer.open(tmp_lanelet2_input_file_path, std::ios::out); + map_writer << request->map; + map_writer.close(); + + // load map from the saved map file + load_map(tmp_lanelet2_input_file_path); + + if (map_bin_ptr_) { + return; + } + + response->message = "InvalidMapFormat"; +} + +std::vector StaticCenterlineGeneratorNode::plan_route_by_lane_ids( + const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id) +{ + if (!route_handler_ptr_) { + RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); + return std::vector{}; + } + + const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); + const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); + return plan_route(start_center_pose, end_center_pose); +} + +std::vector StaticCenterlineGeneratorNode::plan_route( + const geometry_msgs::msg::Pose & start_center_pose, + const geometry_msgs::msg::Pose & end_center_pose) +{ + if (!map_bin_ptr_) { + RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); + return std::vector{}; + } + + // plan route by the mission_planner package + const auto route = [&]() { + // calculate check points + RCLCPP_INFO(get_logger(), "Calculated check points."); + const auto check_points = + std::vector{start_center_pose, end_center_pose}; + + // create mission_planner plugin + auto plugin_loader = pluginlib::ClassLoader( + "autoware_mission_planner", "autoware::mission_planner::PlannerPlugin"); + auto mission_planner = + plugin_loader.createSharedInstance("autoware::mission_planner::lanelet2::DefaultPlanner"); + + // initialize mission_planner + auto node = rclcpp::Node("mission_planner"); + mission_planner->initialize(&node, map_bin_ptr_); + + // plan route + const auto route = mission_planner->plan(check_points); + + return route; + }(); + + // get lanelets + const auto route_lane_ids = get_lane_ids_from_route(route); + + std::string route_lane_ids_str = ""; + for (const lanelet::Id route_lane_id : route_lane_ids) { + route_lane_ids_str += std::to_string(route_lane_id) + ","; + } + RCLCPP_INFO_STREAM(get_logger(), "Planned route. (" << route_lane_ids_str << ")"); + + return route_lane_ids; +} + +void StaticCenterlineGeneratorNode::on_plan_route( + const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response) +{ + if (!map_bin_ptr_ || !route_handler_ptr_) { + response->message = "MapNotFound"; + RCLCPP_ERROR(get_logger(), "Map is not ready."); + return; + } + + const lanelet::Id start_lanelet_id = request->start_lane_id; + const lanelet::Id end_lanelet_id = request->end_lane_id; + + // plan route + const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + + // extract lane ids + std::vector lane_ids; + for (const auto & lanelet : route_lanelets) { + lane_ids.push_back(lanelet.id()); + } + + // check calculation result + if (lane_ids.empty()) { + response->message = "RouteNotFound"; + RCLCPP_ERROR(get_logger(), "Route planning failed."); + return; + } + + // set response + response->lane_ids = lane_ids; +} + +void StaticCenterlineGeneratorNode::on_plan_path( + const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response) +{ + if (!route_handler_ptr_) { + response->message = "MapNotFound"; + RCLCPP_ERROR(get_logger(), "Route handler is not ready."); + return; + } + + // get lanelets from route lane ids + const auto route_lane_ids = request->route; + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + + // check if input route lanelets are connected to each other. + const auto unconnected_lane_ids = check_lanelet_connection(*route_handler_ptr_, route_lanelets); + if (!unconnected_lane_ids.empty()) { + response->message = "LaneletsNotConnected"; + response->unconnected_lane_ids = unconnected_lane_ids; + RCLCPP_ERROR(get_logger(), "Lanelets are not connected."); + return; + } + + // plan path + const auto optimized_traj_points = + optimization_trajectory_based_centerline_.generate_centerline_with_optimization( + *this, *route_handler_ptr_, route_lane_ids); + + // check calculation result + if (optimized_traj_points.empty()) { + response->message = "PathNotFound"; + RCLCPP_ERROR(get_logger(), "Path planning failed."); + return; + } + + centerline_handler_ = + CenterlineHandler(CenterlineWithRoute{optimized_traj_points, route_lane_ids}); + + // publish unsafe_footprints + validate(); + + // create output data + auto target_traj_point = optimized_traj_points.cbegin(); + bool is_end_lanelet = false; + for (const auto & lanelet : route_lanelets) { + std::vector current_lanelet_points; + + // check if target point is inside the lanelet + while (lanelet::geometry::inside( + lanelet, convert_to_lanelet_point(target_traj_point->pose.position))) { + // memorize points inside the lanelet + current_lanelet_points.push_back(target_traj_point->pose.position); + target_traj_point++; + + if (target_traj_point == optimized_traj_points.cend()) { + is_end_lanelet = true; + break; + } + } + + if (!current_lanelet_points.empty()) { + // register points with lane_id + autoware_static_centerline_generator::msg::PointsWithLaneId points_with_lane_id; + points_with_lane_id.lane_id = lanelet.id(); + points_with_lane_id.points = current_lanelet_points; + response->points_with_lane_ids.push_back(points_with_lane_id); + } + + if (is_end_lanelet) { + break; + } + } + + // empty string if error did not occur + response->message = ""; +} + +void StaticCenterlineGeneratorNode::validate() +{ + std::cerr << std::endl + << "############################################## Validation Results " + "##############################################" + << std::endl; + + const auto & centerline = centerline_handler_.get_selected_centerline(); + const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + + const double dist_thresh_to_road_border = + getRosParameter("validation.dist_threshold_to_road_border"); + const double max_steer_angle_margin = + getRosParameter("validation.max_steer_angle_margin"); + + // calculate color for distance to road border + const auto dist_thresh_vec = getRosParameter>("marker_color_dist_thresh"); + const auto marker_color_vec = getRosParameter>("marker_color"); + const auto get_marker_color = [&](const double dist) -> boost::optional> { + for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { + const double dist_thresh = dist_thresh_vec.at(i); + if (dist < dist_thresh) { + return convert_hex_string_to_decimal(marker_color_vec.at(i)); + } + } + return boost::none; + }; + + // create right/left bound + LineString2d lanelet_right_bound; + LineString2d lanelet_left_bound; + for (const auto & lanelet : route_lanelets) { + for (const auto & point : lanelet.rightBound()) { + boost::geometry::append(lanelet_right_bound, Point2d(point.x(), point.y())); + } + for (const auto & point : lanelet.leftBound()) { + boost::geometry::append(lanelet_left_bound, Point2d(point.x(), point.y())); + } + } + + // calculate curvature + const auto curvature_vec = autoware::motion_utils::calcCurvature(centerline); + const double steer_angle_threshold = vehicle_info_.max_steer_angle_rad - max_steer_angle_margin; + + // calculate the distance between footprint and right/left bounds + MarkerArray marker_array; + double min_dist = std::numeric_limits::max(); + double max_curvature = std::numeric_limits::min(); + for (size_t i = 0; i < centerline.size(); ++i) { + const auto & traj_point = centerline.at(i); + + const auto footprint_poly = create_vehicle_footprint(traj_point.pose, vehicle_info_); + + const double dist_to_right = boost::geometry::distance(footprint_poly, lanelet_right_bound); + const double dist_to_left = boost::geometry::distance(footprint_poly, lanelet_left_bound); + const double min_dist_to_bound = std::min(dist_to_right, dist_to_left); + + if (min_dist_to_bound < min_dist) { + min_dist = min_dist_to_bound; + } + + // create marker + const auto marker_color_opt = get_marker_color(min_dist_to_bound); + const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); + if (marker_color_opt) { + const auto & marker_color = marker_color_opt.get(); + + // add footprint marker + const auto footprint_marker = utils::create_footprint_marker( + footprint_poly, 0.05, marker_color.at(0), marker_color.at(1), marker_color.at(2), 0.7, + now(), i); + marker_array.markers.push_back(footprint_marker); + + // add text of distance to bounds marker + const auto text_marker = utils::create_text_marker( + "unsafe_footprints_distance", text_pose, min_dist_to_bound, marker_color.at(0), + marker_color.at(1), marker_color.at(2), 0.999, now(), i); + marker_array.markers.push_back(text_marker); + } + + const double curvature = curvature_vec.at(i); + const auto text_marker = + utils::create_text_marker("curvature", text_pose, curvature, 0.05, 0.05, 0.0, 0.9, now(), i); + marker_array.markers.push_back(text_marker); + + if (max_curvature < std::abs(curvature)) { + max_curvature = std::abs(curvature); + } + } + const double max_steer_angle = vehicle_info_.calcSteerAngleFromCurvature(max_curvature); + + // publish road boundaries + const auto left_bound = convertToGeometryPoints(lanelet_left_bound); + const auto right_bound = convertToGeometryPoints(lanelet_right_bound); + + marker_array.markers.push_back( + utils::create_points_marker("left_bound", left_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); + marker_array.markers.push_back( + utils::create_points_marker("right_bound", right_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); + pub_debug_markers_->publish(marker_array); + + // show the validation results + // 1. distance from footprints to road boundaries + const bool are_footprints_inside_lanelets = [&]() { + std::cerr << "1. Footprints inside Lanelets:" << std::endl; + if (dist_thresh_to_road_border < min_dist) { + std::cerr << " The generated centerline is inside the lanelet. (threshold:" + << dist_thresh_to_road_border << "[m] < actual:" << min_dist << "[m])" << std::endl + << " Passed." << std::endl; + return true; + } + std::cerr << RED_TEXT + << " The generated centerline is outside the lanelet. (actual:" << min_dist + << "[m] <= threshold:" << dist_thresh_to_road_border << "[m])" << std::endl + << " Failed." << RESET_TEXT << std::endl; + return false; + }(); + // 2. centerline's curvature + std::cerr << "2. Curvature:" << std::endl; + const bool is_curvature_low = + true; // always tre for now since the curvature is just estimated and not enough precise. + if (max_steer_angle < steer_angle_threshold) { + std::cerr << " The generated centerline has no high steer angle. (estimated:" + << autoware::universe_utils::rad2deg(max_steer_angle) + << "[deg] < threshold:" << autoware::universe_utils::rad2deg(steer_angle_threshold) + << "[deg])" << std::endl + << " Passed." << std::endl; + } else { + std::cerr << YELLOW_TEXT << " The generated centerline has a too high steer angle. (threshold:" + << autoware::universe_utils::rad2deg(steer_angle_threshold) + << "[deg] <= estimated:" << autoware::universe_utils::rad2deg(max_steer_angle) + << "[deg])" << std::endl + << " However, the estimated steer angle is not enough precise, so the result is " + "conditional pass." + << std::endl + << " Conditionally Passed." << RESET_TEXT << std::endl; + } + // 3. result + std::cerr << std::endl << BOLD_TEXT << "Result:" << RESET_TEXT << std::endl; + if (are_footprints_inside_lanelets && is_curvature_low) { + std::cerr << BOLD_TEXT << " Passed!" << RESET_TEXT << std::endl; + } else { + std::cerr << BOLD_TEXT << RED_TEXT << " Failed!" << RESET_TEXT << std::endl; + } + + std::cerr << "###################################################################################" + "#############################" + << std::endl + << std::endl; + RCLCPP_INFO(get_logger(), "Validated the generated centerline."); +} + +void StaticCenterlineGeneratorNode::save_map() +{ + if (!route_handler_ptr_) { + return; + } + + const auto & centerline = centerline_handler_.get_selected_centerline(); + const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); + + const auto lanelet2_output_file_path = getRosParameter("lanelet2_output_file_path"); + + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + + // update centerline in map + utils::update_centerline(original_map_ptr_, route_lanelets, centerline); + RCLCPP_INFO(get_logger(), "Updated centerline in map."); + + // save map with modified center line + std::filesystem::create_directory("/tmp/autoware_static_centerline_generator"); + const auto map_projector = + autoware::geography_utils::get_lanelet2_projector(*map_projector_info_); + lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector); + RCLCPP_INFO( + get_logger(), "Saved map in %s", "/tmp/autoware_static_centerline_generator/lanelet2_map.osm"); + + // copy the output LL2 map to the temporary file for debugging + const std::string debug_output_file_dir{"/tmp/autoware_static_centerline_generator/output/"}; + std::filesystem::create_directories(debug_output_file_dir); + std::filesystem::copy( + lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm", + std::filesystem::copy_options::overwrite_existing); +} +} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp new file mode 100644 index 00000000..c591dcfc --- /dev/null +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp @@ -0,0 +1,187 @@ +// Copyright 2022 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 STATIC_CENTERLINE_GENERATOR_NODE_HPP_ +#define STATIC_CENTERLINE_GENERATOR_NODE_HPP_ + +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware_static_centerline_generator/srv/load_map.hpp" +#include "autoware_static_centerline_generator/srv/plan_path.hpp" +#include "autoware_static_centerline_generator/srv/plan_route.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include "centerline_source/optimization_trajectory_based_centerline.hpp" +#include "rclcpp/rclcpp.hpp" +#include "type_alias.hpp" + +#include "autoware_map_msgs/msg/map_projector_info.hpp" +#include "std_msgs/msg/empty.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/int32.hpp" + +#include +#include +#include +#include + +namespace autoware::static_centerline_generator +{ +using autoware_map_msgs::msg::MapProjectorInfo; +using autoware_static_centerline_generator::srv::LoadMap; +using autoware_static_centerline_generator::srv::PlanPath; +using autoware_static_centerline_generator::srv::PlanRoute; + +struct CenterlineWithRoute +{ + std::vector centerline{}; + std::vector route_lane_ids{}; +}; +struct CenterlineHandler +{ + CenterlineHandler() = default; + explicit CenterlineHandler(const CenterlineWithRoute & centerline_with_route) + : whole_centerline_with_route(centerline_with_route), + start_index(0), + end_index(centerline_with_route.centerline.size() - 1) + { + } + std::vector get_selected_centerline() const + { + if (!whole_centerline_with_route) { + return std::vector{}; + } + const auto & centerline_begin = whole_centerline_with_route->centerline.begin(); + return std::vector( + centerline_begin + start_index, centerline_begin + end_index + 1); + } + std::vector get_route_lane_ids() const + { + if (!whole_centerline_with_route) { + return std::vector{}; + } + return whole_centerline_with_route->route_lane_ids; + } + bool is_valid() const { return whole_centerline_with_route && start_index < end_index; } + bool update_start_index(const int arg_start_index) + { + if (whole_centerline_with_route && arg_start_index < end_index) { + start_index = arg_start_index; + return true; + } + return false; + } + bool update_end_index(const int arg_end_index) + { + if (whole_centerline_with_route && start_index < arg_end_index) { + end_index = arg_end_index; + return true; + } + return false; + } + + std::optional whole_centerline_with_route{std::nullopt}; + int start_index{}; + int end_index{}; +}; + +struct RoadBounds +{ + std::vector left_bound; + std::vector right_bound; +}; + +class StaticCenterlineGeneratorNode : public rclcpp::Node +{ +public: + explicit StaticCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options); + void generate_centerline(); + void validate(); + void save_map(); + +private: + // load map + void load_map(const std::string & lanelet2_input_file_path); + void on_load_map( + const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response); + + // plan route + std::vector plan_route_by_lane_ids( + const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id); + std::vector plan_route( + const geometry_msgs::msg::Pose & start_center_pose, + const geometry_msgs::msg::Pose & end_center_pose); + + void on_plan_route( + const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); + + // plan centerline + CenterlineWithRoute generate_whole_centerline_with_route(); + std::vector get_route_lane_ids_from_points( + const std::vector & points); + void on_plan_path( + const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); + + void visualize_selected_centerline(); + + // parameter + template + T getRosParameter(const std::string & param_name) + { + return autoware::universe_utils::getOrDeclareParameter(*this, param_name); + } + + lanelet::LaneletMapPtr original_map_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; + std::shared_ptr route_handler_ptr_{nullptr}; + std::unique_ptr map_projector_info_{nullptr}; + + CenterlineHandler centerline_handler_; + + float footprint_margin_for_road_bound_{0.0}; + + enum class CenterlineSource { + OptimizationTrajectoryBase = 0, + BagEgoTrajectoryBase, + }; + CenterlineSource centerline_source_; + OptimizationTrajectoryBasedCenterline optimization_trajectory_based_centerline_; + + // publisher + rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; + rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr}; + rclcpp::Publisher::SharedPtr pub_centerline_{nullptr}; + rclcpp::Publisher::SharedPtr pub_validation_results_{nullptr}; + rclcpp::Publisher::SharedPtr pub_debug_ego_footprint_bounds_{nullptr}; + rclcpp::Publisher::SharedPtr pub_debug_markers_{nullptr}; + + // subscriber + rclcpp::Subscription::SharedPtr sub_traj_start_index_; + rclcpp::Subscription::SharedPtr sub_traj_end_index_; + rclcpp::Subscription::SharedPtr sub_save_map_; + rclcpp::Subscription::SharedPtr sub_validate_; + rclcpp::Subscription::SharedPtr sub_traj_resample_interval_; + rclcpp::Subscription::SharedPtr sub_footprint_margin_for_road_bound_; + + // service + rclcpp::Service::SharedPtr srv_load_map_; + rclcpp::Service::SharedPtr srv_plan_route_; + rclcpp::Service::SharedPtr srv_plan_path_; + + // callback group for service + rclcpp::CallbackGroup::SharedPtr callback_group_; + + // vehicle info + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; +}; +} // namespace autoware::static_centerline_generator +#endif // STATIC_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp new file mode 100644 index 00000000..2b7b99bf --- /dev/null +++ b/planning/autoware_static_centerline_generator/src/type_alias.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 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 TYPE_ALIAS_HPP_ +#define TYPE_ALIAS_HPP_ + +#include "autoware/route_handler/route_handler.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/lanelet_route.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace autoware::static_centerline_generator +{ +using autoware::route_handler::RouteHandler; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathWithLaneId; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +} // namespace autoware::static_centerline_generator + +#endif // TYPE_ALIAS_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp new file mode 100644 index 00000000..f84fe79c --- /dev/null +++ b/planning/autoware_static_centerline_generator/src/utils.cpp @@ -0,0 +1,263 @@ +// Copyright 2022 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 "utils.hpp" + +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::static_centerline_generator +{ +namespace +{ +nav_msgs::msg::Odometry::ConstSharedPtr convert_to_odometry(const geometry_msgs::msg::Pose & pose) +{ + auto odometry_ptr = std::make_shared(); + odometry_ptr->pose.pose = pose; + return odometry_ptr; +} + +lanelet::Point3d createPoint3d(const double x, const double y, const double z) +{ + lanelet::Point3d point(lanelet::utils::getId()); + + // position + point.x() = x; + point.y() = y; + point.z() = z; + + // attributes + point.setAttribute("local_x", x); + point.setAttribute("local_y", y); + // NOTE: It seems that the attribute "ele" is assigned automatically. + + return point; +} +} // namespace + +namespace utils +{ +rclcpp::QoS create_transient_local_qos() +{ + return rclcpp::QoS{1}.transient_local(); +} + +lanelet::ConstLanelets get_lanelets_from_ids( + const RouteHandler & route_handler, const std::vector & lane_ids) +{ + lanelet::ConstLanelets lanelets; + for (const lanelet::Id lane_id : lane_ids) { + const auto lanelet = route_handler.getLaneletsFromId(lane_id); + lanelets.push_back(lanelet); + } + return lanelets; +} + +geometry_msgs::msg::Pose get_center_pose( + const RouteHandler & route_handler, const size_t lanelet_id) +{ + // get middle idx of the lanelet + const auto lanelet = route_handler.getLaneletsFromId(lanelet_id); + const auto center_line = lanelet.centerline(); + const size_t middle_point_idx = std::floor(center_line.size() / 2.0); + + // get middle position of the lanelet + geometry_msgs::msg::Point middle_pos; + middle_pos.x = center_line[middle_point_idx].x(); + middle_pos.y = center_line[middle_point_idx].y(); + middle_pos.z = center_line[middle_point_idx].z(); + + // get next middle position of the lanelet + geometry_msgs::msg::Point next_middle_pos; + next_middle_pos.x = center_line[middle_point_idx + 1].x(); + next_middle_pos.y = center_line[middle_point_idx + 1].y(); + next_middle_pos.z = center_line[middle_point_idx + 1].z(); + + // calculate middle pose + geometry_msgs::msg::Pose middle_pose; + middle_pose.position = middle_pos; + const double yaw = autoware::universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); + middle_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + + return middle_pose; +} + +PathWithLaneId get_path_with_lane_id( + const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, + const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold) +{ + // get center line + constexpr double s_start = 0.0; + constexpr double s_end = std::numeric_limits::max(); + auto path_with_lane_id = route_handler.getCenterLinePath(lanelets, s_start, s_end); + path_with_lane_id.header.frame_id = "map"; + + // create planner data + auto planner_data = std::make_shared(); + planner_data->route_handler = std::make_shared(route_handler); + planner_data->self_odometry = convert_to_odometry(start_pose); + planner_data->parameters.ego_nearest_dist_threshold = ego_nearest_dist_threshold; + planner_data->parameters.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold; + + // generate drivable area and store it in path with lane id + constexpr double vehicle_length = 0.0; + const auto drivable_lanes = behavior_path_planner::utils::generateDrivableLanes(lanelets); + behavior_path_planner::utils::generateDrivableArea( + path_with_lane_id, drivable_lanes, false, false, vehicle_length, planner_data); + + return path_with_lane_id; +} + +void update_centerline( + lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, + const std::vector & new_centerline) +{ + // get lanelet as reference to update centerline + lanelet::Lanelets lanelets_ref; + for (const auto & lanelet : lanelets) { + for (auto & lanelet_ref : lanelet_map_ptr->laneletLayer) { + if (lanelet_ref.id() == lanelet.id()) { + lanelets_ref.push_back(lanelet_ref); + } + } + } + + // store new centerline in lanelets + size_t lanelet_idx = 0; + lanelet::LineString3d centerline(lanelet::utils::getId()); + for (size_t traj_idx = 0; traj_idx < new_centerline.size(); ++traj_idx) { + const auto & traj_pos = new_centerline.at(traj_idx).pose.position; + + for (; lanelet_idx < lanelets_ref.size(); ++lanelet_idx) { + auto & lanelet_ref = lanelets_ref.at(lanelet_idx); + + const lanelet::BasicPoint2d point(traj_pos.x, traj_pos.y); + // TODO(murooka) This does not work with L-crank map. + const bool is_inside = lanelet::geometry::inside(lanelet_ref, point); + if (is_inside) { + const auto center_point = createPoint3d(traj_pos.x, traj_pos.y, traj_pos.z); + + // set center point + centerline.push_back(center_point); + lanelet_map_ptr->add(center_point); + break; + } + + if (!centerline.empty()) { + // set centerline + lanelet_map_ptr->add(centerline); + lanelet_ref.setCenterline(centerline); + + // prepare new centerline + centerline = lanelet::LineString3d(lanelet::utils::getId()); + } + } + + if (traj_idx == new_centerline.size() - 1 && !centerline.empty()) { + auto & lanelet_ref = lanelets_ref.at(lanelet_idx); + + // set centerline + lanelet_map_ptr->add(centerline); + lanelet_ref.setCenterline(centerline); + } + } +} + +Marker create_footprint_marker( + const LinearRing2d & footprint_poly, const double width, const double r, const double g, + const double b, const double a, const rclcpp::Time & now, const size_t idx) +{ + auto marker = autoware::universe_utils::createDefaultMarker( + "map", rclcpp::Clock().now(), "unsafe_footprints", idx, + visualization_msgs::msg::Marker::LINE_STRIP, + autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(r, g, b, a)); + marker.header.stamp = now; + // TODO(murooka) Ideally, the following is unnecessary for the topic of transient local. + marker.lifetime = rclcpp::Duration(0, 0); + + for (const auto & point : footprint_poly) { + geometry_msgs::msg::Point geom_point; + geom_point.x = point.x(); + geom_point.y = point.y(); + // geom_point.z = point.z(); + + marker.points.push_back(geom_point); + } + marker.points.push_back(marker.points.front()); + return marker; +} + +Marker create_text_marker( + const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, + const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx) +{ + auto marker = autoware::universe_utils::createDefaultMarker( + "map", rclcpp::Clock().now(), ns, idx, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5), + autoware::universe_utils::createMarkerColor(r, g, b, a)); + marker.pose = pose; + marker.header.stamp = now; + marker.lifetime = rclcpp::Duration(0, 0); + + std::stringstream ss; + ss << std::setprecision(2) << value; + marker.text = ss.str(); + + return marker; +} + +Marker create_points_marker( + const std::string & ns, const std::vector & points, const double width, + const double r, const double g, const double b, const double a, const rclcpp::Time & now) +{ + auto marker = autoware::universe_utils::createDefaultMarker( + "map", now, ns, 1, Marker::LINE_STRIP, + autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(r, g, b, a)); + marker.lifetime = rclcpp::Duration(0, 0); + marker.points = points; + return marker; +} + +MarkerArray create_delete_all_marker_array( + const std::vector & ns_vec, const rclcpp::Time & now) +{ + Marker marker; + marker.header.stamp = now; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + + MarkerArray marker_array; + for (const auto & ns : ns_vec) { + marker.ns = ns; + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +} // namespace utils +} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp new file mode 100644 index 00000000..f4d15d3d --- /dev/null +++ b/planning/autoware_static_centerline_generator/src/utils.hpp @@ -0,0 +1,67 @@ +// Copyright 2022 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include "autoware/route_handler/route_handler.hpp" +#include "type_alias.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::static_centerline_generator +{ +namespace utils +{ +rclcpp::QoS create_transient_local_qos(); + +lanelet::ConstLanelets get_lanelets_from_ids( + const RouteHandler & route_handler, const std::vector & lane_ids); + +geometry_msgs::msg::Pose get_center_pose( + const RouteHandler & route_handler, const size_t lanelet_id); + +PathWithLaneId get_path_with_lane_id( + const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, + const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold); + +void update_centerline( + lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, + const std::vector & new_centerline); + +Marker create_footprint_marker( + const LinearRing2d & footprint_poly, const double width, const double r, const double g, + const double b, const double a, const rclcpp::Time & now, const size_t idx); + +Marker create_text_marker( + const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, + const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx); + +Marker create_points_marker( + const std::string & ns, const std::vector & points, const double width, + const double r, const double g, const double b, const double a, const rclcpp::Time & now); + +MarkerArray create_delete_all_marker_array( + const std::vector & ns_vec, const rclcpp::Time & now); + +} // namespace utils +} // namespace autoware::static_centerline_generator + +#endif // UTILS_HPP_ diff --git a/planning/autoware_static_centerline_generator/srv/LoadMap.srv b/planning/autoware_static_centerline_generator/srv/LoadMap.srv new file mode 100644 index 00000000..02142e60 --- /dev/null +++ b/planning/autoware_static_centerline_generator/srv/LoadMap.srv @@ -0,0 +1,3 @@ +string map +--- +string message diff --git a/planning/autoware_static_centerline_generator/srv/PlanPath.srv b/planning/autoware_static_centerline_generator/srv/PlanPath.srv new file mode 100644 index 00000000..3a8d0321 --- /dev/null +++ b/planning/autoware_static_centerline_generator/srv/PlanPath.srv @@ -0,0 +1,6 @@ +uint32 map_id +int64[] route +--- +autoware_static_centerline_generator/PointsWithLaneId[] points_with_lane_ids +string message +int64[] unconnected_lane_ids diff --git a/planning/autoware_static_centerline_generator/srv/PlanRoute.srv b/planning/autoware_static_centerline_generator/srv/PlanRoute.srv new file mode 100644 index 00000000..fb50c04b --- /dev/null +++ b/planning/autoware_static_centerline_generator/srv/PlanRoute.srv @@ -0,0 +1,5 @@ +int64 start_lane_id +int64 end_lane_id +--- +int64[] lane_ids +string message diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 new file mode 100644 index 00000000..ed344877 Binary files /dev/null and b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 differ diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 new file mode 100644 index 00000000..0883307a Binary files /dev/null and b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 differ diff --git a/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm b/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm new file mode 100644 index 00000000..406cd85c --- /dev/null +++ b/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm @@ -0,0 +1,146 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py new file mode 100644 index 00000000..ca262121 --- /dev/null +++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +import launch_testing +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + lanelet2_map_path = os.path.join( + get_package_share_directory("autoware_static_centerline_generator"), + "test/data/lanelet2_map.osm", + ) + + static_centerline_generator_node = Node( + package="autoware_static_centerline_generator", + executable="main", + output="screen", + parameters=[ + {"lanelet2_map_path": lanelet2_map_path}, + {"mode": "AUTO"}, + {"rviz": False}, + {"centerline_source": "optimization_trajectory_base"}, + {"lanelet2_input_file_path": lanelet2_map_path}, + {"lanelet2_output_file_path": "/tmp/lanelet2_map.osm"}, + {"start_lanelet_id": 215}, + {"end_lanelet_id": 216}, + os.path.join( + get_package_share_directory("autoware_mission_planner"), + "config", + "mission_planner.param.yaml", + ), + os.path.join( + get_package_share_directory("autoware_static_centerline_generator"), + "config/static_centerline_generator.param.yaml", + ), + os.path.join( + get_package_share_directory("autoware_behavior_path_planner"), + "config/behavior_path_planner.param.yaml", + ), + os.path.join( + get_package_share_directory("autoware_behavior_velocity_planner"), + "config/behavior_velocity_planner.param.yaml", + ), + os.path.join( + get_package_share_directory("autoware_path_smoother"), + "config/elastic_band_smoother.param.yaml", + ), + os.path.join( + get_package_share_directory("autoware_path_optimizer"), + "config/path_optimizer.param.yaml", + ), + os.path.join( + get_package_share_directory("autoware_map_loader"), + "config/lanelet2_map_loader.param.yaml", + ), + os.path.join( + get_package_share_directory("autoware_static_centerline_generator"), + "config/common.param.yaml", + ), + os.path.join( + get_package_share_directory("autoware_static_centerline_generator"), + "config/nearest_search.param.yaml", + ), + os.path.join( + get_package_share_directory("autoware_static_centerline_generator"), + "config/vehicle_info.param.yaml", + ), + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + static_centerline_generator_node, + # Start test after 1s - gives time for the autoware_static_centerline_generator to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info)