From f5b5ade7ccdef2bf10c36f984e9015fddc9b5690 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 16 Dec 2022 17:19:47 +0900 Subject: [PATCH] Squashed commit of the following: commit c7d3b7d2132323af3437af01e9d774b13005bace Author: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com> Date: Fri Dec 16 13:51:35 2022 +0900 test(freespace_planning_algorithms): done't dump rosbag by default (#2504) Signed-off-by: Hirokazu Ishida Signed-off-by: Hirokazu Ishida commit 6731e0ced39e3187c2afffe839eaa697a19e5e84 Author: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri Dec 16 09:29:35 2022 +0900 feat(pose_initializer): partial map loading (#2500) * first commit Signed-off-by: kminoda * move function Signed-off-by: kminoda * now works Signed-off-by: kminoda * ci(pre-commit): autofix * update readme Signed-off-by: kminoda * ci(pre-commit): autofix * clarify how to enable partial mao loading interface Signed-off-by: kminoda * ci(pre-commit): autofix * update readme Signed-off-by: kminoda * ci(pre-commit): autofix * Update localization/pose_initializer/config/pose_initializer.param.yaml Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> * fix pre-commit Signed-off-by: kminoda Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> commit efb4ff1cea6e07aa9e894a6042e8685e30b420ba Author: Kosuke Takeuchi Date: Thu Dec 15 17:29:44 2022 +0900 feat(trajectory_follower): extend mpc trajectory for terminal yaw (#2447) * feat(trajectory_follower): extend mpc trajectory for terminal yaw Signed-off-by: kosuke55 * make mpc min vel param Signed-off-by: kosuke55 * add mpc extended point after smoothing Signed-off-by: kosuke55 * Revert "make mpc min vel param" This reverts commit 02157b6ae0c2ff1564840f6d15e3c55025327baf. Signed-off-by: kosuke55 * add comment and hypot Signed-off-by: kosuke55 * remove min vel Signed-off-by: kosuke55 * add flag for extending traj Signed-off-by: kosuke55 * add extend param to default param Signed-off-by: kosuke55 * fix typo Signed-off-by: kosuke55 * fix from TakaHoribe review Signed-off-by: kosuke55 * fix typo Signed-off-by: kosuke55 * refactor Signed-off-by: kosuke55 Signed-off-by: kosuke55 commit ad2ae7827bdc3af7da8607fdd53ea74940426421 Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Thu Dec 15 15:52:34 2022 +0900 feat(component_interface_tools): add service log checker (#2503) * feat(component_interface_utils): add service log checker Signed-off-by: Takagi, Isamu * feat(component_interface_tools): add service log checker Signed-off-by: Takagi, Isamu * feat(component_interface_tools): add diagnostics Signed-off-by: Takagi, Isamu * feat: update system error monitor config Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit 4a13cc5a32898f5b17791d9381744bf71ff8ed20 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Thu Dec 15 12:54:11 2022 +0900 fix(behavior_path_planner): fix goal lanelet extension (#2508) Signed-off-by: yutaka Signed-off-by: yutaka commit 77b1c36b5ca89b25250dcbb117c9f03a9c36c1c4 Author: Kyoichi Sugahara <81.s.kyo.19@gmail.com> Date: Thu Dec 15 10:45:45 2022 +0900 feat(behavior_path_planner): change side shift module logic (#2195) * change side shift module design Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * cherry picked side shift controller Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * add debug marker to side shift Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * fix pointer error due to direct assignment added make_shared Signed-off-by: Muhammad Zulfaqar Azmi * add flow chart Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * add status of AFTER_SHIFT Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * remove function for debug Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * ci(pre-commit): autofix * fix flow chart Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * ci(pre-commit): autofix Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: tanaka3 Co-authored-by: Muhammad Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit 9183c4f20eb4592ed0b48c2eac67add070711677 Author: Takamasa Horibe Date: Wed Dec 14 19:59:00 2022 +0900 refactor(simple_planning_simulator): make function for duplicated code (#2502) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe commit ed992b10ed326f03354dce3b563b8622f9ae9a6c Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 14 17:48:24 2022 +0900 fix(behavior_path_planner): fix planner data copy (#2501) Signed-off-by: yutaka Signed-off-by: yutaka commit 0c6c46b33b3c828cb95eaa31fcbf85655fc6a55f Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 14 14:42:16 2022 +0900 fix(behavior_path_planner): fix find nearest function from lateral distance (#2499) * feat(behavior_path_planner): fix find nearest function from lateral distance * empty commit commit a26b69d1df55e9369ea3adcdd011ae2d7c86dfb7 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 14 11:28:07 2022 +0900 feat(behavior_path_planner): fix overlap checker (#2498) * feat(behavior_path_planner): fix overlap checker Signed-off-by: yutaka * remove reserve Signed-off-by: yutaka Signed-off-by: yutaka commit 3a24859ca6851caaeb25fc4fac2334fcbdb887d1 Author: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com> Date: Tue Dec 13 16:51:59 2022 +0300 feat(mission_planner): check goal footprint (#2088) Signed-off-by: ismet atabay commit b6a18855431b5f3a67fcbf383fac8df2b45d462e Author: Takamasa Horibe Date: Tue Dec 13 22:46:24 2022 +0900 feat(trajectory_visualizer): update for steer limit, remove tf for pose source (#2267) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe commit f1a9a9608559a5b89f631df3dc2fadd037e36ab4 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue Dec 13 19:47:16 2022 +0900 feat(behavior_path_planner): remove unnecessary code and clean turn signal decider (#2494) * feat(behavior_path_planner): clean drivable area code Signed-off-by: yutaka * make a function for turn signal decider Signed-off-by: yutaka Signed-off-by: yutaka commit fafe1d8235b99302bc9ba8f3770ae34878f1e7e7 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue Dec 13 18:19:41 2022 +0900 feat(behavior_path_planner): change turn signal output timing (#2493) Signed-off-by: yutaka Signed-off-by: yutaka commit c48b9cfa7074ecd46d96f6dc43679e17bde3a63d Author: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue Dec 13 09:16:14 2022 +0900 feat(map_loader): add differential map loading interface (#2417) * first commit Signed-off-by: kminoda * ci(pre-commit): autofix * added module load in _node.cpp Signed-off-by: kminoda * ci(pre-commit): autofix * create pcd metadata dict when either of the flag is true Signed-off-by: kminoda * ci(pre-commit): autofix * fix readme * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit 9a3613bfcd3e36e522d0ea9130f6200ca7689e2b Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Tue Dec 13 08:49:23 2022 +0900 docs(default_ad_api): add readme (#2491) * docs(default_ad_api): add readme Signed-off-by: Takagi, Isamu * feat: update table Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit 49aa10b04de61c36706f6151d11bf17257ca54d1 Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Tue Dec 13 06:46:20 2022 +0900 feat(default_ad_api): split parameters into file (#2488) * feat(default_ad_api): split parameters into file Signed-off-by: Takagi, Isamu * feat: remove old parameter Signed-off-by: Takagi, Isamu * fix: test Signed-off-by: Takagi, Isamu * feat: add default config Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit 7f0138c356c742b6e15e571e7a4683caa55969ac Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon Dec 12 22:16:54 2022 +0900 feat(behavior_path_planner, obstacle_avoidance_planner): add new drivable area (#2472) * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update obstacle avoidance planner Signed-off-by: yutaka * update Signed-off-by: yutaka * clean code Signed-off-by: yutaka * uddate Signed-off-by: yutaka * clean code Signed-off-by: yutaka * remove resample Signed-off-by: yutaka * update Signed-off-by: yutaka * add orientation Signed-off-by: yutaka * change color Signed-off-by: yutaka * update Signed-off-by: yutaka * remove drivable area Signed-off-by: yutaka * add flag Signed-off-by: yutaka * update Signed-off-by: yutaka * update color Signed-off-by: yutaka * fix some codes Signed-off-by: yutaka * change to makerker array Signed-off-by: yutaka * change avoidance utils Signed-off-by: yutaka Signed-off-by: yutaka commit c855e23cc17d1518ebce5dd15629d03acfe17da3 Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Mon Dec 12 17:15:10 2022 +0900 refactor(vehicle_cmd_gate): remove old emergency topics (#2403) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit fa04d540c9afdded016730c9978920a194d2d2b4 Author: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Mon Dec 12 16:04:00 2022 +0900 feat: replace python launch with xml launch for system monitor (#2430) * feat: replace python launch with xml launch for system monitor Signed-off-by: Daisuke Nishimatsu * ci(pre-commit): autofix * update figure Signed-off-by: Daisuke Nishimatsu Signed-off-by: Daisuke Nishimatsu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit 4a6990c49d1f8c3bedfb345e7c94c3c6893b4099 Author: Kosuke Takeuchi Date: Mon Dec 12 15:01:39 2022 +0900 feat(trajectory_follower): pub steer converged marker (#2441) * feat(trajectory_follower): pub steer converged marker Signed-off-by: kosuke55 * Revert "feat(trajectory_follower): pub steer converged marker" This reverts commit a6f6917bc542d5b533150f6abba086121e800974. Signed-off-by: kosuke55 * add steer converged debug marker in contoller_node Signed-off-by: kosuke55 Signed-off-by: kosuke55 commit 3c01f15125dfbc45e1050ee96ccc42618d6ee6fd Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Mon Dec 12 12:48:41 2022 +0900 docs(tier4_state_rviz_plugin): update readme (#2475) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit d8ece0040354be5381a27403bcc757354735a77b Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Mon Dec 12 11:57:03 2022 +0900 chore(simulator_compatibility_test): suppress setuptools warnings (#2483) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu commit 727586bfe86dc9cb21ce34d9cbe19c241e162b04 Author: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon Dec 12 10:00:35 2022 +0900 fix(behavior_path_planner): lane change candidate resolution (#2426) * fix(behavior_path_planner): lane change candidate resolution Signed-off-by: Muhammad Zulfaqar Azmi * rework sampling based on current speed Signed-off-by: Muhammad Zulfaqar Azmi * refactor code Signed-off-by: Muhammad Zulfaqar Azmi * use util's resampler Signed-off-by: Muhammad Zulfaqar Azmi * consider min_resampling_points and resampling dt Signed-off-by: Muhammad Zulfaqar * simplify code Signed-off-by: Muhammad Zulfaqar Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar commit 284548ca7f38b1d83af11f2b9caaac116eb9b09c Author: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon Dec 12 09:57:19 2022 +0900 fix(behavior_path_planner): minimum distance for lane change (#2413) Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi commit 469d8927bd7a0c98b9d491d347e111065973e13f Author: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Fri Dec 9 21:27:18 2022 +0900 revert(behavior_path): revert removal of refineGoalFunction (#2340)" (#2485) This reverts commit 8e13ced6dfb6edfea77a589ef4cb93d82683bf51. Signed-off-by: mitsudome-r Signed-off-by: mitsudome-r commit d924f85b079dfe64feab017166685be40e977e62 Author: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Fri Dec 9 19:53:51 2022 +0800 fix(freespace_planning_algorithms): fix rrtstar can't arrive goal error (#2350) Signed-off-by: NorahXiong Signed-off-by: NorahXiong Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> commit b2ded82324bce78d9db3ff01b0227b00709b1efe Author: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri Dec 9 17:12:13 2022 +0900 fix(ground-segmentation): recheck gnd cluster pointcloud (#2448) * fix: reclassify ground cluster pcl Signed-off-by: badai-nguyen * fix: add lowest-based recheck Signed-off-by: badai-nguyen * chore: refactoring Signed-off-by: badai-nguyen * chore: refactoring Signed-off-by: badai-nguyen Signed-off-by: badai-nguyen Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> commit 8906a1e78bc5b7d6417683ecedc1efe3f48be31e Author: Takamasa Horibe Date: Fri Dec 9 16:29:45 2022 +0900 fix(trajectory_follower): fix mpc trajectory z pos (#2482) Signed-off-by: takahoribe Signed-off-by: takahoribe commit d4939058f05f9a1609f0ed22afbd0d4febfb212d Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri Dec 9 12:40:30 2022 +0900 feat(behavior_velocity_planner): clean walkway module (#2480) Signed-off-by: yutaka Signed-off-by: yutaka commit d3b86a37ae7c3a0d59832caf56afa13b148d562c Author: Makoto Kurihara Date: Thu Dec 8 22:59:32 2022 +0900 fix(emergency_handler): fix mrm handling when mrm behavior is none (#2476) Signed-off-by: Makoto Kurihara Signed-off-by: Makoto Kurihara commit 2dde073a101e96757ef0cd189bb9ff06836934e9 Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Thu Dec 8 17:16:13 2022 +0900 feat(behavior_velocity_planner): add velocity factors (#1985) * (editting) add intersection_coordination to stop reason Signed-off-by: TakumiKozaka-T4 * (editting) add intersection coordination to stop reasons Signed-off-by: TakumiKozaka-T4 * (Editting) add v2x to stop reason Signed-off-by: TakumiKozaka-T4 * (editting) add stop reason2 publisher Signed-off-by: TakumiKozaka-T4 * (editting) add stop reason2 to scene modules Signed-off-by: TakumiKozaka-T4 * add stop reason2 to obstacle stop planner and surround obstacle checker Signed-off-by: TakumiKozaka-T4 * Modify files including unintended change by rebase Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * Modification 1: not to publsh vacant stop reason, 2: change default status in obstacle stop and surround obstacle checker Signed-off-by: TakumiKozaka-T4 * fix error Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * modification for renaming stop_reason2 to motion_factor Signed-off-by: TakumiKozaka-T4 * (Editting) rename variables Signed-off-by: TakumiKozaka-T4 * bug fix Signed-off-by: TakumiKozaka-T4 * (WIP) Add motion factor message. Modify scene modules due to new motion factor. Moving motion factor aggregator. Signed-off-by: TakumiKozaka-T4 * (WIP) Save current work. Modify aggregator, CMakeList. Add launcher Signed-off-by: TakumiKozaka-T4 * (WIP) Solved build error, but not launched Signed-off-by: TakumiKozaka-T4 * (WIP) fixing error in launch Signed-off-by: TakumiKozaka-T4 * (WIP) fixing error in launch Signed-off-by: TakumiKozaka-T4 * (WIP) fixing launch error Signed-off-by: TakumiKozaka-T4 * Fix error in launching motion factor aggregator Signed-off-by: TakumiKozaka-T4 * Delete unnecessary comment-out in CMakelists. Change remapping in launcher. Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * pull the latest foundation/main Signed-off-by: TakumiKozaka-T4 * (fix for pre-commit.ci) Add to motion_factor_aggregator.hpp Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * feat: add velocity factor interface Signed-off-by: Takagi, Isamu * fix: fix build error Signed-off-by: Takagi, Isamu * feat: stop sign Signed-off-by: Takagi, Isamu * WIP Signed-off-by: Takagi, Isamu * feat: update visualizer Signed-off-by: Takagi, Isamu * feat: modify traffic light manager Signed-off-by: Takagi, Isamu * feat: update velocity factors Signed-off-by: Takagi, Isamu * feat: update api Signed-off-by: Takagi, Isamu * feat: move adapi msgs Signed-off-by: Takagi, Isamu * feat: remove old aggregator Signed-off-by: Takagi, Isamu * feat: move api Signed-off-by: Takagi, Isamu * feat: rename message Signed-off-by: Takagi, Isamu * feat: add using Signed-off-by: Takagi, Isamu * feat: add distance Signed-off-by: Takagi, Isamu * feat: fix build error Signed-off-by: Takagi, Isamu * feat: use nan as default distance Signed-off-by: Takagi, Isamu * fix: set virtual traffic light detail Signed-off-by: Takagi, Isamu * fix: remove debug code Signed-off-by: Takagi, Isamu * fix: copyright Signed-off-by: Takagi, Isamu Signed-off-by: TakumiKozaka-T4 Signed-off-by: Takagi, Isamu Co-authored-by: TakumiKozaka-T4 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit 9a5057e4948ff5ac9165c14eb7112d79f2de76d5 Author: Kosuke Takeuchi Date: Thu Dec 8 13:42:50 2022 +0900 fix(freespace_planning_algorithms): comment out failing tests (#2440) Signed-off-by: kosuke55 Signed-off-by: kosuke55 commit cddb8c74d0fbf49390b4d462c20c12bc257f4825 Author: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu Dec 8 11:57:04 2022 +0900 feat(gyro_odometer): publish twist when both data arrives (#2423) * feat(gyro_odometer): publish when both data arrive Signed-off-by: kminoda * remove unnecessary commentouts Signed-off-by: kminoda * ci(pre-commit): autofix * use latest timestamp Signed-off-by: kminoda * small fix Signed-off-by: kminoda * debugged Signed-off-by: kminoda * update gyro_odometer Signed-off-by: kminoda * ci(pre-commit): autofix * add comments Signed-off-by: kminoda * add comments Signed-off-by: kminoda * ci(pre-commit): autofix * fix timestamp validation flow Signed-off-by: kminoda * ci(pre-commit): autofix * remove unnecessary commentouts Signed-off-by: kminoda * pre-commit Signed-off-by: kminoda * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit f0f513cf44532dfe8d51d27c4caef23fb694af16 Author: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu Dec 8 11:08:29 2022 +0900 fix: remove unnecessary DEBUG_INFO declarations (#2457) Signed-off-by: kminoda Signed-off-by: kminoda commit 01daebf42937a05a2d83f3dee2c0778389492e50 Author: Takayuki Murooka Date: Thu Dec 8 00:28:35 2022 +0900 fix(tier4_autoware_api_launch): add rosbridge_server dependency (#2470) Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka commit 26ef8174b1c12b84070b36df2a7cd14bfa9c0363 Author: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Wed Dec 7 19:32:09 2022 +0900 fix: rename `use_external_emergency_stop` to `check_external_emergency_heartbeat` (#2455) * fix: rename use_external_emergency_stop to check_external_emergency_heartbeat * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> commit 024b993a0db8c0d28db0f05f64990bed7069cbd8 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 7 18:00:32 2022 +0900 fix(motion_utils): rename sampling function (#2469) Signed-off-by: yutaka Signed-off-by: yutaka commit c240ce2b6f4e79c435ed651b347a7d665a947862 Author: Yukihiro Saito Date: Wed Dec 7 16:33:44 2022 +0900 feat: remove web controller (#2405) Signed-off-by: Yukihiro Saito Signed-off-by: Yukihiro Saito commit 2992b1cadae7e7ac86fd249998ce3c7ddbe476c9 Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 7 15:39:28 2022 +0900 feat(motion_utils): add points resample function (#2465) Signed-off-by: yutaka Signed-off-by: yutaka commit 4a75d7c0ddbd88f54afaf2bb05eb65138a53ea60 Author: Mingyu1991 <115005477+Mingyu1991@users.noreply.github.com> Date: Wed Dec 7 14:42:33 2022 +0900 docs: update training data for traffic light (#2464) * update traffic light cnn classifier README.md * correct to upper case Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> commit a4287165be87fa7727f79c01dfb0bea6af54c333 Author: Ryuta Kambe Date: Wed Dec 7 12:21:49 2022 +0900 perf(behavior_velocity_planner): remove unnecessary debug data (#2462) Signed-off-by: veqcc commit 0a5b2857d3b2c1c9370598013b25aeaebf2d654d Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed Dec 7 12:03:46 2022 +0900 feat(behavior_path_planner): cut overlapped path (#2451) * feat(behavior_path_planner): cut overlapped path Signed-off-by: yutaka * clean code Signed-off-by: yutaka Signed-off-by: yutaka commit 65003dc99f2abe937afcc010514530fa666fbbfd Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Wed Dec 7 11:06:41 2022 +0900 revert(default_ad_api): fix autoware state to add wait time (#2407) (#2460) Revert "fix(default_ad_api): fix autoware state to add wait time (#2407)" This reverts commit c4224854a7e57a9526dde998f742741fe383471c. commit fab18677ca4de378faff84a41db5147577e7448d Author: Makoto Kurihara Date: Wed Dec 7 10:32:41 2022 +0900 fix(raw_vehicle_cmd_converter): fix column index for map validation (#2450) Signed-off-by: Makoto Kurihara Signed-off-by: Makoto Kurihara commit a1d3c80a4f5e3a388887a5afb32d9bf7961301f1 Author: Ambroise Vincent Date: Tue Dec 6 10:39:02 2022 +0100 fix(tvm_utility): copy test result to CPU (#2414) Also remove dependency to autoware_auto_common. Issue-Id: SCM-5401 Signed-off-by: Ambroise Vincent Change-Id: I83b859742df2f2ff7df1d0bd2d287bfe0aa04c3d Signed-off-by: Ambroise Vincent Co-authored-by: Xinyu Wang <93699235+angry-crab@users.noreply.github.com> commit eb9946832c7e42d5380fd71956165409d0b592c3 Author: Mamoru Sobue Date: Tue Dec 6 18:11:41 2022 +0900 chore(behaviror_velocity_planner): changed logging level for intersection (#2459) changed logging level Signed-off-by: Mamoru Sobue commit c4224854a7e57a9526dde998f742741fe383471c Author: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Date: Tue Dec 6 17:01:37 2022 +0900 fix(default_ad_api): fix autoware state to add wait time (#2407) * fix(default_ad_api): fix autoware state to add wait time Signed-off-by: Takagi, Isamu * Update system/default_ad_api/src/compatibility/autoware_state.cpp Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Signed-off-by: Takagi, Isamu Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> commit f984fbb708cb02947ec2824ce041c739c35940f7 Author: Takamasa Horibe Date: Tue Dec 6 13:55:17 2022 +0900 feat(transition_manager): add param to ignore autonomous transition condition (#2453) * feat(transition_manager): add param to ignore autonomous transition condition Signed-off-by: Takamasa Horibe * same for modeChangeCompleted Signed-off-by: Takamasa Horibe * remove debug print Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe commit d3e640df270a0942c4639e11451faf26e099bbe1 Author: Tomoya Kimura Date: Tue Dec 6 13:01:06 2022 +0900 feat(operation_mode_transition_manager): transition to auto quickly when vehicle stops (#2427) Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- .../component_interface_tools/CMakeLists.txt | 7 + .../launch/service_log_checker.launch.xml | 3 + common/component_interface_tools/package.xml | 28 + .../src/service_log_checker.cpp | 110 +++ .../src/service_log_checker.hpp | 42 + .../component_interface_utils/CMakeLists.txt | 1 - common/component_interface_utils/package.xml | 1 + .../motion_utils/resample/resample.hpp | 38 +- common/motion_utils/src/resample/resample.cpp | 90 ++- .../src/drivable_area/display.cpp | 3 +- common/tier4_state_rviz_plugin/README.md | 40 +- .../images/select_auto.png | Bin 0 -> 37151 bytes .../images/select_engage.png | Bin 46355 -> 0 bytes .../include/tvm_utility/pipeline.hpp | 9 +- common/tvm_utility/package.xml | 1 - common/tvm_utility/test/yolo_v2_tiny/main.cpp | 17 +- common/web_controller/CMakeLists.txt | 14 - common/web_controller/README.md | 42 - .../web_controller/images/web_controller.png | Bin 72325 -> 0 bytes .../launch/web_controller.launch.xml | 5 - common/web_controller/package.xml | 23 - .../css/web_controller_style.css | 62 -- .../www/web_controller/icon/autoware.ico | Bin 1150 -> 0 bytes .../www/web_controller/index.html | 105 --- .../www/web_controller/js/autoware_engage.js | 94 --- .../www/web_controller/js/autoware_state.js | 45 -- .../web_controller/js/lane_change_approval.js | 81 -- .../www/web_controller/js/menu.js | 56 -- .../web_controller/js/path_change_approval.js | 81 -- .../www/web_controller/js/vehicle_engage.js | 188 ----- .../www/web_controller/js/velocity_limit.js | 81 -- ...eration_mode_transition_manager.param.yaml | 1 + .../src/state.cpp | 19 + .../src/state.hpp | 1 + .../design/mpc_lateral_controller-design.md | 1 + .../include/trajectory_follower/mpc.hpp | 2 +- .../mpc_lateral_controller.hpp | 3 + .../include/trajectory_follower/mpc_utils.hpp | 15 + control/trajectory_follower/src/mpc.cpp | 13 +- .../src/mpc_lateral_controller.cpp | 6 +- control/trajectory_follower/src/mpc_utils.cpp | 34 +- control/trajectory_follower/test/test_mpc.cpp | 19 +- .../controller_node.hpp | 2 + .../lateral_controller_defaults.param.yaml | 3 + .../src/controller_node.cpp | 30 + control/vehicle_cmd_gate/README.md | 7 +- .../launch/vehicle_cmd_gate.launch.xml | 5 +- .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 7 +- .../vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 3 +- launch/tier4_autoware_api_launch/package.xml | 1 + ...eration_mode_transition_manager.param.yaml | 1 + .../lateral_controller.param.yaml | 3 + .../launch/control.launch.py | 7 +- .../config/pointcloud_map_loader.param.yaml | 1 + .../avoidance/avoidance.param.yaml | 3 + .../system_error_monitor.param.yaml | 2 + ...ror_monitor.planning_simulation.param.yaml | 2 + .../launch/system.launch.xml | 8 +- .../system_launch.drawio.svg | 4 +- localization/gyro_odometer/CMakeLists.txt | 2 + .../gyro_odometer/gyro_odometer_core.hpp | 29 +- .../launch/gyro_odometer.launch.xml | 2 +- localization/gyro_odometer/package.xml | 2 + .../gyro_odometer/src/gyro_odometer_core.cpp | 279 ++++--- .../config/pose_initializer.param.yaml | 4 + .../docs/map_height_fitter.md | 19 + .../launch/pose_initializer.launch.xml | 4 +- localization/pose_initializer/package.xml | 1 + .../map_height_fitter_core.cpp | 69 +- .../map_height_fitter_core.hpp | 8 +- .../map_height_fitter_node.cpp | 2 +- localization/stop_filter/src/stop_filter.cpp | 6 - localization/twist2accel/src/twist2accel.cpp | 6 - map/map_loader/CMakeLists.txt | 1 + map/map_loader/README.md | 10 + .../config/pointcloud_map_loader.param.yaml | 1 + .../differential_map_loader_module.cpp | 85 ++ .../differential_map_loader_module.hpp | 59 ++ .../pointcloud_map_loader_node.cpp | 11 +- .../pointcloud_map_loader_node.hpp | 2 + .../scan_ground_filter_nodelet.hpp | 36 +- .../src/scan_ground_filter_nodelet.cpp | 49 +- perception/traffic_light_classifier/README.md | 3 +- .../behavior_path_planner_side-shift.md | 92 +++ .../config/avoidance/avoidance.param.yaml | 3 + .../config/behavior_path_planner.param.yaml | 1 + .../behavior_path_planner_node.hpp | 15 + .../behavior_path_planner/data_manager.hpp | 1 + .../behavior_path_planner/parameters.hpp | 2 + .../avoidance/avoidance_module.hpp | 2 +- .../avoidance/avoidance_module_data.hpp | 3 + .../avoidance/avoidance_utils.hpp | 28 + .../lane_change/lane_change_module_data.hpp | 1 - .../scene_module/lane_change/util.hpp | 5 +- .../side_shift/side_shift_module.hpp | 17 +- .../behavior_path_planner/utilities.hpp | 22 +- .../src/behavior_path_planner_node.cpp | 107 ++- .../src/path_utilities.cpp | 3 +- .../avoidance/avoidance_module.cpp | 20 +- .../avoidance/avoidance_utils.cpp | 154 ++++ .../lane_change/lane_change_module.cpp | 29 +- .../src/scene_module/lane_change/util.cpp | 19 +- .../lane_following/lane_following_module.cpp | 10 +- .../scene_module/pull_out/pull_out_module.cpp | 13 +- .../scene_module/pull_out/shift_pull_out.cpp | 6 +- .../pull_over/pull_over_module.cpp | 22 +- .../side_shift/side_shift_module.cpp | 145 ++-- .../behavior_path_planner/src/utilities.cpp | 730 +++++++----------- .../include/scene_module/blind_spot/scene.hpp | 2 - .../intersection/scene_intersection.hpp | 1 - .../occlusion_spot/occlusion_spot_utils.hpp | 2 - .../scene_module/scene_module_interface.hpp | 21 + .../velocity_factor_interface.hpp | 68 ++ .../virtual_traffic_light/scene.hpp | 4 + .../behavior_velocity_planner/package.xml | 1 + .../behavior_velocity_planner/src/node.cpp | 6 +- .../src/scene_module/blind_spot/debug.cpp | 10 - .../src/scene_module/blind_spot/scene.cpp | 5 +- .../crosswalk/scene_crosswalk.cpp | 7 + .../scene_module/crosswalk/scene_walkway.cpp | 4 +- .../src/scene_module/detection_area/scene.cpp | 3 + .../src/scene_module/intersection/debug.cpp | 5 - .../intersection/scene_intersection.cpp | 27 +- .../scene_merge_from_private_road.cpp | 4 + .../scene_no_stopping_area.cpp | 4 + .../src/scene_module/occlusion_spot/debug.cpp | 12 - .../occlusion_spot/scene_occlusion_spot.cpp | 6 +- .../src/scene_module/run_out/scene.cpp | 2 + .../src/scene_module/run_out/utils.cpp | 3 +- .../src/scene_module/stop_line/scene.cpp | 5 + .../scene_module/traffic_light/manager.cpp | 11 + .../src/scene_module/traffic_light/scene.cpp | 4 + .../virtual_traffic_light/scene.cpp | 8 + .../src/rrtstar.cpp | 10 +- .../test_freespace_planning_algorithms.cpp | 62 +- planning/mission_planner/README.md | 33 +- .../launch/mission_planner.launch.xml | 1 + .../mission_planner/media/goal_footprints.svg | 195 +++++ planning/mission_planner/package.xml | 1 + .../src/lanelet2_plugins/default_planner.cpp | 128 ++- .../src/lanelet2_plugins/default_planner.hpp | 18 +- .../lanelet2_plugins/utility_functions.cpp | 49 ++ .../lanelet2_plugins/utility_functions.hpp | 7 + .../obstacle_avoidance_planner/CMakeLists.txt | 2 - .../common_structs.hpp | 12 +- .../costmap_generator.hpp | 63 -- .../mpt_optimizer.hpp | 31 +- .../obstacle_avoidance_planner/node.hpp | 7 +- .../utils/cv_utils.hpp | 121 --- .../utils/debug_utils.hpp | 3 - .../utils/utils.hpp | 8 + .../src/costmap_generator.cpp | 337 -------- .../src/mpt_optimizer.cpp | 403 +++------- .../obstacle_avoidance_planner/src/node.cpp | 62 +- .../src/utils/cv_utils.cpp | 470 ----------- .../src/utils/debug_utils.cpp | 56 +- .../src/utils/utils.cpp | 105 +++ .../planner_interface.hpp | 6 + planning/obstacle_cruise_planner/package.xml | 1 + .../src/planner_interface.cpp | 22 + .../obstacle_stop_planner/debug_marker.hpp | 5 + planning/obstacle_stop_planner/package.xml | 1 + .../src/debug_marker.cpp | 23 + .../scripts/trajectory_visualizer.py | 72 +- planning/route_handler/src/route_handler.cpp | 3 +- .../collision_free_optimizer_node.hpp | 2 - .../src/collision_free_optimizer_node.cpp | 10 +- .../src/static_centerline_optimizer_node.cpp | 3 +- .../static_centerline_optimizer/src/utils.cpp | 5 +- .../debug_marker.hpp | 5 + .../surround_obstacle_checker/package.xml | 1 + .../src/debug_marker.cpp | 23 + .../sim_model_delay_steer_acc_geared.cpp | 32 +- .../sim_model_ideal_steer_acc_geared.cpp | 32 +- system/default_ad_api/CMakeLists.txt | 2 +- system/default_ad_api/README.md | 11 + .../config/default_ad_api.param.yaml | 8 + .../default_ad_api/document/autoware-state.md | 16 + system/default_ad_api/document/fail-safe.md | 5 + .../autoware-state-architecture.drawio.svg | 332 ++++++++ .../images/autoware-state-table.drawio.svg | 457 +++++++++++ .../document/images/localization.drawio.svg | 553 +++++++++++++ .../images/motion-architecture.drawio.svg | 255 ++++++ .../motion-state.drawio.svg} | 75 +- .../operation-mode-architecture.drawio.svg | 323 ++++++++ .../images/operation-mode-state.drawio.svg | 418 ++++++++++ .../images/operation-mode-table.drawio.svg | 317 ++++++++ .../document/images/routing.drawio.svg | 398 ++++++++++ system/default_ad_api/document/interface.md | 5 + .../default_ad_api/document/localization.md | 7 + system/default_ad_api/document/motion.md | 13 + .../default_ad_api/document/operation-mode.md | 24 + system/default_ad_api/document/routing.md | 7 + .../launch/default_ad_api.launch.py | 18 +- .../src/compatibility/autoware_state.cpp | 3 +- system/default_ad_api/src/motion.cpp | 4 +- .../emergency_handler_core.cpp | 8 + .../diagnostic_aggregator/system.param.yaml | 6 + .../config/system_error_monitor.param.yaml | 2 + ...ror_monitor.planning_simulation.param.yaml | 2 + .../launch/system_monitor.launch.py | 171 ---- .../launch/system_monitor.launch.xml | 66 +- .../simulator_compatibility_test/setup.py | 7 + .../src/csv_loader.cpp | 2 +- 204 files changed, 6211 insertions(+), 3569 deletions(-) create mode 100644 common/component_interface_tools/CMakeLists.txt create mode 100644 common/component_interface_tools/launch/service_log_checker.launch.xml create mode 100644 common/component_interface_tools/package.xml create mode 100644 common/component_interface_tools/src/service_log_checker.cpp create mode 100644 common/component_interface_tools/src/service_log_checker.hpp create mode 100644 common/tier4_state_rviz_plugin/images/select_auto.png delete mode 100644 common/tier4_state_rviz_plugin/images/select_engage.png delete mode 100644 common/web_controller/CMakeLists.txt delete mode 100644 common/web_controller/README.md delete mode 100644 common/web_controller/images/web_controller.png delete mode 100644 common/web_controller/launch/web_controller.launch.xml delete mode 100644 common/web_controller/package.xml delete mode 100644 common/web_controller/www/web_controller/css/web_controller_style.css delete mode 100644 common/web_controller/www/web_controller/icon/autoware.ico delete mode 100644 common/web_controller/www/web_controller/index.html delete mode 100644 common/web_controller/www/web_controller/js/autoware_engage.js delete mode 100644 common/web_controller/www/web_controller/js/autoware_state.js delete mode 100644 common/web_controller/www/web_controller/js/lane_change_approval.js delete mode 100644 common/web_controller/www/web_controller/js/menu.js delete mode 100644 common/web_controller/www/web_controller/js/path_change_approval.js delete mode 100644 common/web_controller/www/web_controller/js/vehicle_engage.js delete mode 100644 common/web_controller/www/web_controller/js/velocity_limit.js create mode 100644 map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp create mode 100644 map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp create mode 100644 planning/behavior_path_planner/behavior_path_planner_side-shift.md create mode 100644 planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp create mode 100644 planning/mission_planner/media/goal_footprints.svg delete mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp delete mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp delete mode 100644 planning/obstacle_avoidance_planner/src/costmap_generator.cpp delete mode 100644 planning/obstacle_avoidance_planner/src/utils/cv_utils.cpp create mode 100644 system/default_ad_api/README.md create mode 100644 system/default_ad_api/config/default_ad_api.param.yaml create mode 100644 system/default_ad_api/document/autoware-state.md create mode 100644 system/default_ad_api/document/fail-safe.md create mode 100644 system/default_ad_api/document/images/autoware-state-architecture.drawio.svg create mode 100644 system/default_ad_api/document/images/autoware-state-table.drawio.svg create mode 100644 system/default_ad_api/document/images/localization.drawio.svg create mode 100644 system/default_ad_api/document/images/motion-architecture.drawio.svg rename system/default_ad_api/document/{motion.drawio.svg => images/motion-state.drawio.svg} (74%) create mode 100644 system/default_ad_api/document/images/operation-mode-architecture.drawio.svg create mode 100644 system/default_ad_api/document/images/operation-mode-state.drawio.svg create mode 100644 system/default_ad_api/document/images/operation-mode-table.drawio.svg create mode 100644 system/default_ad_api/document/images/routing.drawio.svg create mode 100644 system/default_ad_api/document/interface.md create mode 100644 system/default_ad_api/document/localization.md create mode 100644 system/default_ad_api/document/motion.md create mode 100644 system/default_ad_api/document/operation-mode.md create mode 100644 system/default_ad_api/document/routing.md delete mode 100644 system/system_monitor/launch/system_monitor.launch.py diff --git a/common/component_interface_tools/CMakeLists.txt b/common/component_interface_tools/CMakeLists.txt new file mode 100644 index 0000000000000..a5ebc29463bec --- /dev/null +++ b/common/component_interface_tools/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.14) +project(component_interface_tools) + +find_package(autoware_cmake REQUIRED) +autoware_package() +ament_auto_add_executable(service_log_checker src/service_log_checker.cpp) +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/common/component_interface_tools/launch/service_log_checker.launch.xml b/common/component_interface_tools/launch/service_log_checker.launch.xml new file mode 100644 index 0000000000000..f3099b3238096 --- /dev/null +++ b/common/component_interface_tools/launch/service_log_checker.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/common/component_interface_tools/package.xml b/common/component_interface_tools/package.xml new file mode 100644 index 0000000000000..a1eba3cb41e09 --- /dev/null +++ b/common/component_interface_tools/package.xml @@ -0,0 +1,28 @@ + + + + component_interface_tools + 0.1.0 + The component_interface_tools package + Takagi, Isamu + yabuta + Kah Hooi Tan + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + diagnostic_updater + fmt + rclcpp + tier4_system_msgs + yaml_cpp_vendor + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/component_interface_tools/src/service_log_checker.cpp b/common/component_interface_tools/src/service_log_checker.cpp new file mode 100644 index 0000000000000..ce89573356412 --- /dev/null +++ b/common/component_interface_tools/src/service_log_checker.cpp @@ -0,0 +1,110 @@ +// 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 "service_log_checker.hpp" + +#include + +#include +#include + +#define FMT_HEADER_ONLY +#include + +ServiceLogChecker::ServiceLogChecker() : Node("service_log_checker"), diagnostics_(this) +{ + sub_ = create_subscription( + "/service_log", 50, std::bind(&ServiceLogChecker::on_service_log, this, std::placeholders::_1)); + + diagnostics_.setHardwareID(get_name()); + diagnostics_.add("response_status", this, &ServiceLogChecker::update_diagnostics); +} + +void ServiceLogChecker::on_service_log(const ServiceLog::ConstSharedPtr msg) +{ + try { + // Ignore service request. + if (msg->type == ServiceLog::CLIENT_REQUEST || msg->type == ServiceLog::SERVER_REQUEST) { + return; + } + + // Ignore service errors. + if (msg->type == ServiceLog::ERROR_UNREADY) { + return set_error(*msg, "not ready"); + } + if (msg->type == ServiceLog::ERROR_TIMEOUT) { + return set_error(*msg, "timeout"); + } + + // Ignore version API because it doesn't have response status. + if (msg->name == "/api/interface/version") { + return; + } + + // Parse response data. + const auto status = YAML::Load(msg->yaml)["status"]; + if (!status) { + return set_error(*msg, "no response status"); + } + + // Check response status. + const auto success = status["success"].as(); + if (!success) { + const auto message = status["message"].as(); + const auto code = status["code"].as(); + return set_error(*msg, fmt::format("status code {} '{}'", code, message)); + } + } catch (const YAML::Exception & error) { + return set_error(*msg, fmt::format("invalid data: '{}'", error.what())); + } + + set_success(*msg); +} + +void ServiceLogChecker::set_success(const ServiceLog & msg) +{ + errors_.erase(fmt::format("{} ({})", msg.name, msg.node)); +} + +void ServiceLogChecker::set_error(const ServiceLog & msg, const std::string & log) +{ + errors_[fmt::format("{} ({})", msg.name, msg.node)] = log; + RCLCPP_ERROR_STREAM(get_logger(), fmt::format("{}: {} ({})", msg.name, log, msg.node)); +} + +void ServiceLogChecker::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using diagnostic_msgs::msg::DiagnosticStatus; + + for (const auto & error : errors_) { + stat.add(error.first, error.second); + } + + if (errors_.empty()) { + stat.summary(DiagnosticStatus::OK, "OK"); + } else { + stat.summary(DiagnosticStatus::ERROR, "ERROR"); + } +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/common/component_interface_tools/src/service_log_checker.hpp b/common/component_interface_tools/src/service_log_checker.hpp new file mode 100644 index 0000000000000..32c7f02e757c6 --- /dev/null +++ b/common/component_interface_tools/src/service_log_checker.hpp @@ -0,0 +1,42 @@ +// 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 SERVICE_LOG_CHECKER_HPP_ +#define SERVICE_LOG_CHECKER_HPP_ + +#include +#include + +#include + +#include +#include + +class ServiceLogChecker : public rclcpp::Node +{ +public: + ServiceLogChecker(); + +private: + using ServiceLog = tier4_system_msgs::msg::ServiceLog; + rclcpp::Subscription::SharedPtr sub_; + diagnostic_updater::Updater diagnostics_; + void on_service_log(const ServiceLog::ConstSharedPtr msg); + void set_success(const ServiceLog & msg); + void set_error(const ServiceLog & msg, const std::string & log); + void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + std::unordered_map errors_; +}; + +#endif // SERVICE_LOG_CHECKER_HPP_ diff --git a/common/component_interface_utils/CMakeLists.txt b/common/component_interface_utils/CMakeLists.txt index d753b4e92d218..d4f6343e381c7 100644 --- a/common/component_interface_utils/CMakeLists.txt +++ b/common/component_interface_utils/CMakeLists.txt @@ -3,5 +3,4 @@ project(component_interface_utils) find_package(autoware_cmake REQUIRED) autoware_package() -ament_export_dependencies(tier4_system_msgs) ament_auto_package() diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml index 78620dab76cfe..90602a65bd1b0 100644 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + tier4_system_msgs autoware_adapi_v1_msgs rclcpp diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index 57447d15cab7c..6a987dc269e76 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -37,6 +37,42 @@ namespace motion_utils { +/** + * @brief A resampling function for a path(points). Note that in a default setting, position xy are + * resampled by spline interpolation, position z are resampled by linear interpolation, and + * orientation of the resampled path are calculated by a forward difference method + * based on the interpolated position x and y. + * @param input_path input path(point) to resample + * @param resampled_arclength arclength that contains length of each resampling points from initial + * point + * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @return resampled path(poses) + */ +std::vector resamplePointVector( + const std::vector & points, + const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const bool use_lerp_for_z = true); + +/** + * @brief A resampling function for a path(position). Note that in a default setting, position xy + * are resampled by spline interpolation, position z are resampled by linear interpolation, and + * orientation of the resampled path are calculated by a forward difference method + * based on the interpolated position x and y. + * @param input_path input path(position) to resample + * @param resample_interval resampling interval + * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @return resampled path(poses) + */ +std::vector resamplePointVector( + const std::vector & points, const double resample_interval, + const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true); + /** * @brief A resampling function for a path(poses). Note that in a default setting, position xy are * resampled by spline interpolation, position z are resampled by linear interpolation, and @@ -70,7 +106,7 @@ std::vector resamplePoseVector( * @return resampled path(poses) */ std::vector resamplePoseVector( - const std::vector & points, const double resampled_interval, + const std::vector & points, const double resample_interval, const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true); /** diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index be3e4b78865e8..5417da86f5359 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -19,8 +19,8 @@ namespace motion_utils { -std::vector resamplePoseVector( - const std::vector & points, +std::vector resamplePointVector( + const std::vector & points, const std::vector & resampled_arclength, const bool use_lerp_for_xy, const bool use_lerp_for_z) { @@ -40,17 +40,17 @@ std::vector resamplePoseVector( z.reserve(points.size()); input_arclength.push_back(0.0); - x.push_back(points.front().position.x); - y.push_back(points.front().position.y); - z.push_back(points.front().position.z); + x.push_back(points.front().x); + y.push_back(points.front().y); + z.push_back(points.front().z); for (size_t i = 1; i < points.size(); ++i) { const auto & prev_pt = points.at(i - 1); const auto & curr_pt = points.at(i); - const double ds = tier4_autoware_utils::calcDistance2d(prev_pt.position, curr_pt.position); + const double ds = tier4_autoware_utils::calcDistance2d(prev_pt, curr_pt); input_arclength.push_back(ds + input_arclength.back()); - x.push_back(curr_pt.position.x); - y.push_back(curr_pt.position.y); - z.push_back(curr_pt.position.z); + x.push_back(curr_pt.x); + y.push_back(curr_pt.y); + z.push_back(curr_pt.z); } // Interpolate @@ -65,15 +65,71 @@ std::vector resamplePoseVector( const auto interpolated_y = use_lerp_for_xy ? lerp(y) : spline(y); const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z); - std::vector resampled_points; + std::vector resampled_points; resampled_points.resize(interpolated_x.size()); - // Insert Position, Velocity and Heading Rate + // Insert Position for (size_t i = 0; i < resampled_points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = interpolated_x.at(i); + point.y = interpolated_y.at(i); + point.z = interpolated_z.at(i); + resampled_points.at(i) = point; + } + + return resampled_points; +} + +std::vector resamplePointVector( + const std::vector & points, const double resample_interval, + const bool use_lerp_for_xy, const bool use_lerp_for_z) +{ + const double input_length = motion_utils::calcArcLength(points); + + std::vector resampling_arclength; + for (double s = 0.0; s < input_length; s += resample_interval) { + resampling_arclength.push_back(s); + } + if (resampling_arclength.empty()) { + std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl; + return points; + } + + // Insert terminal point + if (input_length - resampling_arclength.back() < motion_utils::overlap_threshold) { + resampling_arclength.back() = input_length; + } else { + resampling_arclength.push_back(input_length); + } + + return resamplePointVector(points, resampling_arclength, use_lerp_for_xy, use_lerp_for_z); +} + +std::vector resamplePoseVector( + const std::vector & points, + const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const bool use_lerp_for_z) +{ + // validate arguments + if (!resample_utils::validate_arguments(points, resampled_arclength)) { + return points; + } + + std::vector position(points.size()); + for (size_t i = 0; i < points.size(); ++i) { + position.at(i) = points.at(i).position; + } + const auto resampled_position = + resamplePointVector(position, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + + std::vector resampled_points(resampled_position.size()); + + // Insert Position + for (size_t i = 0; i < resampled_position.size(); ++i) { geometry_msgs::msg::Pose pose; - pose.position.x = interpolated_x.at(i); - pose.position.y = interpolated_y.at(i); - pose.position.z = interpolated_z.at(i); + pose.position.x = resampled_position.at(i).x; + pose.position.y = resampled_position.at(i).y; + pose.position.z = resampled_position.at(i).z; resampled_points.at(i) = pose; } @@ -204,7 +260,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( autoware_auto_planning_msgs::msg::PathWithLaneId resampled_path; resampled_path.header = input_path.header; - resampled_path.drivable_area = input_path.drivable_area; + resampled_path.left_bound = input_path.left_bound; + resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { autoware_auto_planning_msgs::msg::PathPoint path_point; @@ -346,7 +403,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resampled_path; resampled_path.header = input_path.header; - resampled_path.drivable_area = input_path.drivable_area; + resampled_path.left_bound = input_path.left_bound; + resampled_path.right_bound = resampled_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { autoware_auto_planning_msgs::msg::PathPoint path_point; diff --git a/common/tier4_planning_rviz_plugin/src/drivable_area/display.cpp b/common/tier4_planning_rviz_plugin/src/drivable_area/display.cpp index 26071d8eddf53..97a232fd9a549 100644 --- a/common/tier4_planning_rviz_plugin/src/drivable_area/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/drivable_area/display.cpp @@ -308,9 +308,8 @@ bool validateFloats(const nav_msgs::msg::OccupancyGrid & msg) } void AutowareDrivableAreaDisplay::processMessage( - autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg) + [[maybe_unused]] autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg) { - current_map_ = msg->drivable_area; loaded_ = true; // updated via signal in case ros spinner is in a different thread Q_EMIT mapUpdated(); diff --git a/common/tier4_state_rviz_plugin/README.md b/common/tier4_state_rviz_plugin/README.md index ed96226f6bb75..ac1e188fb36cd 100644 --- a/common/tier4_state_rviz_plugin/README.md +++ b/common/tier4_state_rviz_plugin/README.md @@ -2,31 +2,47 @@ ## Purpose -This plugin displays the current status of autoware. +This plugin displays the current status of autoware. This plugin also can engage from the panel. ## Inputs / Outputs ### Input -| Name | Type | Description | -| ----------------------------- | ----------------------------------------------- | -------------------------------------------------- | -| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | The topic represents the state of AUTO or EXTERNAL | -| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | The topic represents the state of Autoware | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic represents the state of Gear | -| `/api/external/get/engage` | `tier4_external_api_msgs::msg::EngageStatus` | The topic represents the state of Engage | +| Name | Type | Description | +| ---------------------------------------- | -------------------------------------------------------------- | ------------------------------------------------------------- | +| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | The topic represents the state of operation mode | +| `/api/routing/state` | `autoware_adapi_v1_msgs::msg::RouteState` | The topic represents the state of route | +| `/api/localization/initialization_state` | `autoware_adapi_v1_msgs::msg::LocalizationInitializationState` | The topic represents the state of localization initialization | +| `/api/motion/state` | `autoware_adapi_v1_msgs::msg::MotionState` | The topic represents the state of motion | +| `/api/autoware/get/emergency` | `tier4_external_api_msgs::msg::Emergency` | The topic represents the state of external emergency | +| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic represents the state of gear | ### Output -| Name | Type | Description | -| -------------------------- | -------------------------------------- | ------------------------------ | -| `/api/external/set/engage` | `tier4_external_api_msgs::srv::Engage` | The service inputs engage true | +| Name | Type | Description | +| -------------------------------------------------- | -------------------------------------------------- | -------------------------------------------------- | +| `/api/operation_mode/change_to_autonomous` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to autonomous | +| `/api/operation_mode/change_to_stop` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to stop | +| `/api/operation_mode/change_to_local` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to local | +| `/api/operation_mode/change_to_remote` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to remote | +| `/api/operation_mode/enable_autoware_control` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to enable vehicle control by Autoware | +| `/api/operation_mode/disable_autoware_control` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to disable vehicle control by Autoware | +| `/api/routing/clear_route` | `autoware_adapi_v1_msgs::srv::ClearRoute` | The service to clear route state | +| `/api/motion/accept_start` | `autoware_adapi_v1_msgs::srv::AcceptStart` | The service to accept the vehicle to start | +| `/api/autoware/set/emergency` | `tier4_external_api_msgs::srv::SetEmergency` | The service to set external emergency | +| `/planning/scenario_planning/max_velocity_default` | `tier4_planning_msgs::msg::VelocityLimit` | The topic to set maximum speed of the vehicle | ## HowToUse 1. Start rviz and select panels/Add new panel. + ![select_panel](./images/select_panels.png) + 2. Select tier4_state_rviz_plugin/AutowareStatePanel and press OK. + ![select_state_plugin](./images/select_state_plugin.png) -3. If the AutowareState is WaitingForEngage, can engage by clicking the Engage button. - ![select_engage](./images/select_engage.png) + +3. If the auto button is activated, can engage by clicking it. + + ![select_auto](./images/select_auto.png) diff --git a/common/tier4_state_rviz_plugin/images/select_auto.png b/common/tier4_state_rviz_plugin/images/select_auto.png new file mode 100644 index 0000000000000000000000000000000000000000..4772cfea3e48c1793c56fd114552312642f61311 GIT binary patch literal 37151 zcmb5VWmKC{*DVUgDc0f+#l5(@7ca%FNO5-!?vet9;uLpx4ess~cXuZNPWqnn{rZmF zn~{t>zHL2w&AHaxk*dnF=%_@fFfcIa@^VsNVPIfE(4Q|dBJ_xnLxUys58g#W{u?s% zEj{c;o0{cA=%Kg3cA+TjFgm=XoMWQ4oV=;E0dX-8Rhp~s2)Dv?bfN} z&`0G8@MkJlc0OBkDS`E}%c?qw!h4qJS}7aB6f%7}UsM|$dgK)UJ<9=;lNUcc09rHZcC z_h{*zby;IxOVubr^~6rT-0h2Ozx3(HEUv^R)@eT{h`RxL$6Mi$=uF50*LwO;7M*(g zRy}IdZ*c z>+`fXTgX4l-^(_5ZmgG^zLu@HRlx9~3?Rp~{$ab3O!wBo)E_-pW`7%ZcsVL!3hTa1;p9u)vQhgu_M5%Z^Y7UwcZ)3eUs&6<-vov@{K2tU8vW)yO_veczV7SY55$<5EF!yFbJ$q0P*e@xx$>Two~4bS zuDXP@Z#L?4-xIYi^5V0AXIUOS9OSOaLDfvQIB&$WID_Xu#@@WpkupBGHec(88GZ6Ds7AGGEQU=KStHR*EuwU<5dL&F3$jQ=7YYbK`5;WTuk`>uSsIdct(lfCJ1 z@=(n$a(H9tl+AxftT6Lpqp60ml^nauDtO*0<1YH(2E6diH~r3JiTlO!Lphr+&>I_1 z)W6)@zZ`b$X!=z_L@;9__q~`Y7wy=I!&2wqi=Cw= zOmcsJ$F|Y4>yVpzNbaZODV~ubY!#vIJfO%(UE(tq)aE>YTZ+7$s-F`rM*_}rBhfXz zo{x%{zQImLW|j}1Nu8LFN1UYe{WW0lgcb6lE?<*2H+C}S7GyYCjNgWFS>xL_Wqy?D zpUrb17vR2>99`|AoAe#oDlTijN}jHFVcAPvOm-am@oCBW0Ei2gQx_lQA8JJqt+BiV z)N-lTVbr?+fPL-$S!>Fv^C6QEFv}7X!#^?x)O21|Z$E9|d;(!E7$0&aIIZPgo~DhC zW|L8QAL0}$pGJ8MGx@eLkwemFeYEKn*8PInexGeh89IRqL{gUSSt4%H|HvPR+@8z_ zsrCSzh5QvJ(Se8E%o)=1Fvo5^;>TFl{Er+x?kZtc<>uRk4?s zKWN28qRElW_fq`ST`=4KmEU^8X#Zc~iAadwaz3gm4S zyyIT+UlR9guG#3iYn-@5+SBv+8ShPf_~vYwJ220-<+7rLjoEyN`#%ud<{VAwrA}3` z#2!Clf2*S0l6~ElG93Pbnlsr)Ot8?xJ$~Y~RCB8(5sqGDkto-5GC1AlVX$B?D3PyOqENR=Q%S^{AM;o#r7ujwKgmoxT*vCqm#`aaGR33k z%B>^@jDM7l9*9qg+3IVwmpoZg<-0`^B{}Km)*4h76$%|Ia( zm`+}ywd>-ojFuT8G?rpXeB;&7Pyy8UNI3GTN=SU>vHLQi=MaBCqmYu$1sjc^qKTI_ z=<=7#LcpS_XxyBNo&9Pz=)R}CBq6} zf5ky7o@gOKn*~WLurB5%h3F|&-i%DY=S{}n>DWd=$RjDn7L8hnF-k5mlt)W{oQ2)3 zhDdB&xNpSEsLrxTLb2>S#~Pz?=y@UwU$D5*H#*ltbnn$rvi5@#DAdqL{i5qA4u@p2 z()?kx(jb4;lWGE+?(*G{=v$0QT@EIC|4ir0@D;8!zm4HR$K*w_p1pBz7<#il>;3uK zC>*Acqw~_khCguH(rh4x5SOv};L&}->~7~7j5s}nG(}SXWF=tZPRmhTgdt4!WrNq~ z)oBswt?T>TuFxqFJYzspz^Wo?H!L2GAM&_>?$eJA#31*%0*$w@zWv3$!46{NrGlx~ zXYa#IaLU{9BA>ly)Y}O`467e*Ri%O6{Wi|~!1!*ojw|%@W1_yJvT3Cp1>kToTu0LL zSvW`e#XG|#|969gTL$f(M!gfqn zyB6p@xcj85SBZ>PV4E26)_kOgg`ZkBEr)etS5*he6KoOdTH| zkIU%W6US}lcB0&rtYkTpkD`>;n4|ZAL1NUAD5Z}4fs&9!AVJX(fasIVYSvO=_60#f zjNZ&FUR^t44Fz4mQ2I2C2GU6f7tZ47io6Z(zdG2au~l)~ylWj2xyK*8 z;Xd^r-zkw5H9Lwk%|F^2f0mG$$Jzr5Qk?iMr5 z7U>hdCn((91o<~uMEbv+80{hIU#xd9=|7T|aUKeFvL`2Q(8F_|iRt%Dp30HQU7Ubx zwi-(G-{egZW7?u%D=@Q2*r?AL!_773)xg&@jfQ90={;$H8l<&<&d&fW@ot_$uH$#w`8?>ms zB}u&@Wu$B(K5(Ks&(~c{JIy)cW$mA@^w4VjS_*xbjr+%$4LT404;_lE19 z$BdO74jiI>SEx-LZtucdVAo`nwiZ%UM>3)P-9g+xU3#TeEs_<82Fq-e9GXoM704*IQX%aiWD4~DGT z^9s73{xKblqXXS|cO2e5a~4%OmyRmum<*h{xRbM#zHeI#24;sQ26OYeher*YP)5tI??vae#y>!L=_#W5S&)dJY$l-Ze*q(H}0q^J!b0s1R033O$14}n!lGB7J+gp_uZin13+?M zF8vR~c4tA1bA)isj_wq|J=?5gXg7ZH=0icBa}` z(%IR0iYrzomMA_oRbE#&?LVB35u1=u{>v9smxBo!DyqD%vteOjCN_(*p+Q=rhlao|yy(3QA}ErEQcLiL*sdZyS7Cw-i@$pHnrsgb`2rFFRr z@t4pG)K26c{~dIrhCr#_oWMuGzvhslh(eiSL)?GL*Ol#2L8g?dpxXL>-IfW$1*8=P z#LOw4u9vb-Y;1jUTe*NZeg6I4M>sU9Z7OoMAnUH9XzYTCV-%a&@6~Yqnd;`Y4^^Hl`pR|EOhfWg%XD5OZLR-i#WtNU7%wQ;W z%mYl_ATQnPdA1yLl8f#=xzGw{a;1Yqulll=H7kev+eEi*)4wL@OE8iF(lQOQH%9H= zOElk{*qsX2{B|P;%D(u1#Ps(k(Bj6s=CVdj7l8z2(o|8-&6_@+*l0ttSUjzllb$oj}VQ<>qDg`i9iw+vVd*$p$#u8VL+7hj@z+ zbL0^MCf^=C1_k;-@7^6v(FZ=20oerv@H*1E3u$bS9cF`K)NS4kUTKV_KH)033s^1r za3$IkeJP_5hFL!qW=`P`c33R@IFrLNa5`f%Ej<$E@OW4kYFo#fzv0%zX{BlkDW#|n zg}OWaP(46sc+*W=5mKh!v&*q|1=dU}6Dg-w?M*Z4J+HuEMlSXI(L>*p!@FgcPln(srnH~!xY`@zc%RaoqscsXH?Ux^OYRiPu4Kl@0Ah5OH4^`a$C5U zfRi4}?~1^BgiVP1iqM_6GixxbC0b-+S>Jz=`JSKKk<{$`t#$qg`T&5KGC_y zK&U|GzLhuS`c@=O#XIf@6Jy|}pR~P&N48Ze6l?oand4~HXHld!R)P~kzvw8M00HVm zQ>|_^__*D3aAlqcDZ;j7v)6EvZ|gHMc8~hweV#-m*o-986dlWjC9e%_@4zk<%@SF= z)#*V}osdXRKVN=w8iVbAOe$`Gr>NUUZ#mmXSW=)NH)}Zi*{*H6B8jYiOg&UBQHT3P zVXSFvFFBCq5;kqk41|6%cU@shq z`buW!oO)&Go6f=~k-QnKRPMS)scpqAJ4aKwTH<|E0xL&YUK zXCNhftNQTpBE{}pyzXdSybPDpi<_!%sUibm`6D(-&VjZJ>v_!1>7@36o^W1@yYunq z0as?`=Q0%>V{w`@rr7qE=+viYB<68DiNzG(>s6u&iLM8d&~LNyOR(cIq1ojrDpHbZ z>_BWn=+#t+l1{MbGKzdoEehkkMeH52+D{u%M(hrg-yelpK36d_BnSTrfqUe#64Q1{ z*QuE+P)mFVP#F2N;`?h1(TNM0k!pC3EAC9r^gQE!jOEU)LvUd09b`g{1v-~%l=ZA- z4J={k<5nk#x|{z#E>xF)aiJm3eV%SKCajxrmQ%T%;S)j*<_?iNaa`X>4jeaUdObbJ zo4%W54AHDkB~&+}@;@D9e!O=#A$li_(f=P)nRMv29+ zn8|oC%PI%OuCbv}XzTh(AUmmP@Iik4sZ@lh5Cgo=5jxjBL+ZzHFe+bvSs1PNb-l&V zPU!o>dysu!X{0j4-*vR@*Ya(tVcPK?+{E@q=nW^`i=Iq&{Jav{G&PXJ;v>g&!h@oP zL3k7x68fBjOI>5PjOvcdi-o%ZBLQcdpTD;`HaVyxfN(c`OEMe2W;9{^RHJ{|BK!DK z#!o`#iN1EM0qN27>mWnBu*+R*=!oS0rA$kn_5cdxQ2=eG$I9=n)GalA$J;12C77X1 z30UYLC`=#ZjlPdh%~t9OGtzFHG$^%Q)44lh)+C=C+;K|ibYMcN-HZ9>AZ(MGCnu#6 zo!D-k%=`g>@5pD{0-ujl@1#U8302Tz8Rz}2Ed~$Nn{%xco<*Hf#L0ywhX~*fC62&d zva9DeG}tRVx#Ef*g;1Nt_Oc*{B>n3*BX}cphc9_#O`J`CV9&<-;bJQY+>O{Y*O$VU zSb>)|Uyjlz@3;tFnK=jkdYa0;&(%fSAq7i`_}YGUV{(zrV@)VCGVDh(CCY4{+S}E& zWDnF5J?zg42hWtt`&5EiQ>@4#zsb5$zgQZb`<&38I$+XN>hRL~$BVCN(kN*Pqss`5 z$}XB;9#5Hb*)C-;k-sLNF<`W8N+VOUa`c4=_jRdKkNfX`j%))_4z#2e_Sr4jar;fCcPLNV?$vJm4_C z6I|qA4bYVXbVl)a`D%*g;w3mBMT1JljPTP2l`9Rx{R1wRXHlU*!yUJCbB`+u696l+ zhc-7gWn7E+OR!~~H=yf=4L;8wX23a&kU8)mBYb;Dwne%eRRH^SPeaY3X9EP0#|;kc zy4C_hov-!%Lr;|u0G;sj$Zr7+L9Ksr?JqAK_A1bgZE{6JE9(D_a2VaJqSox`sHosN z41tDh_($)d)$uZ{!mnCXu(YJdi!P9u{l*BHnVqfCtw6 zxj&Sz(-|6S{F!P)%e1Iy9~im%4%^F}>dBodPJ$eD%^TIgyS)^n=;ZXxxl;zyJ2Ptd zuzz=_-`07@!O6Ix8Tpclx~uBvPZ%V(cO)T*%P5$QJEoTlVzFh)nF4Om}tXt*!Y+G_SMr;t$YT$rQ~_H#uyiv5t7PtX8{-+OEL0 zVbY)S!-Iih5*(Hqmpo$$V$6=3w!YGVVQx7(X!X`;73tRX4hT-;U5}XFk24Dudi>72 z$n5OwKQP!(I{*BkkdTmIbv}|(`l^w1dTRSsT|F`)qB!}7++!qBe0H`uOUpeSmtnCQmh`TTyl#RPV5nmBgf#EZ9!{2p#vGEB(HN$h5-66BJa`PlAGNf0HS< z=&Ys8&G%_=eZzshm>_y)-I8ki^q`_|Rk_MYEX0y+$wXJ<+`8Ohcr`@RYSMw4gg-Qv zL64NT;zbA=PbULAp{9}57mjL7u7gIX{^uHxC&=Y**b8qZm(Ld8ra`bb-lssq%7g zKX+2Hst5Oqh}W=;-8eg&HDNk@yR-%CB@M@xpBv7sn5O6SMc(I)>nn^AC?$ zn71^Roekr=;8%la32h#zm+Tv_J{N=S3Q zJamlR*!BDoFg&fovYO0~9C3{FXNu|uSWGo!ZnfCK$*hagOqvZf&)vmL+%g6w+m1bu z9|GopGSfZUP+rg!yJ=%2VUfjXKKyXwU`$oTU#58uXPvX?fF)QLWby>>j9OX{!?=tjiLd$f0*Vu7TU?$ z57`eVEm>@ctgn>I=>_uH4VV1A-75*?~&T&ceHHVzC;wOIo)V@CI z8C&<&(%|xZsgKx`uhc*Os=+pV{vER`rA$qenDI-_D=hN1@Piv+%+?lRl3oAxC zgkKErSr#qU?P4lXnq0azQnv>b($s8%@=`ZWY(z+~W0j{(QNC4I_{Hj-FHdMVu-9x1o6IrK9b6cE; z$DB-2OB6zM!uM+v(=Wt(zo?oSyn`LLqT(ig*Bi`!0wWEaNR-u;AuFYch=Acl9FG;I zi$0BE{uQAL`O*lQkdTwY>MZs&Kv=R(H9Mk0xhV@G-Q}Pay@8r3O6c)(`*jUj8{7cK zz4OD}xwT##uM=ss2ZrfiZ0yeyM%`owM}Z#qTmgkSoJe2kxPDCuP-^?#rIb_lzhMmI zDtsSC%Ko15Oz(ki`K35Jbo0=D_4d%OUi%X7<{n4WiuAm5eSE3g@D#&y&(yEMAwAlw zvuVJq-E(@!l!H#s{31$MvHBD-8UY$2ngHpaKTJ1@K45bCsTxq zkKyn;;fd{#85IN7sgt`ybP)m;O^7d-e(1T&Dg&b!bnf|cEK>ank)3ASuu3drd*m7) zy=r*B{I`jY^sk5UPY*ICcYbBjuHQ~wq8q*%D@Y(^83hvAL<%N%7-)T!HW^%#)*4D$ zoqVFRets%?n^5Xe(Qo|95%mh@1*%PQ?jxwGR2xrgKHu5NRy(+~VzQd8N3H1}x-f9b zlIhG*ExpB5GWBQwL6)0r(0XD6C*18a3r$AkI=i-xJZt=y7N8S8To%iA!O^S;YP(-F zyS;SH)1pd)l&b`p`dkG_iy7%2tSf%5mMd~_&d4hb{}H>QntCI@{kcf==>R$pm7E*B z#pe)3mY{+$nCk7n3d0tY84U*EK9P)P5A3#}#_cRa_Vl|eR|KcTiw2SCLeIb6-L}#* z!;p7W7E2=+z3E(gC{P%;V&@+;PuG71sm<$iVCUFZZeKM%E>X@1-8AKJ zs#OuY>NY{A=sE^w>jYKh{+riJC*V#xcw0V$t$my3x>oASC}D&LwVNE7I>OL%^i~?{ z?N*W-Nvq5cf4CE5{S3*fOD8|;5wJD`oD>qw8NOrR=cX`?M zglQ-}s?c?7UIDotXeT-Ps@8HYCRoRC1LQB}?3wNjIO( z8eS!6DJar`(3dXXc+FZ!R`-OSprTtFozkDnxUNgcDZ?jxg z-D{*9x7{eR=o$ya93Ghe-Ie&u)xk$u%&CL&`HGkAtJ>#h9R>k9&#fQJZeo+SK&3Gw z!&{huLvKcMk+VNGdMZg{pj>N}MGF*!wGAQ4qQl*V&{Ep}zyYC$S(^3nzx-r|Pknu0 zqx46sR7_Epn;Z)yOx{>mC{`mjn%(pH?2gCCtb5dJAxfmv(QpXrsFL4_y9!+uEy0R* zn1{FdG1qyC{re+|sG5zoFLze+>~O`7e!K8WAHFCqLice6VmW{)*r%27@1oKhD_J?< z4Fcz47}!&bj)+%rX*_IAevKOS7vmK#*57Dn`~2j6BwEsqPp*5yB{p|qVdZml@y=e0 zese|M8YjBmrxQ1TjLr-AkEN`VZT_u<@tWcCDEYopt2O#{CFXaRZHlY1z1bjml* zdqN*SYa^?yYhj%-^j zG+X5-rppNMrxlxaJ5zzmxoe%m1MgO%MK+_Ak(7wMBOT5Q+^{jniM^~Ah7^KZ8N-M? z-Kz)R4<@>uik2Q!<$1f4b5-xK2_s?O{xVEL6n1sEFjn=z?*+G5;<#CVS2~DOE|S#H z2nr4!B^|WoffnxFK~|-;uCAJXc<231qu;i&vx#|hJ_|R$l`;_Xxz={7Q}T7Zv~G~to;(zs>foE z9-PIY_w&ge0f>K!fzYN-Sp`$;;$~*f1pVf(xKbCL$bFf05|SRgGBZeRvQ3G>S{&=e zb%Fx)%c;DMaF{oRI=2EVBAN*x8XChB69cw9 z*kojeIaZ854MibF2qdehm}7zDi4>nnXfZ-%y}R)-yQ!=!faHGYr({UhpCB9hp8iN? z???r@T}+J~J=H${b(erBIKHbUpll?JxxiExl$`o;?cdRU3CHJph~?$wwPf1}C7vTU zo^I6nh|bgu5h1@|l=G2(W*E~@4SuC!p_dMv5)*WjyeTG6R_GnlH#z<>`}q@6@N2dc z1{)t=Nb8wXuRG_9@gdM&kk)6!=DnDlKQV@i>s~OICpcD!g4Fx=Ooyu&=g;zBO z7$B)<2kLP-k_Vo1ZdK6bq<^@C-LP%GAg#U&k5#8o48CL9n!w!L0sm?8pN8C_8}Cs~ zAr5^BJ>uMYE5Gz*Vg)Ot6Z6{Z&q_DLV4xZ(e@t-8keiPGWjH4=I<^pRtX{o$T)juX zi%ZXkic6x5(1e6aV&R}6S{DX{9qS}APYjz^q2g1e;uE9c(#}&>RV%ACO!@L6mlWov zp(qoZ`_@U>pFOHBB_*YVFRoum4Vmc8MDmAK@98#yQ7~xs(Z1^1rbBCih!q7>FomQ> zb*?lJ0g92z5XBUon9fJaHbHHhYiCn+vR6W`-y6gt$Rr@f-)uZj34~_9@Jtj;n9W%d zofTNsX8=ELXYX?y>6eKVB04&#H#;DXt-@O2dclOD3O-2QXiojPCAtrsU@<POUPkea*##6B%? zUm^{Kso;s_qsqPpxoM`XDgR6PlqNzAiC*S#7Sr23)Qx!a)uS7Nyh^084v+N_ZSY2X z@0Mb89LbO3@pr~EyF@Q?1h+)h{pSu!c%v{aKUB)T0x2SbgB=8)D+baILht*q^yA*Y zQuGh40tnlBU0Y|4)pJJ_;|OIQVsC|5+8+UU1fB>n=g1wV)Q|rJk3W+)yiN@ec*z5b zs)FuwzN>`nF-&n@uYyR@+(L-8x+J4Fm&velx{voR<4`)I6OBk!U8{@$0Fl(xRBi(t zr}+62_3(fI7$R#*#&w}$E-&7wazHDc$X8K+c%4WoRLHj@|=ss1Xk{zv25;!JaO zb@c-y(N8MF=8Z3Lb=T0~S1$PK6{F)rxrSbvfBV>&qE~edpM^)^DOIjOTN@1&;l#tG z^N}yhO1Z_Ku>L1cH*<~6{RObRXo%~V9)3#Ipza1bll~9c_fD;NJoxD+^$j2DL`+|Y zk$C3!`a&^j^EywP94uL#XYD-UdXVo~4o-vi`?Hb)dr7{KjuNWysv7VaKmk^+XbTK( z(`@LzX-_d|b>m5GH@g~G%t{w(h^{(CJh{pvy)x#aOZsJC)=|*F(@n-VwzaO1AMA4} zP&+il`|;zk|8=R$xVl`9-d=e_K0QH9Cjl3JDC>n)FK&Op^kR`__Cx8o8~(O~bKTyZ zd!Hx>51cAXGTuFHWVE$UHXdP5pk!++J0S8T>~r%4c=*X`Jbab2nL9R3YBqluS9)hR zIVLaRafg)4#*_QvoL-1K)k8EpHtL|W|7cItKu+IG8Z6=H__@1VTx!lvqR9!9{RW85 z#~szA)4RPzlf59+DTGwvsa>}M*pw?HQW!S*TFN0J zy5;m6#=1df$or)H?82GT`X_vU6Nk+cDl+F}QRNfPFm94NLa@!#sN8=-3r`wvDi9p4 z&6o=fTN|UDWFMZHOwJ%uE>=5(5A66;f2I^Y0-Vu@0(FK(>IB0!WMNEtW@P(oq4vDkN9}oQTwD^vj>ox^W zq8OXVTh~VN#@Z5on&F;6v{I-i<#6C|o@;^SH8|oiIUhgVcs;qw%kL#gsO8ki6>SkV zu%5`99C+TIHk@5ya>^8p!VOo7T~sGDp&B5+qc)CtIjO%iX|LQXxK=8~pD?mo@Nt zVT=u&#%i?pfPmSUNxbxPBhXxJf18|<_3PW;zGt@+?IwHD6Azl5ZwdpvGak2csQVS0 z0Sdk8waKAvGN0WYDO$H*wwCU3%?zi5dE9=fk9Gs`v~GLACz==njH;aw(^GAudWhQz zuhUSW;k%Qwo(JH2N)}Pd(un;bs%USnJJFqO-2cUH^ZvRFf9ns@si6&N-h}CY^BrJYVF`l zC$o5?;=rd>e6bQ}0v!yno?bC?6-V={7p@U+0ON8%!Q_B`jFHE@tXFi&|{}Y8C*0z2s`0Yd3cKZ;Ydxo{- zk2<+NozX$QK4N2VaS*6BekHgpd@j4;v%VjjvsKXg_e)sd{%nfKqIQeL^4Br{LUzbH z-ecud)S%iWrWJf!?T`j{kZV!lWK+RTV;J_Po-do(ut-kQx@gpEr43)8g-AzwV?o^G z?|bhGz|0wgwW5+oiN2*4zJ+gi1A@VG>yzJ^GU}ZAMjM91Zd0$5s{?l6#NQxRzZ>m( zh|0A>Y@U~}CpR044Swy9u3({up|w@FMFT~jb2Zw({$@{=@8&o$?SFS4TrbIX(dAPr zrF)p&uD#{m0jsY2lUH-k+&b+L{~TE@*}xlX#~ZwwVgMwOpe@T{M2l!kfcugVkvs7}a4VrOm_mpl$Gbx5sc^in;5_wu_5v z&z{keTC21$mAntDoOevL?f1fxA}#Ueh_Q7|gaq%7KjL~G9~&5Ski{IHMc4T_n4nh| z%^_%XUgbSqXykdF4->+xw1sH~D#tPV{4N4H4EuvlzNpGGePwznaxdKrN1R~(2qf#) zotZ63L9xRMe4|-~l$_JG>-T8jZ_gz2xqLKO8NendSMl{VdTpZrpwPz9E_kDRTIWz? zl)m7a`bZQm0O}c>^~NrRQ@wQXd4J){1KSA-7k6T@6x^0yklE;atX^aOI}-{^N~QcV z_^rI}j!bI3eecxSQjBb4Z5G&g)jxX15m$vVcTL>LN)ttHSJjs7gi>{LpCA?0I7Ya_ z+{yN)=em9C!|fK2mfD#{XiDs~kjm$WBboggBx}NZwR)&rA4lBh_ue=4_K8W+(pNfuLTSlMJ#hO>W=mkM{u z^3eJYT}zydYy)Pb?RC$&aP~;M)(qw4`HsouaSsIimbG7#sC5`TKUs82_ufEeqRBr) ze*J*%m7P@v1Q`DhUG_7U)bVB3^qt_G*em6L#<$ozS`IsM>Dsv2jGQN+(YM_$%{FLI z-=*aIjP>Au%8dTO7a73(qCIru;L}>xb}bGxo={p!AN2{JYfb_x&(hF9!N6ekAX%2R zu8KM($NvHaHxW=c2UU+MMB}lPz5vLaj?^@gI(!~JVqvMC^VI_93~`r5S$~CO7k{X! zsUZ@f6BJRe|Ob-@kuP10^RWlK#`*+?~-W?&K zp&Fh<&}8kfyb!d2hN%C~8N~macpQ0a3iPtJ48pz=JhGgT(>N&O-Qj0P6+T{Z3H) zHxxE#$L=1IdmrW$F$y(Wh~5}!%k5ZU&UmhqI6vzv_(ou`_)u|XzsAfJ3R@B72iRa5 ziQWtNoIHGTodmZ(Zhhjp5K)|2k3C&zDAY4$n%F5zx%(U4{&??UE4QEylE&^)#AuuZ zL}A8pp^!oDxNBys63k7t<{o<0sZaFxitjaN*D{;vq@5O*r8w^dn zFFRl2AVnF)1vy%d-Kz8U#-RHr2YM!Nt+`a0for6by!H5{6 z^__y>qTl6%L@WMsQ&Kpgu)S>*L0awDP$C7F9H|51uDl?(bIW@p>kxE@&f9lYlZO3j zD)8g!i~TB?y3cr>kO>)JI^xU{^Mw)))YQI}Bxi%Wi17X*PBr-Xv_6RTdS!qn-|NKce;IjNZ*!j7`w9bvUGV)f z>RMhabxxUXX@uy3UK@7%A^3AKXUwmjh}p08N#Ahi=0tTKCCLJfQ3o~Is0z9%3WZ+L zJnH|B?f~Dq#67NR@kNHuFxdtyHZxZ_>)F@u96s+I`Eg}KRUzagF$d9to-TG6*5@KT zl^D4W2ZbpkrPU{&H(o9T))g-L>{LYicirXN*|&@VUoK(DY{CA!ktLILB9;_Q>!N`X zKQM2eYri2|%e+;_cF#n0QVo8t)#6I#7}?W#6X4ybOZaI+w!3WAd-?W*v8lZ+_5G<6 zF4S_#l8{AtkrtV$cS_0XIrOY%>qL?8EB7w>jWWtMH>yd`OoBg4H~w*Hvabj7xOfhu z;WcKAbSyr6>Qu4_TXSRE!SEl#)FTZUyrTdtBD!064{tQZXD+VxHV7`((dp4VRE^u( z&&M`9GM&*155AcC%;efrvV&Xux}FQsKyX^U1y)~XvZo?HL2ZS z%rV=<`W#}{w^QvptWT!(E>2VDe^i(w z$)_giDw2omiOSRZFSS%{j{bC9Cwo<`ml5Jt`odJ_h0QHvdtxlO7Wr>uTB~1GUZ)|I z!<&hD=PRs&OZpTyZMINTSY5RlCGBw-A}&o^+CUAd)mY|iR!c#Y+C`jG^UFJw`J(Tm z5AQ4x|9nxt{!p8{T}-UHdR7dt&c+jyZ(6cza|d&I#g6k9G-dkvS=G{Gr=Z;kw_r3$ zxd+3NhY_+Xa~WwV$GPwHNiq$pepSnaO3x~T%XEGUq=m1i$|+F&!JT^PUD2t&$HJC9 zoZcDmxlXB2JKxe{|3E+#=G`aLiAalZxtow$P9?NN-jAgm4iNWk@j}V5F*)R@#cb7RPD~C9ZSz2UA{-{{h2JToP&smz=g!0n)(=yXwot@>ll?ZWyz*jdp(6L z4m-EXoKT|TQTDC{pU)raF!o9vn+{$@@qP-J5omc>E37oyS*=tuNrv6+bV0aDIApO% zNN%#gzn+! zlPgWK|2O>=_muRE!$>B)|1&ik02G>M#lwNkbF~+o=;daBk^I=biuFoW2ilOA5r?%} z0qsPSn>4?UEd0o(VwSF1pp;RfU&Q9fHjwUxc+YzNg4ZPxo$D#+_R2?l0+ znEjpWpTo=U=tZUqF{tac#>D{5-(a`YYLURw=?Mw+{JqNq8@<=|t7ljg;R_3)15cHL zsrN1Y2sY+jo!f`V7F`m-xX;oJn`tWUlF=yb&*E(NLaV>cmHxV{d2}m+H$k_^nlK0o z(X6+0;eSNVA{K%v8-@K|I90(0;tKuz3DT>gJ)wbmSTb~UNnod-}~x( zUF{I7FzjS{!^T^#w;YzUw#6j7MPKpOUp<8&j%->@HcH_(*(L_P{+ykD6aqG~_C62% zCIC<@t5W*RS>=2^U{A?DuF0`8EL+5!qA6Bn>?QN~Bkz+>Tt+6~|DY^$`x9+F8h|Kz z{WzFjt>sGf4~}5{Q-kTuO1SMCYe6n~lj2LPDO*}t6=5}%F*M%~PinCvrA!L!xC4kP zAwsL2nh-SHli)cacjq$!2%PVzc2-y!1t!n|#=__=cYBNpMwF>Dsa0elnu-o{{$oV8 z%F~azRfOFZMeP@~qR>*^6xpmr#ejat^=jzks^+9Y^iOo&CmJZUr2GF8n&bMn6Mk=6 z86FxEt6I!k)Oy{Ahgx}VsaJ<#yCGXnPL4KnGOCh#$)$pd%Aj?F3D29T&jTsBbgHN$ zgtINZwr!Y}SVH1LNsS_a;@vwFNonTdrMo$G@tSD#4)y6?LzPp~7q7#TfOn{JAGV6$ z%~00g)-t4@ao&JUd8tFtsm0`$FVYt~14HS>|WEb6p z$S|SNa;QS6Q|4rz@iA4AmKpE8nwpwylH9+lveHKdjcVLnRa?*R(`jTvTTWE^x35^& z!!y40pK1(U$<8$K_8vz9`GXVnKW%;l-^YKTS&9VZUpIYLtXKQV>S(BQeg zqg-NF({i1`c9H37ApoegM>t_TU?fYI-!8g*;1z;-Ve#L&t=uzN%d-=(r2E0x7Y0>`}25Xa$9A6_eF>J(u1Jq(8j^!v> zRm?e2c7m6cHm!Ol0!xI?A%y4l{Vovy}uK5!yF(&;NdnLH)(~qK2_nr^?2UK@;RgJE-=6dE_Kr+h#eT~RQHHqB)yTYLXgb?%m(3d~I zC2U6A$OpZ^$=~`mn4%u($m3tIedDM_=G6RJ`RCu%smH@pi_uO-Yd3j+794vzFI?|* zkgRo#Y>?A(zCFY%c-}GR0xCJ*utg8;XYk*}wN`K1UMuJo`1T-nyjxM*o3Iuc3K#s- z{PjJI>)S2~x=eS{gz>cmqBVAbf<52P$Ni0yN4@qy8Dp>b)sE_KGa;Mxv)XB!T~5tl z*^Y+D`4dV343K>S23F9PK4p9V61x;Oep#i z4f*3t>WRtq;6ZXAZID9o?V`zHxe+b(+Z0k%G%d}7q!-1wiR@>7ijQ!l2GqDuFo%-g zns|!Z@m!e$jide)w_Sm`5Z7agtD~p}frU`h9r)S>D6K!JJ7#jZmFbnZKl%ix5WryK zMXc(^t~yW(=y81QFEWW&4t`NH5YS;9pXNdv*9_;a%JRrtoRbL0-R~qO@rnb z`7Yvk!DjE8uE%tmx1ai00r$PGm7)TBv!gbcFR=2SH$nJF@ZvcpQ4QDQF=|H_AH|GP z&VKaj1#4EOtm}8~#&ncInEC9v$4kQE>mAwXKS3=-8W_~^pGSO6qB-)9O{!fz^!Z)s zr77WGG|pg(;65B?CIl;JmsCh^$CtuLQBECJ3Xi_Ch#pw+k@6GhEk{XUY@nP7bkt7y zkKvZ$y~S8gkV@W$dE8uf@v;>wa!u4UdR^?bg4-d^OaTGR&2s{#RHLyP{S;=R zlEV;M;KL;L={JVVpHSvuaLI&c#Vt)H12L40bt^zf>PcWUmU>D!Qb->nJma`NH5M^6 zT$eQdiz|ab4%DN2gY7xIckU}mMYNK|o_l+FD(c(w$rw@E#P5VW(8WXM<6B4)cq<;C z-sk20(fT{XQ0zsU+5T8GgC>KAClcuIGRoORe|Y?RZ%bchy24su1ydGhu6h;M4RK8Y}diA;!YNByNf~*Ahp1Cqnsis zKBG92%(h~=iD5imfqfA~{O`Wv2gwF03SFS#E9r# zD*H)iFT;zZJBcs;78G*8&yOGiF4-bislyDLxA?+3PGJ`C5ssyNMR%_Jk$z(M6D1>g zTA%dq^=K@%p#c*Od3tB~UaP*`?7(S5v4ywS)5)=-wlVyBEHzb8j#5J)p>==T%Z7Ee z{Tpxh+a($NO4Y|%KUl!?<9;pY(MB_)ANhKJ^Lt085j{-%d7nl%pdQB#;WejjbY&W- zX3af2Om*Nt!Tc)dG$ZA(-31mV6K#cqN97XrVk@im3~xO+knjyS_dKv(d38pBS#2&< zTxkY?Pgsz%tnMDCRY=ff;v{U{-Fe(Qg%$8GB&2q;*4*+#O^)h180cwD;2B!^s|t>g zQcB?v;nzlHi3*N4cDv0-Kk7k7I$m{LGnyLffzv9A@YCEHhWy{VYby?;e~l#dE7~Y1 z&_E2Hgb^Zp3cm9j_@KyI@iDpv!7-(cZD0_`hDr37fQTji+mdJXywIOQubj4e zOnh}Px<2rLsSn?d@?7kImbtCQig-bKfnvmjjm>g`1d?8(TC@if1Og2QQ!>Mozw`iiyD$)Hx*jr{>mPjQ%4XRv8EiBwJ^t3%hOZzZNN4L0o^_( z=Jybe)ZLkjToay6I#^XpKjki&ZI%eZYwimoRzs%$CC+@s$qkF$|0+$B|5aqE$uFPN zG<>3Ih9N|bHIZ}?mS#$K^sgZ+u0m@fi^ld9b4dC{W@!PP-0b=7XLLwSImLz6x$eIk zmXHW5s>%xS7eLBRh$D8P{Gr}Tk4v$ry!^1fZ_=W%!RyAt`}R=vz~SyA8pLC3y8$4~ z8rQOX{#g}Yj!#cd52;mEG&Dk2&uafugWB?#@np%$%2v2%Ks*f%E(b~rwT3bM6qLe} z*buFC+@eUmOv$Y!K>)>nUU-vQvVCrD?nx=w3MAlhZm=nfshIpfuEQz&{&{2vD%>uU zobDmjIu}=SoMJPwKL>mfq)ZR5?l_3B zKLAEG!NxcH|M4Yx`eMKsU1rar{aS z`~pEf=JWQ~e_r1INx$%4=lNfU&3%5fe(WNHbSz{EX@OeWvNKK>`h9hy4_k>ri zVs4hTEupot9gp3+33vl#U2e)(XNmIp{6uV=g#+U)fws9`hkkE=g`>NEv{%3M9KHO~ z?5^D%_r~jhzw3BL;9wvP=(0kZL_Hcfa;V*lG<(t(F4Gfve?G*@&21)amF$UB8~rf} zTk9V{ebo7G+8;cbzVwE1-hLs^ zgHP<)o`J3b8;HwTXo09&3q-*|-gv_Bah zLc{s54mT@#)@wcwY>5vO+hA6hY?qcaDdYFQ(ZTnz-3QS2hl{8%0_I0IVA~bi7r?ag zsJ&ezC*4wb!a>w@&d(EdN-Vl+nhU7MVdG~Q@_;vp%dmCIXO8>DZcRlkhgg^Gg!Utg z{veM7!bF$XXI=~kdD|4ls?LCIP_2j_HWM*be8MmqIqqp_TAzJ9q}$F;;C)kj#r@pmvhY(On?ioeT%S)7+3S(-|u-y zpxKIVzl|+5#F7ezzcrksw^K*4*x}tX8H%~Dy0qI}&#rl0ThJ)K#glrwC=qVt-G3D9 zn(T^gNXwQUwc5)ERG3fhkS~gi8|T#RM3r5brXQfxcAmJjc)Cu>EeqQ)v?}MKS*{I) z1c*k(y+6{5o>qgiN~QSNQF~lbYsv8&Iid8~u;5jIm(?+nN3*1utj1Px;#ne@#5f&YNBzh+0w%fN-zMrJ4#PcHS7K zQ^V4HbPkoL**9d3N}0WDC*d=^v_q`}ppZ_7_OJ}ez^Cn@daEMlXkR|m$X-FL+H!kq}JVy({} zoEOJ+I__sN+1hv6$8dPWBBvdG{0Nm@AMdQGTUX;0nJ0CaU}^)B>EGMpC{pPrGWnuT zo~gv&{96z@rV6BN4`u9%y;f`M{aM?&1FBxWNt$tlvEY|lnO=f!LJBmM!<+Pm6Y-94 zcP~u9iE*wM>#~(8L|A8zUcj``+&czZH+w19l)JeF0AKEM&+DH;esC<=28;_>h&iF_ zq(Hwj8OGyYwc_k>$m0Hhyg)`iZUHTT89PpyIVA7bz=Gl7#)qe7?+hmO<00LZ zY}2Qh;KXYNQZ&nNEKN4yRV~ zV<(Ao0WR9&w;Lr5A=$G}j6J&`vO;g@o-SX6*&SN8S73tIRYMk`ZV@`O|NWS6(7}W8 zWKzIMj3RWGE#8HqC)@!ZImgYYw%7Fp%F#!cOTBM5rPvbU&Mr*`2ou+EAE=*y^ae76&`md`{jrWbk)|^Dno4KK8!uM6;p+Nx)iS$tr^3Vze|l z>vNI&(~a@-_0HukOV+T3*~&@5=`#Y~Bf=N(vgO$7R`1@}w;h1-V^vdZp#lE5S|(Oi zYP}`+LuzT-ravP=EGA;PhF|ei>odaJ-6;)An;mY=n%D0BzM9VNCsEd*{b8-!-wF#X zk8yDYYI)c;l*{{{1X~FbW}^=B&!nbbt55jN&n;gz-#MH*ke4}bT;7z4N8$W%s2}Iq z&8qrhML5Gn(z0b~)k})(Y$ty<^go`A*6FmB!D`D2+>`|j);0p!%2T#34{i}5*KH4C zpL4ZjX+JQ$cj@{2}U!Tq8TSa+FX6Cr2FFTJvB3{|NfRYjNrI&9hQUH+-+L}(ZgYFDLI;Uon z$C_4Htas_Bv8M z^7!&n;o}{Z66$NM)Nmn*q9i+^O1+xVKwM=5*A$e%^RlrcTAB^)mbAEY7qwF!tUOHp zei>ZX5v`tyHr^!9tMP^=O%06ztWl%(*kLD!hVL`I5*=MJ;iqg~2ve`-$xnrmtNaQ&i-f0p+I~yp`KE{{TGrtw16-~g z=0v8E7nf3$;tMC({?RDtBpy1-S->wNR5oA7m>Ioea4p#P!e5YW& ztxERJ3&$$*@Oa|MkJ)Y55!SzTX|Hl`)&j?j7~ENmUgh+3e0BEM3IE#5|L+{6Y+~hw zNSKrkHdf5EVCN5T{5lKg?>%LhZl8f1{K zOeEyQpWQ;P< z-|hH7N3ehz{T6KW9{d_eO(ftJ;$+zuI{RrFs>-6j*8zRQ4aoTIgg{5%I+q%5C>wf5 z<`0OAR|o{*=CdIfy+9d7M$pqDEZVt< zFcGRd>8ceneE5=u0!m;vp$kbF-AY{q#UX$Ip*WtGN|;Fgnn%6%CQvH=f9fiJ$A+1( zRCx=|3Eg^-V4_e_)>Mj15~2OR&eyfHGDF%R&U1&){+PKgMAH__jBGkKwf3>vbWVP& zC~s?jbt|!@Z#1R`N1+=w+!u!NxgVr*8xs{I$w*M`cy2v)$GdclY8Z@)=@3LkNj2DK zF1!^beCbN}NN5)PjOw9mAN#?FL}$lj%n=+4mceY4{ni%qA1|#SXo@&VB(MKJ`z>4>Au% zhCozCDqwg9VW1)Z6nDe6=>=ox-m1wmk4_8Bbrai3Shvi3ZoL`fcdb?hN*d}O#|gN7 z?CO>m1U9U6Nhx5OW+3hD1p&x#_bW05x|I{d%Pk^65QpXUqmm$bZV#f8umVmb5QYV2 zsS>v#Io*U>t)bO)r33Hs#s4z3ce2W4Y2<9aM&;)N0=+TSvTJAFzbvdr?&2gD<7&2L6tEeDxZ=c z04u9B5NWUS*Sk{8 zLTMopx;lCVYm}YI@ENzC{^`g4haVja2tS_lKdMHay6P;TKJnD0FH{i6k|N^Yhvwch z;5(ya3q!G7AXvH!@mUw zpI3tjmTl~^e?}nFVEa}x4pJ}|SClH)5LVt}|4HfVYsFFIOOO~;ME{q6G^HWU_H_c( zF)IdlXsZA$0HpRwq$AZ3*45Gb(*hH9a%T5@0S&n9_gB%{2uuF!&yZO+sXFRXyoD6p z_UFgaXUU(@|1wbi-=VbSeD2-dy#+;RV22M(n-$T&x$QrdDgqy?-M65p2uVcq6z#S1 znzv&Wa4nCGiw? zTYPllT9=qoiTQbD)7z^U^d(B4m?wW9G-X(%3S?SMXGzIdydFWK*QKNRp)akB8VA+= zUhW-3yCxm_Mb_~)|6T1$>t`lMXv?Nh@`OhaLeNG^YJ-Mesc*Y;LjjjN4WEt=M*awK zS6C#K+sb~c%_2*-5-l7YoLf7wQ#QBF0&3Q{bObu-u^>I|#&~aYFG28pEO7Lq_iMgr zANKyxR8PWCE3CYGtFvQ1bHF4OY>0WlpZ7nCu)xY3#d_DJP=}z##zqKr+O~fFq!e8a z*SH+JY)eB;UGg75z+ti05E2IvjzMmu-HGXDHJ-`+FAKN94EUrp5;dP_g10r=s}Ho! zBD#d>V;Io^eKrx1!GF2Ow3UBJmxw^G826#EgRS!5&a}N`{69zoNoK1|X;oD$et!Pq z($a-tkz=MuNJjmCn1MylLzU^k;*ydEtLefgrJ<-YC~X*HOk3?n@(8(-3snQU@&!3PizFX?TE6)dKMB^PdkTY|c+8g%zZuelKd-nRww zHr3o%&-6{6xhEwjQw7ywscC*_xmEq&EC628utwSZJmlWc_9=7e9YDpc)- z?<30-mn^68-J~60kNiOPA(M2+{T}KaMyZNR5n3AR9eTvU)`4~jr;*bUxxQq?{Ug)~ z5`51@iionRs=zDnR7s|n1JrtuU?6UA(5t7$5%Tocc`zcY+LKK-F7=0SXs_y-p4AVl zRbv-u8f0YGYg1($g7wdctLVHv*De113Phu6?KHO)6K0zCy0c!8QxT}UPyQ!37ZzB5 z?o$R{i~+GCY5CcCbkeHuuYjw{mwr{xp69=8^{9S! zOP9~ICU)SaaFe=CcNcn*T>VVXI7Y)s8B-0(WZ-b7bA6hMZ`#0ubHu~nx+a$&IWf+G z@3~GiHuReVY-y`Lz10l<03c4TzglSpS-(H@Oj-1KKlG3lpCy;$$GS#GD? z{w7wL4cN_K_09IPU>Eh{N-ICv*sX9;lkh<81O14N>bH>Zt)wy$efE_e@|#HQVrjb9 z67mVyiSg(GmEGB|VDtp9+PA0YMoHfm?g0eN$?Rx^`lLuvL92gnfujA7t(F9-H|Uey zJ;~LvuR`vVBmxGPP)QT=6bA`)>G#tK)$}&NzbCYgCt$4m~?`!T^0=t{~a^Ih1W2M7YC?n#crsLyhsQa0$e)f!t4 z_B&J*x?1Z;9S?x`ne`+xN{iCYET>l=oF{3eP&>}P4Bh=MB56t(*<=qi>qpk&d!DIj zCS<;`%x$}-=W%V%Eh}B?cYLrkaC@e;b+$h2{=oD8D41ccci5R+Z*$M&o_4Gwh-&e5 z(v!(B-1!R6jK9oBD1Os;yL33~(CNelaJ@wLLcTe~C{Jpoc8ya=R?%iCqF}+fzodg| zgpYt608IQo&~y-8Gc+>%)f$MA~{1RDcCe}$TY5>$gtfqM7)VRo~}9hMF^oJ zMb2w$VHRNSd22G&3?#hzh_`(Ub6D=rsw9JDRFo15%V^=kB`wv5ODtoMol!3BjkDot zY*Ce_nq2fN+oa|TW$j2j|Lh|;Rxmw4Nf{f@%5RWBB)cv2taWoxb5aN{6JN$cApYEN zsnv>8S5H&`N(bKMVh5+a+~rXs;_VH@BQ`fPd zZG|9T8;G#%xgcp#-JS^S)J#(fYrD5Dc2lBS@Z*4);0yUKoOF@*9J~;aH<&I`!#)!~ z%KDyJ_3>r!H3Y%Tu9#3cQkoSsw8!~PDONm+1INa3`?_4G1|Be>1{mT~J9Uy|6ssn@ zRNr&>Q{oG_ax85hJy~05-LMj?4u2~s)RZwXnMH%|XDeg1 ztt>@mC4JH3IW#->ol9MhW=4K@g7-~?y%l<7xu0osuM-V5Y%*yH{`g{$l78ho;mK`p z6wKOu6|BpC`-EY9l-9p!oc(7p<$lBa4FPSl}WTf zn?^ALB5d#XxNXp||A&rfT2>g#5(x5@QpZ0M)Pp8XX~j_KzpHwVo0Jjyo%70{cj7a` zAQ(NCIT8KnwYG{(jzlUM+>2{2Hs7anlxfLENbGa*SAuH;R95VK{L{!bvpw`|(Y@q3 z2s)UXYdr7^5U^K$OYf`H=k3UI!Wl%-v-KpE>3eX|XgLjJew)lBxQ(icV6e92i^EvNu?u&%V)nUe{d z$$+vL+L;83dx{9=rT8Z04!XuG=T5-HJ_U0#E}0{#f^!Phn*U{ zfFa+ntZ3uCt2B!Se+RO#c0rxn3&FO%uDfJwIW`D51j~i9E^VaTdLgnKlcJjp_T%GR zXtdBKeB1WD2bN3;8TI9K{(`&FR@K^&Ed}g)hVrl*E=CF;dCuIKcJegC z)vAeUveME#a4h7RM5l+?WqiYlF~j3ypE&Dq*H>aKocgZ6Cv%f= zDmim<{AIY-Fu+NstB&F^)u0V)*Zpo`p$_7~&a(xYR(Iq^{~x3f8)>bQN))w-2yz}} zQtTK&1(-f0G(pv>VU#|q?A%@{5-630FI$Kd=NjH5Z>v=g42HHNOaqdWA0rzy zi`6mO=$r^68wVGLBe_!uQQ`N;GWsL2D1S8he!}eCa-1rZiusXNq4*|ipK7qv6dlgo z^XdIBiqp%@R4TK+)q01UaL>$F8Fe?@qAk!pzoEoDUTk;>D_=)x;kWYi+V;PbOrgaK zTru-gc$DE1f_(XvMDS+Zlx>H4j-hE1()pFYb88Ea!zE@k&UmXb7k0+Jb3m3qd`Tb^ zhMa-o_2idT(bNIb;izhH^}?C~Rku%elppnl5L)9h>X)`F6*x`fTh(aotmL>BJcG}? zG%W`Qz6n!}LUdmJA6;b4m=;ntr-PY@T; zPO60(nrCP(JV?N!IPcK%)Wid?Nb?xABR`Rai56)3iGe0un$(=AeCRnFB3N^|$uS{~ zo1F;aDJCYrNa{Fmgkmb{=D>+UD4SAix@hM=0$q_i{U&Vr)LaBmwh1&kQ(H$tns1!@vNiiBAM?1JOI)(SKN3OuUX?Vq_Gtz;ci@3 zfOLxxP@Vgjlz|8muC*sM+bnK7@*V%+Eks{BbGPLohQygxUKArb z&8exW{T&`IC@)8BT{YYs#%fj(aTJr2lgl|PQgVp+2mPdF;VbGW=Aq$iJ3=x?PXv() zgMQ3W0w4+cm@*Roi@5(EsM&f5qJ%f9q3W}~6kr(*nd_f?ADhSV|It7qxqR7_dlx%107U zl!j->f)56R3F3zOd-!!EaWyp|7TDe0?@S-%n@45eu;%|G$SdCbN6d!|6eQI@bqPc4 zr2lvG!2jf@#GTPgnw24p(EVi`OkX1DZ1?qKf|P?1dgj%X5*m=!ZffaQNA%=LM7d5O zV_ankSm?2dZ8Qe{s28k;Dk&wSe&>2riBb37i$qPc@^$cYpKoE7DO-h+uSQNC&ZW0% z+_WTPQ1n@{daNW+QZ}k6=;zI+&%s75FtAm`8J3C+#$1)-<;R4;t_ z4EV=%P>c0}%W9)>z0fSNz@)%nOm3;M7un%dT+@w%*~ohh#f6v%VQ+Ce|M&6sJpVNR zzcc|Q<30=5_vM`FznjtvM^=&iIuf3P&#u*?;USNM)zci)zK9>r`_fz%u*VpVhve^FR?@&M7wKdWw&tZ!66~-0yz5^5Z>@4|kIdY^{aK7N=moMvqe( zza8>1-!0#fjm{kWg9EjO(4aa00}j_s1FceRfK+d*3-wL(gr!a$EN-voOwmhyiIAwB z<#>eljO@_1JsZ_y6pa$F>rE7JJ!-9x>nzycS&B(+Q>y!LlfmXs#f+ixLd?FJXqB3+ z;OxvXJUsq2675y!gW#i|7GavmHJcg47rcBP0+YqQ5Y0@1<>PFlMC3(AxvkY`*Mrb( z#d%}#HX+iA3qy=;R-F<$V9|7#J_Prmv^8zd>3P?ab(z9y!hw_aQ_NHi*-lwF;s)MF zw_7-CBqcpO^_9;Oa^1zh|QM-JYS4C$L`)XaZT_s<}8oS zAQ{)H(?8izGv-*k+L6j1WgZFnF5>PN%hT8QakNmKjd8iZHbgO)MO51bo@_%LFw7y6!W#TurUjXMvzyLr4( zN#j?K;n{=n9%&6H$1JXBuin>|Lc`oPwpN4B%b7WU%$0loNS(#$tP1~GGbh8=AD*n< zQey+se))pLLW&K={fa{da2C;Wpg7WxK74Ae00@1CgNc_rA++KI2|Wq5M!I>W&Bz%7 z9f&7(3-OcCVP%^yG2U+MS&)!TzJ8yh^gB%dbeD@3_QD{AWBEy>0_OPs^N8zL73?Fg ztMR3rtjFVtqg}7jv__88+C@kOn;dd+>9?x;wBs%J!NF;BB4J8?)~R-7=%TZ>z@`=S z8Hv(3Mq#{1My$kW=R8aZD_*3M42U{h-@HGcM2NJB}^qe|5H(zf`y|2;z{ zvrm*@H-AP1ZLWMLLAl-eTTwaIUlS-L}_|u%<(9%7_+f^T|_poNm zm&6~+(*uOoX*`Md0WU+D258BAEB9o*y|dmkrSSOFoAD)MW9`{8Z(vLu8U;|T?ECJP zODl)VveHPWfF!q#%p%!qo9&;j?3dRQ-}Z$l4M%S5Zr9ZJj$cXPw}n#R`Kh(+`*kJL zDeebH%QYFm+d2@kwIO7TS%1lE3Qw_w%Y)p<{lenmeEoEzE8;b*#8#gpDI0~wRO5Z)_Iy2S!%yiT zI7Yhu6}Y*K#pfwHP0G4|eg(hbb0^s1L-Dgc6l3EFgSfb+?2Bqn*muh_+Sj@g-`7(K zfQmB;hQZq2&mU}q&I~)}ota~cn-&vqj%O{>0;ocH?wcaRNt`||?(Ym|!A1zWJ|j1? z_U;$MgM_7qs&g0+Gu9Dj3+NOmYATm^25`G!Q#zkYdgJwzS@TnN{&H}g7ntO5A(+)| zZ4FUlf2F#A#CUB>rWUnuoS0H?AoWaK7P}Fl$*>LmGc8vC>Tf{xmPr&X!XyW%|;bfISrXiNUX z_P|!E1VcE#9?}D0Bplo>_cYe8Id-1X4$8P@q?OHT zXa4&6Q>;uW=U+L(rHgUu7dT%#|8n%$*qwx}>b>g2IVJG3!mez=GlwYQ^0yff9apLV zVh5DT5mW7f*ydow4kmJ{45jTIk0aKyx%?k4y7Jt@-3i~zo!SNcy! zubvHOG2D7(L)1MpGiv`4#7C{H;xzU2i2hA&+#x)lAR7THZHt2cp8GxqnY zS1ThmkBdPQ2fT*efn`Jx3jLE2x*^+~Aj_dVx5Ds?{pyfr;%{XzK#X*Q zy1O#M*E2FNMNhtuYnme6I}Lk7;4+^0Z=v`(+i}J>f76XzZ8w7^a-N-!ij04kiU~g{ zlKn;;DZELZmqnS z0xtY>U%oNglin7Jg3tSnm-p%hdFJeVopo8y-|6c{DrS}h0~m1f-c!1o0Zwx<@86#& zlOB&JKfAiFLAC+*XJnxZhydjt(h}3OgN?T><5D&qAsh(1+wok804^RbE}Pr2#`X1e z{MEjs`K0pX&0(>uLgov1Kzoet)r6nW&FqGr5pw10N8C5|xC)p1t<5`O zCu_xe+2G+56h?{5FzZZeJDipda#J4Z%u*7rH26<>d|;b88*+HO(ZiVz*DIYA$qryK zgrAm4bOe+bf9{}GPVokPwP`h!*qk)ctCBNSarLgUeoKYtujSLX>Pw2j&i zfrj@2Ze|I2+Ka%iQlQUL8=qGlQae#XX~#6 z#QblCE65J(%2$8fPEV&Zm0pXgnDEz} zo(VQnv(q4#cqvP7&ffGo>mBjU~S(jp(H>uZn{7GQc3jZ@Ln0(Fls7(tq_Mg0d zK{s%I?_YD(SvP^X+PdTJo(ZQV@A1Xpgpvjja+M-l!d|@@kq1$MJ z1~ZW*&M4L2>_d^)bwe-e5JXmFzh6rnF-_C*IX@Xew8oX#MZ`a$&VN0;0N8He@~oJ> z%FmMCH}{!m{4E{BlIeTdOh_YeeJJjrv>qH{39i`lBolPQQHKb!+`1)zdoh;xBauXk z^TE^82dIYx+bHNMXZ))n`IDoEOW1H4;t>yjM@$h%SMJ~VfQiwQr)y9b<=2fH%1Phz zo$N+~m+PYd-Ls4SIE*+;lP!gf_wO_TQW$#^GPI8FodlP5yC*==u&mszYJj!S^PAdl zj{BP>VV6V4J{Sdox^Y$fE4M|WjjTuL3f5W+pd;mIF8S#CtoPMTanOJ)&iuEOcP)4{ zB8{TGcqs*g&+a}X@asNftCb2K_TP{4EIE#U5iDu0t07N2xt~Tk4XC&qJZvs$=zo|N z=JW80B*jdi{_>?y;d;JH7T%T1Rb#||b7BZ%X4qwD_P$h|-%Eoj9zzQvXK)~=)tO9_ zf7DYJf&k0OFg~->s!Xbqya^PQGZ(6}!n+FkQbuJHko2{Q1+NkvmQ%%*HXUctx0th- zbt;49v?k)jDAmBgh!?_ut3_Vjwj}+TyIS?bIY#(F?Y}L+du-t$rQz6`@6Bg}kv`aT zZ{C#en;RKGDb4eM7e9+|XbS>(cuXufDM{!)Qdk3ZoHYiV(R=-j(XLFJjWK}r`csP5 zL&rK3bCrH+2D4UX!M?Pby*7@<_XgPH9B2!ZR2Tn%ddl>2^I@+Zql3Fl$i8Wz1D{;Z z8{gc6XTMy2TG5fxHK~NGGHi`=SFc*ZjStC=&9Ph|hi45qW_u1}ZoX7~#pRC8$3 z^0Rv|LX8>!=A*{&QC56RbiyM_IO^;mCiw;}=EC@k{a6x{K2OY>ZXKK7`a4^cTJxnw znz~nff`yWal)Y$a*QHRBk41y;AE6cty){(A4QI13-KuLjgo6`~}683&`Ggjsfj z#I*Q>^)dSk1wv?&3wCzeeH>ZE1@~|6JAJ^*MtG~Eicjfo?HiBnDB5_PRc)@ZtntQw znC9nZH`9BjCFhE6yxRKxEe1CyZYSa3m2*cu&$RZdH{`qND3;@j1Mct$rIGA_1n{B9 zQ?TgY4PIcSUESy>YUwpG`h3q&+k^fcc#{80r^GSJ^K_ zrT_f-u$CK1wZDwMw0O3^M!} zDVc1Wu0F$eJAmc9>QM+(@7Czf?Zq#OSD7o}+zPlU^t_@_OR|eB3Sm?;eJN!Yq}Q{G#!|BNME>KGaKJ zOVGo!Bd~ywBf(z5>F)ed)VnT{ii$36K-ZGKB{I^y@x)_vKfQNws`QLmXmqqVlHJen!bQkFv{p*F&-bxRN&!@bSL$5)lgwKye%isr zzZykNo>8#A_dN4W|Jq4^IRAEStKzh+u2m9i`D6@_wY&I>Y-gL$>q)-5N9DC~M%iR~ z<|sD)7zmm!ifr>tj1~VCjg!|PK%eS$LyLt&CcT^PITUJKdx;3|jYh0AG|`{kDxaJ! z)Z6`&@AC_2EuMFYOgM#BI&}L0|IgW#$0xtvd2`YQf1{J~8W`x~+g-M!kWpy&H~7a0 ze@Dz4)2Qnv4h-RCthG9dT(JuyCJ;?FAv}S!>MW_-;AU6>loLv%h%wO=4AsIx66X~6 zk6QO7t72mZmlGF}B*aqBHZ|t2lDWD<8LQ7S>=w6nZ7V=FkE{N*kA&mt(-QTsj`86X zO3?W4Mz_bO;YrGfSku$DSBu;iYG6%CKIZYW2qi$Cg!SPA3bJ9aZ8AsA& zUfw8=AJfhgfl6e_cYiPkr_oZJ#iPdl#@63d>m9=9QwpK&Qudn`js+&)yrMns&jx)* z10j-<5!(hg)C}OzW@FrGzinHKq1~%GOOaj4!SRkhQL=HFBC2fZTl3v}Hdop=;6#1R zspmckc+g$4Ar{3U?~5Qe?E8ium*MIRy^+jNt*OR0vcpw6r{m)OS_@uKg3YDMqU-4wV^i+50@49#@~4o(RSO& zBn*Ceymbb;7_b>e@O^f@p{}2t{7%9IjL%6NP>2RL0cRzCw500)xnp+2G(aQ}$-xVu zW#8$QPG$B?59_G&Qc97&zd}QO(AD#(JrOhF)ov%Io-4V=|QU6jhsHEx+59j}_ z$)n00ZoUJKxDydh$Q#4*@Fju&Wt6l@v@Ir3H37t$&X=Am{#1G8s;B8;eGq8LzDi|K zW&c6;w|a7lQji1RnJc$FE$<3k69pq>#5N!%yY^HXP*<9Z#_h%0TE-K8h&7nw!TJrH z+c?l^QSaiwRfPZeiHol2T%zBnLx_gHKwQ^Q899XrPb(Q2NfukBa!$o`=<|Y$)MA3A z(Y{s}57R!!lt57}f4;l8JmJ1gF87>wTScQy$>;c5pA>C7&Sf@Aax75OvkWX^PlwmGP2OSd2 z_q_YcTNHeqbazH2@b~-GDd8ddhxZYii#3K*K5^+L5ypv?@0*bQ{+dq~pZ0~pcv4P- zW-AK!xSu+^=pAVnK~odUKEgaDg_z3xE?l3g*p^C)Jf@fIoxs&2!x^QMGlvrIWvhJNuCinwVGwTB+x>2YmBGugxr%_nnF-Bo|vG zGi<)=1?5*dU&}E-!||MI3GSa_EBKRHtrO2@tA$J!yOXRZN-WbUv)D71_TyrZgqGSo zKH!KB2e*qyH8x^v*u$cG#SZ^J0g4NC^b_9Xp(ANA$Ck~hLD5?;VGy+H_it`H4dFj!(RN}43BCptr)InWWM70gI_Eg)B zSR;x$dXK&b^Rv52^ghqNm{Bb7ZZrX>*_dDBm}GPHR3nV_1+Pj|g$8AcH$?oLw0e}K zCTctPl_pAYL+f;%1-Aljp7>cg{od0&nU;cm;v;D52ZK(bHmALHCm;6xME!I~z;uP&wWRmJKyqDC|up4(vGRgFgP&%i@N z=p4}S3Q|Q`y>hD39Q(AzV?_r4n|vOrLt6sZb;sL7!sh0LL;nay>wDilAW+60~(a|NSwC za;t^1tP>nfFGrLl1cd@@R_u1rgvara_b0sf@dctqnj^LoMpzGQ%Lr%TvS8#IIFd^x!0SVH_wuD zz7m_wiYYmlq9PGdP~hM#V9GXOsi>r~@^VL?R8)>JqlA+5s?C^PfvtM1RbLv#%Pg#J zGrTdB<`~M}F7Xf(hut?q$&fuIzj1J_dyP(>?BGvPQkdKkw>@O4paE|5=LP zw^NDqYe)ND_p>q~ox)$SP4FDT+|`d@{&y@Pff4-W-F$pPn;T*%Xxn!>knlMD0s@Hm z>wZFCTgjv08U%$mBNo2ROzlSQ3k@Kk&m+8_Tu5mnSD!LqI8!I2F{gJMLLS{hh8=JS zWLSI%*#$$1jqqxwf=k)*37I^{?lZsUj#Sh{VK1zWnmb23ulcB0)hxzrht9 z==bWw0hgVBnPuyLX@D^1u9j`O>N#950r=~sPTR#hHeh2?C!jg4?Vx&;9D0d zE2%qAqAs6vm^b3G9czSsoxbQ24)D3pCO#Q`ioBdktfGjB0=Kx1IA)0W4R=FHGxd876JD9UJS*i5CEl`iNUo3q4@xug_Pqpx-^Sva%d zK^{3bg|D8!4a3dqHH(!PjTRhT>+WBnP_Sdi4u%gO-ei`9gajr`n9%5`gei3^|ClnD zHA!}QPhQ4b|GbB8T6s8dCAMtY!pM;$YxfHZ3V8L^S1Bwk1i)Z0Fn8|U25bBj3I*G@ zZ*TVbG>x6)x5l%!?OI+R+ZKgXz|IB3SR48=n;-6iQoO{Dzu(0Q&sX_yhJrVurXZ|M zVnL)Ml5sEp7_bDN4|XywOof!cgZabXrpK0z+^5{k?ISAIK+WU^ut zR5&;{@f)()u#^ip=#>p!oE9q+o012_wis-0D)uK+agxzQZtpmz>-yho#$?Tac(R)WI9#>en?12X> zzgO%ck8qkD20*)@1 zpI3v@S%(W(vR{Fds~(Uj%Q(#8lq5cQOU;z^QMffz=I?@_MyFTPIF@(~1)=G3H_LX7 z;UDGK*&%6V)UR)Y!N8n3b9n8w*8rF^XATB~p}{#f#O*0~aQiFAxxHpz;Z5u-iNsd# z{{8szx>H>Tv-HI7x_!@BmLHD=M1I+_2j!4ws6UTI8S(@>>%A^b;WL(1$H!j3&9@L& ziuu)|QKGC~-FF!(1u8w4r>d2x)J+_}(KstnafQhvVkH$9Y*>pcu`0EwBrB$~C741z z(Y2{(olb?K#g0f2T$OcKFEWyMw1nI>ndqJzh@1Ax<+W5Tz4Zk#QxfyVW1Fc_FUdv@*T#_8Aoe5~wWJ%`7>8pf0VdW@de zI~hs5$dT_V7&7}R{~SRwb8P)=R_xB?C))(3UFYTh%v?+C=}(19hu_L@($^ek?{g*C zecbRI)`33jebH-f?466bMPz?ngyVyegpYOpRpI|lXM4(A2_+qd&*R0Gc&ub@X_^u< zgTc^j+O>(Fl5UTF#-WD+6fJZ!HG|;EuqCTd8?MrKDLB*Pkrk|csLCEyD|C6*aLbDy zpa}9N@L%2pHadR8@4Krnp|AR0b?LwFIzO2|74oyYHBl(lsBfg;2x@d5EmKj2N{u`R zN`9r$&Ez)|KX-R`L{UT#geE4v$o$b!C=`gIh`YOcv*%pzA1uo>q$Qn=n&(q9GL-rA z5$qmJIpyfkQ9{YhHm6mgM^8EB1iJ@cb)KshKfPX0N=gddx^p61d2z~nW`5i60b!o}(L=;7mlasIbd}RLE^w;)hSIRD;gQIcBU2Z3TCnQP2 zjG4sbL`JxbxaxdYz0?69H8mBZ(MVCz)lDUp`Qza3?v7rsCor(Fr@kR+X=xaZ#s=@6 zllkp27!2t3dOCORe6vlFxejLiGK(L|e;}nOr~KZx9QfCo>nG1>uEUqlIGAYuyY6*f%b1Que~V%7#0~qNz0PVobK&dv_NJl1 z!N!)uJ)?>(tn=>=u{(V72|6~G*5LC)tn*23_HNlOd!j>asnypl#d#OD?6+T$&2D3S z;Mw%6j~{*5etcS~sG@)R%ugz3`MU7P$XQv9J?n-(^3Y8e76ugD+5h{R-4drB^7ffw0BGJX4atfYgVlZI3WII$*S?t|jL zU%iE$MWPRSu9Br5n|DdG1{!}Jbh(zNr!l1$XuO$)$%!|}o)FR_jhAAn81$0X|!c}fJHeSGm z^*KkYr%rqr@VGkK?yAqN73Ex7QFS6#i|pBX=2G^GO558mA}S)b!fXD!rdif;;j*38 ze{h7bhbrByrfwd8t=c5W7q&H6@fv~bA`_=v)^Tbr#e0f1H+GYJ!KFlfe@DvWaG%@S zDPQi>tL_HAd)9X4za>6i?Zm5>!I7c0vD(w|!erCeVYkJlZjFO8)9PlethL7v_Dga9 z#VYevubnww@+x6hCXThIr>A#f*1=2XpJy?q`|6{<+oi??W(n5IBZY%yM2A}L?~3#?s@}d`kySFjVbF!ULY&Vk zsG-M2W~WGP_qU8IKI1ieOP1!SPZj)^_J=ObHVwNCy2vn|a6!Qj?akgArr52~s)1!? z@=Q!j`#3^Y=sd2BJfN+kr`PRGndjT9{?wy8`;FegX*#{oxo<;;RtU++Kd!vIe6PIN z+i%}E5`3q3Bwq|xO6qD5<6_wotoXqqcXE1K;@Gh>8X9bg=YIs#PXDv-!W|+k+5LC| z#}rA&q-NXz^=jG3_i4=_%0J z88vJjbmwA-qTr||R>$dY61A!R2%HfbV|`a3t&dysoCNg4K^rMq7N`i$p{7 zE9~172iMGo(9|BC*`u?6r=y)yNczMTNxhvbo_B3d^LXMTq$GYd=Z)u|dG;$mUn~C0 z+Cb3sf+S(%wzLX`$-w7r|x$&R@$eU^R@U=n#Kh#*pSx~Y#o(b9wTs^T^M<4Jf~I|k!x!wLJ11|Jm!Du_X0{@bu|Mr8 zWol`f-9ydkZ^(9{`UTHA|E;iio(6%Y>jtC-Hh#KSwU9I8J~BdwnN)<{sj>=k9WdMR zBiO{%cEq0Vb9S+dZBV@C?Zh(4p4GayRz{p{f8J=@7D)2re$Ur#y|kowIW z9K!T60`17}dz@B$Bav$QoF}KIcCIfwSo>UGl(BOM2mja-d^IM;s8v6UWR<740;}FV zCjHnvJ2&@Ko?fM^!N)#{+S=NsLxV0c*CfyG+Wf3(@UyMdV!K)?TUGihb-^uuwINDL zCrS;mLTI(Yl_q!z-s-V|?Hlf>2yRhKVstOGW?c?E;5DmOIv4U{U!ZqPxE32=xdM%6 zx|~Hgc89CXTpxBT#1;>^)h&y|~C~zQ<>ixp;VbzNTGzD(!SjDXAPqOD2KyIP4ZQb7L@dD9T!Rf>* zrjy+YIa6|llGk;L2)D^uGo`&ATM=-Xd_1o@l0e4zQ zWxn05@?COra`(f+u6Mm#J3cFmnodsMW8YSQ3I8lLHE3|q%FD|OW0XvAnD}3;h3 z$#JCZ*e~F4Lo59>YA(*b!x;4t9K4>T;;O4_B%Uaq)tBURjHif}Usc6K$}}*wvkSG( z!#>}|5#lu7dqzD@=CqL!f3zx70m<-%zNl%8(BZ>}SvGA7R~1_Hqqf>UHj_9*An!q^ z-f&09(6FCGTsnefLN5?CD*LpsAiKiU)U;%#I|M7kQqfrz%EKnt!04tO-n8t(!ZW$8 zy`03EwfsJ#D_2-H#P>~+jOhF#f1Ta)I4UYSHg=-IIm^7JHP`Z(_q52E$J1+)UcG#i z{e5yQcPmXMCnv=vByO4O;#pkC&>D0rW^BU3!Wz8o_3PLDKJc*y0ZN$q_&mjJ3+>&z zo+$u0=WOzp0|ySg%F4QwMZ%oHr3y!G$hYf}X5|#s@5H{wr9P?=bj&&(X0H3Gp<%vx zz}wp!wR~4TfsH~`{s$ZM8q3w*qwRIF-C*aMXcgOg_)yijvHZfYngw-U zKZpDFD&6|f0VnalrM|1Om>p9H-_3dS?<0Du+b(Jjam0GND$Q?s%6m39yWrzgAr+gW ziA16+u4ZcK=;$A5FGitaGhw4*ilBR5rzavNmP;fy*kqOVYu9;&Xs2nm zw6u&K2s|Fgnqq~?wXi~tmvug}4>RPd_>Z7c+_ZkNTc}s*h_=`2ziQ2wXU4{y0)v9G zMMBU9%F95Z+uVx{u%(-=BY~8#dAl52h*jaS zVqBA?FXH!i+wcY}u44*V^5HX&PdxvDt-Q2hY}8Y&Y4X79fq{YOgoIQy$e0v`D*QIK z&moooRMg!3d{`+bTM*^upPemYAJkJ%WoRX}+s{w1tXWl%S-Z3(Pq1%)=ftb=h#}>tdkbY3HQLLmhs0FK3>`u!;Vb9IYg@0VaFdrY67RkQLZq~N!#o@xNh)j5D!V-czXIEI`T347rN)vyYwgJ z;&NItPHO)QE?&bj*5mxn9&hhk!V0bz>6j;G&B^bTXEgTj~i&EbUti1bSMi8iw}Ly zOWwBRl%_!tl+~+OKb80W?y%h?MhMUOfOe+%@#D33HgbB5RdVj&<4b9`&n+<>*Xt zM0Oym$;73c-?urWa$QgbKwu7TJ2k=YEGDx!3sxVm{Jv*wUeD%!Xy};OQYGC6rsVtM6nU;QVD!tjn#wH{q zTImLy@SI9nX>d4U0arK;N6{#=+CQab6VQkdfVM8 z=5B6}ZKclb+I-c+VdYzBGY!*hTTIAMz{CLl3$3g*CF-`L$t@x>m;K;^d0)e2br}z9~ z+GhaZ&dy!Cckjjyh^b4VB+TL-@%tjR0(@bIrKLY$m|_b1jh2>{s--p=+uGhgn|v}& z^LgX7PH9#krJpn!|N64hvC{(VUf$lArFbDCw~v|G?Zri3bd?bv0)tLo2n-Aa7)in& zVGUZjFY>Idt?hYqbwvRz=AlD}y0l|bFYbT*_%YgsrttRs{QMulvT^FymlizH&9Ozl zQeEYj7p~(*&}`T;n3iP15O4}H*c;)3&qBrp7-)DpKWX7#nq!%3xwZYUUDw{lyQZCjesfAH0T17u~d4L zaaC5#t5%{hEuNj!5ec-40d!+!WnBnjtjUXu`xR!SSSqsKi*baYx`|otlH7fm@ECHl zzAb3(cqXr2y_#W@EO9&5p>c?Xl{JSzPzHL7sF}H0_z(B{P~pYVMUR-Cm6w_keQ|1= zgpjs;S=Bsoc_$AK50>{*dNaWLyO+a<X%(Nu-mt@Rqy zhYmj-7(^OC#KrBz?geB9W5)uV5!{073$HO-INJ%r^7if9 ztp>Cc+@VZZVvO9BloT#LKB0)3t6WUwS0ARO?MIuB5;L-}v}CT$xvP{kNvC7};?9TKKK#i3c)MZtpN!JG}yXsO$(Vk zK|A&@{|o9W8gVb~Cc7576x+RwT#XCO7TE|zhd{=jo0wRpE%$wmzJ6b=esqkP*we%2 z-|+6;suZj7k>ktnF^Ll$|GLu}YwKVaK^-(kO9?*OM5a(I?d@5~`TMtRK#$rIyarFq zOri$!2BQx{{a$7FRs*WBhg@z&mw2>lRaWmE??iKf@KLe>Xi8G!u811k8gA)H%9&C_ zOU$LZIyK@>C2%D4VzLwp#MZ%q0bS4|3ia!0$-{Yy=z|lh&y!rOB)Nb7{CPf26U73J zW_Qo@yYCggrKRHe&DOISm$9R0W%Hs(9Xe#kmnthygPuXN0(Bserw;rKsT4@Dl8})( zTd1D`?vvh((gUN)G$>X|Zx)DFq~u~+o4bE~UEOTy*VsCt zV8wU-2P0}2Dw}9j5`jQ6A_3k)(QA5=^(%V!Xa$8Ymy%w{3dKh+M<&{7l3l~ zGK9uF7H-NtBHklB#5`+l)wLxHMb)wgE55elA?E`EEiL(>w=m=vAX}880 zwWnSqXlSTpspFR(JLeq`8<_(9*nY&v^Wl+Wl0(bGoXxjmG|k~*(4!eU{gQ9nq0Qip ziOpwzapChBT0rSN5gPIga$%i!k6PsS_uy2`lF5@HJE0vcOf+io@Y=r(c_L7zMt`>B z9ZgGJCtkjun3V;LRQSd-V42`s)ts(f=+whhXVH|UF$=@w{tWAp2JmJ8>*Dq*NuV26#;@KmI#vqQ0w{{s2OVi{_>WaL zS1>qoz3=c&TXI#-tKD)>i7k)2r41u#)=eOPBvVS^BVO3~}WMe@NxRJhgz@0W3*b=s$}2sva+k z!kS30CFJKn0nclgEhupl#Rk|t~I zpDGGKc%bvUfZFp8ULN-4GdxmCuO+uNinI8iss?*v&ge)9@2Fr%{V2kz*{&ng-*!s7 z#e=W=B@FfS4~Etp&vMt`Wlh-{yap6OM?@*{d?%EQZ6TGr6FgI!%Jc1r*305el5icC z0waygy}e^K6c+zF7bWTh!2;t96QLlJ>JUG>Z?PuKG^Rh?`^EWvj@|FqI~P7R${WeJ z(<8k!@cgC2KV{>oY2-e-_#*MB&*{K}M;feK2%D?4QmvO@oq9gs+03+e*#9 zy*kQ(45%b1Co4z0L&Tg1za-D!I!-JG2&njm@DR9kpE-U1ydNndo{ z*d?j)ZH~yj%IB0-FMbhID&f#??R7b^9IPL-5@OMB{HrLyu20$%Q3Dl(q3$2GZ`;Kk z3iWm|RF>a0zWnOvtfyI8>8~;}*w?KA;$5=%tHa`N#?*j!lAj;gBdz@MOM0^~W}v^y zSOKb(3sbg8t-lWD&LNfx zhsKFlZe82|SAOBNe9LAE&r)Ic{Fyl)f2+~qvAMO?3>q_eD$fiu>co|&vhWS)Qq1}~ zIzTFnN``5(VBICg&BfI@IoPCM>J>RV>uKmWYmV|i<~c41o|~?nmGkD!d(3Sm!7YFA zvSJ8v@$w2n-yIm!ivxXRR4#Oa`e8C*<-Tnj7G|myEMO8qNF~1X#H2}BnoLC>AspSo z&E4Hy8G>iUXB7qVfFA}7#_R(v!~}z03W*d9izp!-rw&)e`-MO$_5&KyERRxVX69Vv z7sFBNT58xks_){Lb7JAOeNPe+2FCkp!N8V<&Iv`}RTwJyI&W$oJPp+MqOeWY9tiV^ zSy^FGhF-y-A{W#C@^=7JaGaa8f|Oq1Hq0Gp3@x~hUPhM#OXL(Y?44*(@c-fIwV0Wj zl7bRq7(V)CK07MJ8M9qsX)FwI7q@W{+_Sm|G67t&gc;xOt8cFO?S&~REAv}zB3=Un z;9nXI0#X>QHClS4Dcnc*rn$(xXd2v|VHU55h8%jd<0?pgVc{``Y1H9MH{h*4Hh#Vj zjT)WmKj(H_rR>snilMW;E=Qj zhg&f{uYCr_2Xh;BHm=TM@oh``Gy7k*3^6i<6!v|iB^zNI5F-|x@xWo2w03k6+ajwZL z1y7G%XCk?x$ zu%|&3{7Yh#m*5s-7ni^AZs-jMzLj?LbTGZ>Q2GnfaNX?0z~W=kL6^CkbO$&B@a@2a zAJx3@r+AiDlxAh1dx=I>NUBLhvjF=6Zkc4aga=8)Z{4dt6YJnr?uk1=*CaBRx*ZG( zaL5{xgS(P2@cHw@<;_0|=1=~k>@11z2t2WG+xSF_<%Ox?VeJ9eV}Qot*DZPm6qXCA zb}YK0QLQa4&tPAOi?3LiSi|a|GV|=CW|l+l(Iu!{mHpF1p>T!7;sTqy?6Jf@&vAcXe#kE4A_d zKAcZ%O`_Kx52L?EOR^=GIq-GRV6cOrvIZ^MxXJ9)CrzDN78|ek^-vFT=&hi-cpUyv zLG6v}8I^PNU=6DNVfT12BZ=S^R57L;or>v{D=)i*r?+pL%(y&_%c0F^YSxp=yv+Mf z;YTWWnTJ0w-L-3xKv3%NE)g55Fdfx-e40R(a~AVr_2=k_@f*vX=y#vdgJC|f^&iYJ z_FZ{k!m~a>!T-gcQ4+f!e|-T2f9*_t2D~X&fpUf#G|8Tc99X{!V;1b@?`CUduOrGy z7tn7YTf_cHzBqRD-feH=7utnh1r*RSoyUF5_VBKFws~nV!61aRfGh*E`nTB671F#~ zeoo~N<|k5}>}XHK(oZTp;9SG6T|heC4&u5>K>k1#eJgXHxa(k5I z?=kAT08BPuTs8i8LVHqMNW56_-h@dRqdcph6|V-9^w0gb13XVhgHS@7v?Wg$bNDdU z`S1`F7~-{=3nA$ND=^(b7QrUcH19zl!~LS`fGh&;1L~vM%p8;fUud?5YlC*6{L=qQ z21n%C%B(kFS5Uw(vF}#*K)Zl?AE&OMIePFP>rFtQAzu5cwJry02|GuY&%bw7k1||E z@O8$7+_`}-lF~!Yw}V<=X%dc0#G7L#!w|M3KS>#+yMRt$nMG>fiL+n_QRH`XgdpuO z<<;&m-!&x?=*@Fk@9}AIuJ`%FsRg|Mz3sT9r@;Z-QNCSnZf=0NZcF|!VU1r5-X@cJ zF+&TH7FaM91l02z1ukllLGd5V zUs$^mqZycW^k^tAH#bCux$?=bP3udSM_gpykcW5kY?FC#N}9I{eg)44uh>3Mo)ORH zzBJ7ttBijc|J}PjYS?U$8)!3K=A}l}uB@Ee3ie`SyV?%$(=eEq#i8Y8oZ+hq5GL^F zu^9Zb6%-ZAMsq&?f>tm70XV29s2Lp72^+V_Jvw*#i}pk@$&*BK@=r;$&%!d#Mq%=i+B z7(@F86vx!Z6t0h#+xTaOR!;_@!^G4yfPbNr88c0Ows$M!m;F(-(hnxyj&0wt7I?SN zV@v?V(x{gE?%fYP7k)PkBMfYaZUE0caA4#nCMFo>(XRvsz%h_iq5gxWR1H&I4$$!u zu^BgBW|AvponHRsF@A<~&}H1ZH$6TfHE-C8Ndct*}~Pe+26m^FclV3j1Vy4 zHOZl%J_cO|Dq>iswoc=PAEl-1{4*Sl~omvoQu)Eps2`KjidsN zxGWC=5`#oO!P46L-f#DAd6aGWEi~g(r%nNJh#C|rO7l)HcVsnNGY~LnhiX%Aa+G0r zZ%qXB5-gE6ac(>8a)t~CYdKTD@Es&%Mv8zuyz28cNf>AlKlg28M2Oa{SplKF8vp^G z#Qqr$_Swh(td|w!Yj=uo6u=(ixlc|?0&5CJ)G!RMHLGxq$rls#GRDWM`6JFIOM8y@ zhEM!bSf6PMXG2wlWeXkz_SxUx|K#b@$%^sl=p9RQS<5kc8Bkl9pXp^d&yJge^#Sl= znI_(C6x$qDz4C*Z1fV382k;*-E<>-uJxf0umhK~jHQ&EIR4F<>p5eM}xKq}Dc^i9p z);UB^^hI;Axd5Pd?AVcr5Y)Kmy-I=Qe#!z*htGr>)cj29Do4CorV&wHAe=E{4_nrM zCXtxi<_8m@DPqqg3_Zmrtzj$yJA1fkDo71 z*JIfEbw??Jv30B9JLKD4$B&}TZwVHck@?8uP~v2?js~s6uhJvE`8mMkup7pTnZ#hw zkbuDWFoePd<^)EtgW)x&@Sm26vw%Wgmayd|i3$THqR!%&>(Dlbk})RuuuIgSE3Uz4 zyFs^p$sfnGuxOgXhhTU^-1MAk)}D1=9-+XKVn}0Pq01L&fQUj74DUuqO01Pr;VW?BNcnKJpDPdGFbg7rHr#@C~0#3^XW=dzR4Tj27aZX67GQ z#n5+NpYg_c^6}pHzo(O@cMy{tdL%ZSXjIQHcpE+MWx^bAf=HrAK`Dj5a4AibYv)dL z0CK3zj3PkP1(FvYCcyuxzzU>Imp{$Z#I#y>%J+{Z$f@oz%+u?GnGR77L9_Gw81FXA ziv0+l5C~U&ywx&+A>U#{XR{ge_ZHB01QD}%>7@u;l*MiHVa&G=*-#T;D?wI(mg2sJ-LR*E9#OYxu)W7k~G*>KwaR$lG>g_;Xu=Zxh$QpccWULaS%A z0ZrkZo-{^E#eYe*{OV@p3ANH0ygobogoA^aW&KkIjez?u6gYj(; zyqCvdaunXIgx=w8CJ}AAyu`^KuE6qpT=(i2di=DTIL!EwYj1x)M(kB3=$2 zAi6ht3{+NWUIYsm1&y(Ry1^fhW9byeTEM*_C+Au=jL%WCAd$IK4&i>#A>r3c32kNf zWJRI?aj62YDPi0XBjbT-{jz!JsGKC0nCYKNSQ~HxqYJzm4)s4H2r9^*yZ+49>D{X^ zyB?eYA}>-Rn48a=1`)*uAt|SS|IUs42;^+EX`pu$1?E9SjpO2+>#CbKD296wsdU>l zd}kWCnK%(CpKPtM2#62H}`WLkvWAk2pgwqr7ur~-A&lr0G`Yb+&*x`tE5z{~>?_7aWHATK4ZOblVFJJ%i7{vAN7{y-cP^&A z$4FH_MWG0rD)&9gDX|Sf(uV4QO0~5Edusoq) zX>0pa#mJ823W<0iiny=`p)e5`}sU%&eL z=F#bNJbXsZ1nuvzq~vWoazW<2&agU+j2w}Xj?&uWw33Rrw=0M|N6l_@lnxE0E9O?~N0GQCFxczsdWclA*&yqx=f*^u71IR!~VzxIz0~8yP zQ2(u;PA0w$-nw@tmN?79Xf)7Q&~W?u%mF5V2`5kWiv_l(SUFGs)CD{tF7%0^Qz8Gt{7Vlb_G(;A(`0}}D2!;R=*Ilsby(6^ zXrzqvAO;iONTl`)Q-4-yUt=O zIUkT~<*scT4xyhCm+8y#=z4f*Wcz3v*Q{b;W1B#OLogG+4=kFJnz~3kg~pJu(7Ts0 zR)NC+`oh|;FwiSdsVrYAq%s$oMOz^;0RfHo@W{P-_cLgr(Z}?M$4k~H6IQ;U#~5>g zz7v!Wsco$gN16cq+d*=$)UEAE$#w6`y@sR1cPPKiQ|OA=iU^Eu+pu$ewE<~r-itsU zo|huaeiCPa_DVc6&{eo{$p$x7H#TiGS~dWQSD_%^E>iRo+wlyF(NrU1q$a-5>02_0dJY+4P?TikH-H+R<%gZ!GxK-T z{{5ZsfqTEP*zf=S|KK04Ar7wml>(4mwpG2py{T;`h4~jh9}jhImws^gCN+iljrzZP zLWFAFoelxsrKzKGn;Ewh?x0Dy@;+yYzo?zF?7tt2*%26EWzW9ur+?F_Uw1xRwL_`L zh8XDe3qJi$9elWcdiy_zfGEc8qCbDS?2G(vYnKT_3c|Qrn%fMS?UG z(wrtN0{EB2Cv*~Qs@<|hqNoWZ5J2IqvY75XQdviQ3ASsN7@}R=hffwg2d^|y%y8?YP}Z?pCGUuG*f7+{|?ddmtR7_?|e>~TRd{s_&-){XDQg)mOre6WEM z#>fdW96x)4=S1C6&#pV1;%Zx)N;q$|mHEa31BO8r{Ma{s${<%qL;~CweLRyo-l)uv zXU(R!s~_wceRe04=h1*U6LKh+(1ZHK4w;hb3V4;q`?w?fseUS20v z)vFv0RD>X(3u%#)TYrqtQ?zIL#A+9){_V9q4rUKZj$$8h-}`KR?8U{EO3FzZre4Zx z0+?of?hX!0;~*M^UucVG$?LhVY0`M=gM(aYnm7LJ+|M?Bu0T|pRYXM6bAH&j$RH}p zNd1t+mc=g(cBV;n0|})4QFhjOqMTwvg)aMNRjYVdxZj%y4K(bX)n5K+;WC*X;LILA z5dLd3ge&X<`9$mwnj|BM(sh(LyNRLwkBlH>3bg=Zq9$T5coT97uneIEcM*t-8UFG1 z1nRPcT7-(=E#tP05=c41@rA!WQ;u*Ua+)ys6y{qSkTXL(7NMZ`coq0ONG*b*Ac(`m z!O5wst2;)a*b|(1nSq{u9oJiq0pRaKtH(=ywkOQemKTW(*IIDPScA6?I(Jmy=Ypgq$GfQ?s|Pfgzs&>0ak-XH0r`3 zB!jA|=k&Fd@W0O_Zhj~EM*`ag5&1>p#?>nWY^6T5==|l;CK$)MWrIS;W7A8Q4xRiX zps{gZjTu(G^|H77&ZsHZ5L^&qVnNrw3UX|0qZckx9P(@(+<{1LG+z!mTPYOHBAeTJ zd~*T4HC}c{wfI%X^Egws(Dhrk7-z0>;zgu#rnSLODNWM^gfhy=FB%MRc?xN1Y}z>W z2=!EWJ_8H06D70#v5X^NV20<;orC*%Vc)iy#xOrc8DCm;0s>^ginlPKW=O-3Pk+hr zI{Nx%I4cL>45SG}hior=y%os$bY(5iWr2BqwLY=}Nss@Xxd3Q~xbW+Iz)UHBdwriX zhBitK4m?BLfRp7nnZyra$&n)S-sDjI(_^QMj1o>du+OR=R#OpNy(WOZs{hZoySR^J z+tF{*LKRh=XEz5c-nh9k$ZTk^dd;$eH+_+}I&Ofz+-mG4!E`u4a+kyj@f&BJ@5?Co zqLyioZ*ILTN}C-Tv8LB+e|Ov)CvfJ>f&MkOcCiOb zvq#9x1a+~j57&J5w_+*ZWsyi}x%LG6XP-OMDHDF=$Bjdk!`J{)4@^6y!#H%<*n-Za z+Fjfqd+pP^r7ZytWC@jI+mw{&A|hW4eLQ2A{BUrDnXtKLz@R=MYx9O3yvH70C1r}y z3{9h%9;`MsW&dIo+acdRpx8@R{&hC+1K}=>kx|7%^q{x91G6wJ+WwYoQ(kGuXO}LQ z=H#4AzF6+Wi;5`JXRQz)_2s{I=QAVJ&G;M44(Kvrp(erQ-SuaeBHO>QmTE&F+_nMe zLR*^?$X>tD`HqWB`}#8I5yuan`+j){u>r~0y80N_oigvk{3dq;s||R>4>&|P&Y_U@hJmF-n0R>X6Dwq=T2reu12Y9O_?T-O~;oZSqz8%CnNZ zJq(wnH+4u&TrI$&iI{7Uo_r23CvCE2eJq!f&9d6jXL?inIrN3MvynF8XB=DJ=jb&L z1+K)~=F|#A=Cl?cRmvN>J~bq@b?XJ$tf|qidgh0tzAc$vuQHAlI3?A;S9}t{j7+xi zts~FS<|#c=mgZzj$MoN% zE5yAzTGV+-j>BgJ?F7PFzAGvAPMrUxj*K7d_T=kxF1&Q69c<`&gIqOC-f)%lEE^ah zPQ3agP!lAq^V{e0vUi#VolI1}X3G$;K=u+p*N1uY&=*1qkt9MY9pVV%xM7_7VOYq? z7rW3xeo~13)$^~yB<=I;ysHn9Gt}iro(vDm7u5yi*9}a7gT4NBx)^~737oG?%gVU> zVc@dEn^i4F%U&Tyi`H>9HT&l2)zQ(d7FKB|N=G8p$-FXub$$q6qx?W<-EpQ&&e!5b z10N&pGWA=9qThgVjTl>{)q`3H(@HMAbM4|^b_$`A(pU64v-(de%S}$*(Ya!SMU8B2 z6{alv#Q?OZZGKzj6+tp2cQ4Yg$>~5yG?LX}d(Q|Sd>$UOmogRD z&=xAJouyUiY!1NxEt^D&Etycixsg-&($lIn?7LsK^r6UBpAD~~w|RF3tW@tcS} z*x{M_mVe z<0lDO7aV3Xf>ydw<1ePqi->;J)qiy`_2_L{#W#zHJ9lIpN-m(DHt_JM zsW$Bje=Y4LS-w&11z*eYHrLg{+ryetLrSK!Zua%bT)84zTKdQI&)~5ne)A{ad3JLg z9xJ(VwzVgUs6WuGeRMW!*_Bga?gDoKk4^*SrPGjRIF)q8C{{0HyPu`RA8xb<<4p+` zKwpjn(RAPMiS)em%j)qW&V3)HHf}t!;`xP;ilCmK6#jv<)GZ0p_b(~-hWtKS+I!{2 z&WnT~9jRC~oEo@Wskgcs79uc#hwReZvKIPC>f%7syzdieqcKss$s|% zzcCc!cpBq$99)rKd_K-`-jKdzFyF#2*I#Ng_l9bH0Eh0NOd;P3ji|Wz3L33XnylC} zt%>NkjPWWSvZgSffq_&!^Oy(K_-a}&Zjrs>MsV;tmsa+K51G0KI2@U-zwUGzZgtsTO z7bnH#A3W;t4IulQ41FZD%kzBt?5oUtrZZ(zTET;Z`+9oVPbPkw<=h||65`t7mHubm zP!oy&T|NtU-Hmfm`JT&*TS%Fg_ixjGXS}-lkd>s5g(VN#(;&_L$n*+cc-_h1K~5wXZF>v->a0LA&h zR6P6mJuB|7)QrW_MYqU=#aB-53S_=>A)D%-?!P|g5`4CD_cjR``q?JijLHeU1PPBP zXDbK3RBo_rc+4orj0~a1Yt;E#@v9aoXS07aHk;#ZGTe^-aoI2AL)5?1(_9yzqo}xe zaAXA5R3zhM7O%Vy`Y{{QsEo`$cc|aX*E6}uGwBz(05X=t+QA7L023Ujtb~#ScnTN~ z2iYe(>T;X7mT5cP9gGsJu5b z6Fezwo@=S`m)Vypam0+qOJ*%@o{JKGl_~YX?58sKy+WIPs#39C$x&Y7dytxY%vo?OZ91W$Sif{=yq)&lhvwam@q>%`o~Ko?SWM}&l3K0qbM&+z z%T!SVap4?Qzp8ro!f4Qrc<;=O6*tfZvdW5Tpk_t)R2w@>Z1_{s#DsqA=lAVG5!ruh z$}*nN*xh9cl)pgZ}W{At~0oMnFv3Q7U{TOytaaE8V zX>D@^fs(~Ju>-IDMhZo7S}aF!OU4p4JslkrJ5sf21s^Zc%u=(}{%xIeQ^GYb>fFmB z@EHc@@?L%?#4Jo1Wii@RM~4>MY+C7|rw^yz`UcwFkDJKQ z0g>XUt|i1N_y7XoyJG9GSKi`Kg@J*A^n>e0Yo}BDibTN=^YV@`;<0d>8sg=UDccfO zjZ@N~E{M~?EH$&WebuDbcVV`Svg7S+EM*}O=0F<(N`6mgc>oRrf%D+7I&y7@LZy+Y zIP>D0dF1Y0*_T>u{Wm%~HZ!xn-@y|iF3n|8Y@M6)W!IKNX{0MFo!kkFpF7FxV<#tX zs|X!rSzS~-Jg=v2+gi1HUiwPP2}z4XQXGk`EL?o7TSExbx!LT1H^)44KIGY4KD&pu zw9{up`0Epbw(j+(sd#p;*n7+-HPcb51x=|Ww&l%`R}097?FPKXYOA7bA_-YT%o zdaAz6Gq-B!W$lo4gjdG)90z+zT|@ix!k#Rf?i|A`8EMbRH>!D2d1vGHqT(xyWn|T@ z@6>JX{^&K^Mq!io&j0lJ{1Z9v=QDok67yA|+;?u(c2%qo464x4I^bC5lyp{}W)hRZ zytS#xDNy;Hhy9=0QI*ZdqbR0|%zm6NZEKLs`)O`(Z<|s#;|WQK4$2z46=Y^WDcsQYad-e9QU+EhFZB6a8xN50~EZQQ*EqtXSefxDw745L%&r=Ha&! z#=hmyb*}|Ba@R}C^VwbAtE{TJwfj(ri_H7CHv)Q|w3W3#BU1h#Pt5om?qCw!$`RqM z7g?Y0dURJp%)Lf3Pxre!8!YQ>b4wSmmD81vaIeIy@LQxFw6IKnDjkkMyQosN06+Ou zNk)3Dh3zK`AK8ziDg}poojf4Q6Dy|O{>*OS?k^}E&V8e4X_Fpoz9Yl%tHxf(lNsev zW4B%RZexw8Y4i7OvWmrw*DrcgH`ui2!OnB}^@)R*M7-kS`xwyf$z2mN&YESoiL6LU zGEPY~RUHX7p`;ar?G+K)Yx=WwYlaWSfivX(J}pBlI+) zp}A>Aa;uMr_m&$b+0FKQBC==4!Vdpz=h+=mc>F`oKs`Oe_|Lq43a1ajJ4sCJsE+=t zCfqz4aZs4x-8N5^tGZ0gY*m0Ym6c3~4_}V=&8Kc91_}s-Mf_GN9Kzi^cH&?OWZ<_q zS7U%M0+wxUFMiq9=>5iU=olY`f4!=`TQ4#k?Oun7}$l2@c;Sl>-c$o#* zBRpPRHg8`t)phZ5BU$^U(x*5RHK(5xU+DRk!A)>D3b^msenTm#IcUDEa6_{-by+U! z@}|Xox1Jr@JU*Ewkg5U8Q4Y<*RyHU!_uhkGt#zhOlnLkZ`-b(S2r)F z5x#X?TeEEgDC6d^ScnFt<~ojFnLE)?viR(X z+2I8DG_BPZmehrIN(N<6j#5;g$h1UB6t$>62NG4R=mL zymq5?)S+%~YI#4zvx1=HKTq(l4dVCvfK0(nb6s8qzlFohloOxlnC)SA!w17D#>VF6 z)9&s^SKt5$%tADGOa^$9U_h`}D0CW+=BzFPAClbWNJ=Zr=PFR2_9zq3_%{@v?zO3w z{A1hmrWDfwn~cqzdU9`xe~V`~vb3~a8(*5=)8R{a`$;u4<6xLl&&ZgTmd}=8tiFuHm&1nb z*lKTG`}%WD58XOXJ)57a$URvE#WFEXUi927YZ$q%{HEgqw*R2{Y`S$tC z_qekonWC{Oleu~P>sS4)wWy9y2cV77{fVa~izf8-Ic_4^lf^W z41tuVLLB~33fd;zj}PVU^FAP@(4RDgr#_QTE5@4*Z)j_HH?^wY{;RZq*(Z!0S+=D~ zja;)_d=!tC|CnCg04GtW?|WYVfML(4JYjyH^CtG0N+K3xKXVOy296u>AAdGBu^|Yb zyR(tBXI|mpP z>&X!-x{8nAS3+QTb43NKfdPjYv8^s%E_>(v)S}jukH7z2C_V8Vo6m(zRXf)QHoo*@ z3%as>@NkuZDYpc#3h}#@s$SkZ zC3M@bpYE+q{^cR}UJ+ckO12@%FzZr=7JOI;p^U8S9b+z}J|v>?VOc%%@|jfmi=#W& ztVBG3;fWR$sK_UP;oy8@ee$^|nZ*17Pmg%gj$6kNbwLgwO>@J<1%;z*ao5)eRUkJV zWZ(4dnj}tS%#>+tUW3%EvONvPjq6pej2qu1LytF?tqireVYh>yEc1S%b9$JGl!c>y zubP_8ow^n8mCl>IH{}2*v!~d@$M?)Mu#tcBZYBa6OY_Ty%8>0H9=kP?*a0c}M!VK{ z(<^04vxw`mk1u{XSPO-v^}93vI@7je&G&0-Z0!m`IJkIAmJPv+Pv)1uy-{&L;@=7LK2dt@huxQ=hqA6rg1b| zWOc2C%t1B2Ah-0V*S@|WkN~OYrntlJp3dLB##joo9Vn*6GOny86S(%s`a#wc4Z|xb_4Sp|He5d;|l4Ce!j?W9RK*)q4#U>^u*1!I=YaL(#N`#t4 zRLAET;Iq4ae$NXT8yjnA4}4EwRR4SFLZI>07>m^8v)MX~kkzl#o4+mlnZgHR0^^r+ zL;@V&mozaYHOD`{IDJ>KPJ@@D)i!@FD5xUi;bA_)x}Ym^uQ@q+N|ybCBO|K@^o;(X zIrk_!kV)19C`z`|Jha#!%!Pierq1w>$O*1^U6bcj#ya4 zyf~ zekXv%`<{#qKcRm4mY$Kd&&!-&{jv_RcrvsAOGr384!vWGH^RfkvpuLWwsulhlB+JJ zY56}2lW!4+_KV=yDd63G9CGRjq9$CUUz^T6Z`9J5zSx*_(JJqh6K_C#$LCb;i^2lq z1;1UmS&sF$Rv-d^Ph&t|UvuIuQXWW|AYXtF*1*R>P)S)$*CZLAisj=|a(Kmf?x<`S zLP(6y_yHTh^I^m3ibOLHbGq-{D2RzwQ+PZ?CS_>5)L#HP7Jf>Rc2-f zfsq`Es>P{`Q~0C~1pV=e8Hh3h3z|u!p^(cOZEJUJu_BPfbq(xBt9aHz6$N_BG+@n( z@i%&H#pNRG)#DUmahSnlnyu5fz)G~^;H3Ul%eIesiRBw9)@Q~z8mrz`GI%k~Lo;2T zlxcT;!eZ04%X?EVN)5hIqnq0LZeit7G?nt6#!+n>_->uUgb+-2Jko|@DAC7BRX!@4 zDWrkX$)@!X9>kARyXQZXEXAdNh(Fxeb9;oTG|fJCY;4``-RzH}c<>tx^6=$i+_g7( z25G$)_7$kAD(={^ervG&(zAkMa(}etqlG29!NLw&Ig`^chwB1whre(tzxlD-DH~>!hf5`h1 zcdFO--R4nLXpo^nrc}z1GHsP6q(L%85)qQfJd~7DQA!y?8mNTK^Aggk49k#to|9pj z7nb4NFZ=uZo!>eCz`3rouWRqCwqkug@B2RQ{XEZo-w(wFiIiMI<0vwDNTByMIzIzz zrPM^?{^6}Y+-WcKjR^m(uC8vO&I=nmyCF}vF;Cf<$srUa&xrUDN-XXmC;{mW4--QEArAUJ5;(n-Fmn{ujo z_sosEng#s4$v=z_U5*dqnTq7gtI@tw7NLyzCCss4x|uIY+9A%vw4paqPHJ5s9_C4?@{P|3Y6czF3rH=7ymls5Dx;|{;G3~lE-|_lb@Xs35 zM>qS`ysq}3$UlpX70b+I9Y{%i`(CAL&+#h4uh@B z`;o{o=`W>S(!NE;T)SBl#Q!Yn`Xq4m$KQ7r&ffk1;^#GG65wY-m;;IffD!2>MQI?z z1tbG12HzL4E8enE9&Awh%Fx>KQcqSMIGPc4qy$%eEVOgrO5k)t>Ogu*KQHrccmvf3 zR=r=AEk6#LMS8*^)0LjNJ+;X`kWbA@jTTX@`ed?f4iti3Ms!}FDLej2gb^QTXW3M?O1#kP>hxeUDu+5PH;LL6Ii$mBqPcPSK!)hP_OXEJkkwtguNYunk@c->YJX!_Iy&~r6qszcXB#(d2f`+-`Ln? zFUlv=<9%(tQ0YmA`<DdhLV}T3rRl*pxw-!1qvYO-Q$G!< zzQ3=^v$NY>x!}R`tU*TK$5{NLE9HBg?qlP4PE7=_++2gSbcLv5&YDPz#$TAQ zeKcoej>^V}7wlQ)ezMqHYuyW@H7o8On98KD3aj0Dw&sqoY8gK_EB(*nhHLv;(ym=8 zerHk3*0a8WH?4~(aO1_F$JM)S(w<6#)kH|A!G~ zW!fF>%n|EPa+Adp*&_>NV9$PnlH4f633)rcmDjo11%FSc zu6!($NvSPQ1CVr15=E1WI;7outcGrtSrJ=PTa$F#Vq@?~>a|-&i4~oW^B*7SPPxrj zw{wW%X;ijOx=Pja=@g zt64V%>vfOZQ_JjN_pa+@bEBuG^cB%#oM?GTkBb=D+iZ5TKNWn^dGl=4v|CEwkp3Sl z^#LMu8r)gR`k;2paA!45aB(hDRp=?IjN24zlPWnL9Og|?mC>DfHJ(Ci1XvaGWyShw;ON^Y!gcczKUgkQ>DtNUPuE>?G? z95?l{v+RjU!-)$I@BgUunT@#E_G`u~23!D-&=^Bao=ZokFL>Q9Aa754uG^$fz3hm( zd-QXqW9JHSvkLvI5u#jFyx7DPcAnXixf1j{xP`1{_usu( z(`|DN>8)&S4!q!H6Qjw|T1#uN4B&Nl3>HlAv;bHKO99rEnNExwlVetZ6EfL$Z%c^u zx`{sBN7vNXCr*M|72dC_lPpNNC;QG)JR3+i;A+ETXL{6f<^_2GB9Q{fX$0j(L2PW- zx`)I#w8e6|!xDydfR(0!!M9vd#s~j*C}z4mi?E|>lQ==dbAG&Y_ijSmgvtXL1_>V6 zqY!TCX1NB>vcXXegcbMn6d0e}a*iDNabkrPOY4;1uO$==mXr z8^IVubpe8au|iANT##P_LPM!Tvh&b(AOaCI+im-H&T|^=0VdoAUJOY$^~L|?;NJ#s zn9$FN0Vk}lAo|jCXy-&dxx|E{HfccC2U0aGAu-^HjC?r7WYDYBMEowzOivM;HMk{- zuItV)4io?{9KJFA{rxUx&_cD}VUxdTX120Sg1C3!eRqa9^%+(+R8~^wsL7zxav+z2 z(d93{9dHE<1n9!bz(xjoc&c?aHunzrbie}P5GcWb>?hcvIwiEt|vHj~swxP~FK^Sg&34l%_Vze7WDVdeMBau3`BFE>|j; z|6m1!G#lbJJlI>_-c@DA&&7hzlvdync*DViMKoPzWEutg~yS_ukj>mK&%i-fg;3-KX zA(VK1lTDmuVSXHMtpm~F{i_VHy?!B?T}&U}1w>_<+q5;DJ*v{}c7wTmjDG>_FtFQT zv*8~S0!OyGHMl1lp2?pt)GeF$++!+wI8WQtqXo-nH+WFgEs6d+)*AuIOk z8edtQD9rqx9)n5@&ThtNj*n4jfB*jVek~tLi-!(x);$^J=#AXm&Z3p-crmTtr+4OAa-dS`Y5^(Dto^BX8#<4z?!eo!}b!1wS2Z(TGy! z+wvOkRuFmKp{!|hC{mE(Hl|K9=R%l z7T)Z;YI9;l3aRIaweI0Ios)B<-nct-CMP^WiCP}w{D6JNP@rib?g<#k9NP7Ao8O7~QSwpP3J^GfeSQS@2su

f;xF~U5e+ml4DvDLW2dc`bgl2@CnF%E|)2G|zOQ#FG zYPp1kK)nK+XNWRw$Hi5o7 zgb_uBm|}agw)r9p6w!rOw5c zxLtY%M3#EY)y2gnTquH(Prgow_*;?BRP4w|>4SkE)u*E2|5YHj+?v%a8CWgi;;u0j zds1m^(C`_k_){f<=%%6KfAHL~WXdymIA%aDEpV%U%&%PI1|4D1p~++U@8egIP4GDY z_Sp$6FMB+d+}6ly4h^Vf)iQ;BbNv^|o_jQmBW8+N1l)L5Tt4dxYXrZvyl%mCJ z&Ku+KIY_P1=U0PGLdT!aLzgGQWQ^dkxf(EY9jHbCv=nIRrjGk`;01JY@Ihh`8kr3uY@DG&YjWrC;l5zq9v)T`y^*RDj2=y&a9>)jDaB z@TOqKK6krw@_SNObwesvI-F*zl*m)~Ce=nb4}ooCMn%)L$j06Lpo!n-6Blr%;9uUq z^5X1Ut~}LVpK;J4X5ZY8>MpN6VIng`>y)8zrHBW|ZP_dHN%X2n_`74p=1}jNdEWG1 z#C~F4Q6ZHHv@j1OduCGh{I#SD4m?IdB;631ZKG$6yQ&$x`w(%^f%gQgRh?-IzVUu0 zn(N75ew*pprRoXU6)K*8*1u@u!adv((;R?0k0=GDBGB)!J=Yajah;7wJT`ok27T!H ztO$0(1Tlc5R5lW$wG|#jYejqH^fe9yzm+(a%@E|=WH9th*RXBwN?=K7`;E|bt zl0^d2@}lX?SEWQ^zAhe@@*X}C%>s3b<@2pIkk5~wd9G5Hf*=AC`AU~HCLX&#+PwA? zRHWYQ7O+13*feNKQ@v5Ge zWY{TK^L=Y?QsRZ{=V_Z-z2+8K5ns7rM&H08QuzY?N4B#1BeV$0?77y=Dp>4KZr<6< z&5BTGbw)1U5N(gr$877rNEp+TFn`Y5kNn%HUc_XgA^zN@W1d=yWik5n;%&~eRXZ&I zH2QpC@8;g7S6kowZ5*p42@N3tjY~xH}7t`^7EO+lLmltWx{h09!}fr z*hsjgby}FJ?vITFpNXCS#^opZz!zQj#-C0fMk2hOT~ZZ z#T*34MRVmMww&YBi(7v*$X_la63%xKZ?#_U0*NKgcI#|bz~bv@h6Z^8*1x%d1D~Q7hihH5rcbGh+8#8gIt%qi=L>FDWB6f@d@g7dcelluAwkO zplIqWaIG1jjTDPK+v10tnS;P|%1 zSpp5oXN{-`@FcA>c_&RF^s?2wvMxWI$)*-*LjuCTI^`Vef5C^2E`1FL08KflhB z7ae^SXLu7zC4%cHBNhq(4cf!oykI~EDZAv-(XHtMAd5ONBNJ-2t=@;MQWwb536+l%Xmr+wo*rq6Q@04@Z5x{Ma}mXHs=^s-1RAdO+L!@AWk)_ z|1wU@))nn8$6yPUS;NCV7Ooc;%n!p?9V{ET>6O>@TZO-+q3IWokz+_$zJk}Uf1F^P z6&QOvIpCeYm?P!27>S6V(bi<%!UJ1%S)4sFDZ@DFgW7$fz$n4yTG-#*y5{5Xk(l*D zuGgvjhOiHwt7k8XWZJh-GFhQzx%ca`rDR5<_4;V3#F(%yEGt$lViHQATBV#GFGkK6 z%{oUE#UyR`t#Z5gwsEcGh;3Lffz)&FZN>UGf%HZR+J*YO&*MUHlf>8P;C+2rguV83t48e|Fa!zN+A z!zl-AH#CIMu?e(hzM@}X0n-JpcdvE@u+sH9=PuF`q+xQq8{QWgOZr4Eb5HT}ZSz(@M8j=u10UXv~nh zZ5L38R;CVIoWtkPTE~|A;aYw2VUg6bvaKNzl_D&yMBbz}vh23we=npaOOA^qhNgMH z1Ve_Zg`o%Tnx@NH4yLJQ?8-tPA&FtD4;fx=dh!4oS`nTa`xXlU}# zHkUF7&msoe8sWV>*{%3kG`PrH9|ctA#$9cKerS@2X4}tP*>oZAVcW0^OK^Wd`llYusnJASsRR^*l$0=lY%S2&hCg4LG)ts51i%6xZp!`nkBynY)TB!xr^@1% z)Qgzu&5^iU&-`trNYS8M`PeH{d2W%?!(Mv+BSs=jyGLI*Tn(pg*~BspG0RzMkR?kC zMjow+gVU+s-|Y~HgMLA+>#BzE)<%o{7bAah$!NVlA(cvmT}*-b;37)*-ya?QOwUiX z|0OsQqi;9Y=8{wOy+`|IA1ro1s=1H#A}!rCk>%SSX%i}{kBZ&<%uUVS{bbv=#6I@D z`9iw#uc8J*3hISt8(?`EsFT{X)^O3T*ZFk)ONLPf@!`8g&6@5t^GJwNDyPH7CvFTO)3t>2^U?MA_T7I5NwJ5+zzbrO8AkEh#j_KDVNw z!n0+#C`LC_x~f0abf~xCfr_7Mz~Q6DS(4qy_4$~4jExnkoA&KUN^j?@`v%bW$0e#L zD|TzpJib2cyS&;V0Z!}}yY({p%FqB61yPT7Yfpo7{pZ>oaj8~Q`?1>m)S!O&jCi|6m}tVyrpbh8$Nf0*d3 zXn6H%3jrtnWRHeZNaLlwW0abohI^8z8NF zu0u$2`-BA2$_OOdP_==h+AveG@`U{4WbJ8pDYjP8(6C+0&oH#mHU3bxbfGO~Vr{o8 zDl_CAUNLh;L3u>m3>6%U>V}h#2h}fj)a%xVOkgRDp)b^Iusp9!nPZC+%EiYL`$2L9 z2V=dO{*>U@f!Eviv~&w`5Uqmr{zOJE2q-(2!kf%KsX3Oc3c^M0h6^dir5^LVUalI- zDn9$e%J%m5H)wkG)~EWF;#NKvBP*t0*{g#=7ojRS5X4QpeNv!uivCqgh_WsjS zF6(;mzSbVk3GhakzM$)(soeieM-;s#u?b(l{>J#4?CFO`>62S-N;;(~|4(lFt+`}# zFXK=8P=QFZi)4n8KRUQRJgM4;b2!#zLsV=73mxpi#=LB3H3>)-M3?Akd|2X9hv~}6 zM~Y7pFw)gSaa-Co4de;9T~fk35bSq7_JimxRkwq%PIW2hSj1&qL2DzAt)oR4eLU0Y zdiBHQpTzv5^uKnrf1Hz1n9J${c6=nCI0)Hs4WO^8_Dj<3Pe3Gqck2ZSVnRS!qmX6` zuRm(}kqdf9?SC;j3zR&?kBcci_xu0r<^M6k(~SQ9*du|mL-Ogdh5!CzA^BkbDSP+G zjhk=8R_um0^Y}l{WHLX^EQ2mSi5@sy{v|qPsWf56*G~G!CHl`$eUO>|DV6m9h86lh z-ur((_~&rI|IbLi|G6`NwnqHFBuoECj{lM4M`ZkeM795q_WVz@=aB-;wHs`>tYUXu zq4KZZ7Xv|`MqaB7s;s>m^S8vIC(mb{UJJGI`bxg)y%Zjl*&l4&GPL;m=ESSJw|{^2 z=$DECqXDAiN6KfJ=Pv*Ha9h$tOlSdv%lYGw&#}_y>Yt32|8*E(zU}f~+eseyv%%tr z{h$9FiuGda3-lj0@N4Rczq%?~b^bb>FvRUIqxhc%KZ@z{e`NSU4*yY`kJ|9)KTpa> zIsE?}9LB4awTbI!^iJE^KauZ^|NW&mKVg>seT1=h4PpKlTt3JDTGRG_LM!CUiTM40 z{E3=(jdlh1H`UQ?S$`offbCdY`dR!+_#fvMGd7qj++x%Fag2e zVYMqN@OH4J!pCj*Rwql#|2X&;@a2Fm;7GGd&oz~-a(@N6YCd&nUESN-0ljD1b^Eyb zp2y8hjHO0LxQ8N;eDpfx`$dF=Bc`xnoDMbqfTvFlJ@*tOz7vvG(icDYmNi)m6lRayO7P&yLTPw6Yksi~2n4P!ixY4VX~~_Q*H8|l%on5F{S40vT|X-{f{-eH<9N`O(^vUMytPd(E64_@|cgG22%Qc zpN_mZ!X={HF1I>s!63&ags8U2KHj8N1PAuDIcFxN^J(Di?6FFpGdY?n*w{h@peOgT09`Jl^pM;55fnk}G~!fH(3bMY zvM2D? zm2(77y^is^L+q6R&g?KhmA2gtw7MNWxq};R_PJ|q%2!S#(*G4;W%T}v5Zhax^Hy8B^MhdGJ(xFkxP#R? zv`ocH`ULd1A-??~c~3BA+&)jD9ryJf{b6-`3{9_~z>r)ERQT_$4TB`xTKHN?bfK4( zk&^l0p{EZFZ6OA(pk;6nbwRNB`dbXtAnJcAxqj6+GutJYm?6Q7rAw&KMW8I^#}2mRpjm zxZ(I}7q2!YoyL)ba9GO`Mx?%nnpG;#Xw4qsfVH0;>w(_FE14_ao#W>;3rBCUw@bfi zb{$=Nfo?AZw)&*HWmxl6-9sAvct!w;aNkN0 z{Q3m0a}3vhr_p}(oa02*VEK@yN_k)d_U`JFC>30U$JMX{v!RRkyCY!gGpQ%;UElQU3VSIPaH~2)z}3{ym0Z~w)p3H zEI@ylXxixIe>vm}F7w~&4D5Z9(6rG9+U4cXiC3|;8(sGn<~lZeQg`12wbcf-?0S}y zBj-e~pG0L;T;8_n15N}>An7`b;;PH(6%Qqy@Qk6;)9}TVG8O$NuDaOa?!ojE5(95# zZr7UYJ1=3-hB771x1mS9G)`JzEnz(o0=j;ee#wkBuq5h-`TdEhAsI6!$?Oc&FA8Y0 z=gWz|DF&G)9apt?mr4c*8jOi5g5FTMgS_@SyWvKLz?wk0lC}=T^e@|#Qn}$bDYc0! z&sKi7)7%^msJp(D3(jy4zNva+TNKbT_bn#_V6$uQZXFxB%Q%G#f6o1Pz5L~YP$xM3 zYVf6#_U_|5B5O_}KYq5eTP_l&yV-pecodsLdBR^|Gyn0?)~yepLXS^Bp~ps*gYF}b z-hpB-I9ApzQo}MXvQw>fp`cj!6WN<1I!Lb$#}NFxV*rb$hFi$Gy7mTxi|S?d@`v9n zM17~<%kVj5LHuW9A+huD9zM?HmZP9e#Q%cASUsof)s$l^F$P+U{O$l@31ZBjTJEt~e2prNXX#oV zELFx$m<@wFR1+cs_{3*@CVkO{<(SQ_d2y&qZVd|~Yi^4k{;YM%N)p;-wi!*Z3EOxv zZ}o7qpem!8S1;WbO72w}xVQhs1rTH+KAud3pj0HWqAcvPA^c2)TUOyj0eT`}FQ`UC=5`PkQ0HHjrDW)hK{Kave@3!@Wc>&dze|m)zd!SN`s*1>e`q`)pn+ z$*diTbJa$nAs*W?zxJ%gkueq&TYB6$98+cY${UazwVl)qMqt*1j-e>8!U<>Me&9ZN zBS^@rtH;60GbzMsW2wG{bp8ES`}CvI9ZKG0?;sMRm=He%hUZhmhkQtB2co#K?Ug#6#^t>V#rSads7M|`5Tj<=x1FGvj z7_zL>mMuRbn`A1aLcD9{f@3ZW>)+0`*yclRd{dJbYJ9ulv*pRM1RbODI^J!g9wVbVF)bhazca_jw8f*G+ue zC>3PwkKX*F*6Fogi=R+6C1)w?lR9?eK)2@Hv0&_euQ3XE=jlj?nqB^&4?U&iiYeP! z%S^8lcfgzMQyQ-sS&KV9wDzDi!{toDmzCjpu9=<_AxLR>e;ywxaR^y)@>zTG)`9tI zD0=jw+ii3L-s5IAGM`4o{%i2_^71bqZ@c9k%NjY|c-n;z%Vv(RlEyMs`k#Fn{4Uf( z&%{m8E&jwzvwG}WjHTIB$2KO|Tu)0m_-=;l+gJM72Xw{Zl5X9wMWj$P+v2FIY<;+| zwDR%WHmNc2_(iep{ENTm7nucp&y{WB@A! z?W)^7utbj7WtgJp7%-2tiH>~d=4|qq01G9(rAFMOuyE@8C^1KmX7L@#*i31$x!8^U z!KWoZ@u6e0?&{M!@J>gXxzW{+1&Af&7wGZo@HmCN#d^juR{~DnM^$_`;bXjbo>sbh zFL#aJ1$v%lN4=(DxZ~OVCh51MvtgW4Dd`(g`@{2;fQQf|1G&(7Q92-vDqn5oU~ZTw zVaETV6E`JZ;y{%)WS(v^Qr>$!0&Y(F#W>qV&u6^F!nW{)g|Cgm4m!AZeF@9WLX$oH z^c(^q(8Pe~x!2w~Fj~9p)5qTTkVimxbHKeoK@tZ8^#*V;Hlp1so7ll9SOoV}>&~x8 z2zf~*??Ofs<;g6*i+XYgEnIlcG!8O)YlJHa zZWrO48Cjc<9??4d6Y?Hk1|{+M=C{(n#B98W#|hPt_atI)*Cj{7GhiGA$8MTLc0uS{ ztrI?vr^bQTqf`|pg6S$pFNv~i@W&F$Ip1z-Lyr}Ap%i0ZRJyaIMvvga=C~!j+Shg1 zhBFtmRpe2Ki8^1^%|5rl@XmYY+7I2gaWF;v{rn4+{G}JQUFjS?J76-*YWT|PYNxYg z-|LvTX)U;H*f&#=6jHmw~au!cd`gs^|?J}tv#*3DHVv>8*tw{K>7xPPqk9)v0p zyLuVwUzm!(B0i67U$T&6StdX*avIw=v{hyEP#IA!KCmE@@O|d)Z%l)#aU-??hsGScS{nZV9!BLfkvG*=zzzQ?#lEB%9MUAwO`Cpy4 zdwe>5J7Y_X+HNEQE2A5B&o7Qn(m1&VXlnDTT1OXm?IDkX8oDK$`-7hzvbpV^Z0Wq| zK@J76@hiP9aSMV`upGl~i{LjRUCR4+-`QCFSUE47_&LQc?%vJJ;%6j>;{*~D+!xhG9E<-$D#IB^2P7nCcEY3eRSrT(z4LVvcg+*JN+32EPlItrCdBK zBVeTxD~wg~-g=_xLwTW|&n;BBK%shDg>XK*58yfcv9bGW(S!Ox#<$A3P8r&cG&^Pu zxIQLA;Z#LWw1r4z(0d-a*(mHe9~HanIA1vST3Fk+-hmA!#z)JZIZMIEW$SN_vV|E> z5iGgQ?z-uTh}_+2PbXKjkyD7=W1;ajsE@NHjVdEeCVJR=b8v`6o9Mg5DP4V z3SmnAsW!5aEjI4EQ~FIg1!{7qZFO7A6Cm8#qJ=iEMAn|Kmb#5Nd9kMJ99wIjw%5B} zgb@%*q^k#nR>ExMWNJrNSk5j?QFhfgE~CFSdRRTxhygyI20qvkp<(DtCx%_ed)?^I zn08SmjdfU1Uvlz$tonnM$D;Q8Ijce$yb}~P@g(@Z_p~`)=%*85)cbYr3~QRN7M4UG zaUQFhPaax_VI-lo>iZ9FJ}r|Tdgr1_mA*RDM!jLVmDR<2+b=#INE9L-&!pD0qP3hCpy6*oO4dqSYDmmw>4Fn_s9WlCnlArD5z(; zM>k>eSCy>}xM_ZdGCPyF^{P)GCSzCwWXNvt>SPSXSQsg@CBCJCZ&JbpgRWz5k~>E} zBV~>{6>*;%rk`(1#`SRCJv~!AX+jHq6MiJLyxM!rgCCfyW>-(pF%HaWGh*jws@~5x z@9%v6mJgj16z6NWQ7za8kdzs}C9(}%22l^te(hB28kLZX;w@xZD+9tQ*OV8t z*afM5t}=$|BTNqMs-%cJGRyUk`&9-^v?GwU&8kDj3iAeK?DCb_LGw_}f||abo?d#L zIVZh9vacNA#`Yb0w2pLHz*7B&I$~U#wBx^v<+spI>9;rPmDE(+c>yn~AQ`EhigBxz za5mJM_4YYkNfS*jZ53f3mT^*x5CST?*MG^zu7CgJRH}b(g8Kvvw^B`tKwDymGeE}V zzB_lP$?3vy1+*EeWmh>{l{{RbRc-FU$!a4UAoYX}DrJ#m~O zDaNCEPc{Mzv7PV8Ff(9%x0EnCq^3Q8JF;W`Tk?3Lealv%+9|FZ41K`v#H;d)YKXA6 zL~B(j)}s1g4J%6@I_<)3^E(-Fw6Fi*Rp;e!9VKCo*5%f6n-;3va>mEK&@><3Y!Ql& zm_^d-1{p({F|Oscv1}{~nhtcMwrwmlZd<>Kr1oq~b%0w~MyUqda6UiAx8BP0osIXI zKDmcIzL1kHPwl~dr7(1|hK;{p+2^b1cP`xT2&(P$vL{zvo<^bZ0{`CGq@qI^WMhmd z6^m52JPha-ZUp_(=}Fr4uck(PYC-NW%qbk)=3JsonR8GYK&#P=% zsFizBqbRo|76!e?fvW7EfBG?d_sKZ?XU`>c@Ld%bLn@c$Te+-;h==j!&!-JV-cRbYyJ-r2#p=`V(BB3MPfnBe-8)2lp& zlW@IZ)s0l|SvazdM&%c zmPp@_oNF(q2f4caSLiSL&3z^(E;)|?WBSd|XcJGXgw1>g0SQKzC5pdU9d8^4=#ST# z{gEIVD^ez&2MT-wib7H~u__B9DLljQnRla~OetkL#DoWye@E|XrW;22b=;zLMv*(CXau@FmtLm&!u^IIH%Ln<~AR5B9BSSV*2Kp>A#H z^NR+dM02EoFcv@=clW{hDQG1}G28DO=pB$AfxjtDD=CC%eRn+)AwA-lYrNQRoF)X> zemK0c$S9zdrNs;#nKd>Jmc>L{MI#%!os0JTBFJZ8yVI8U`cLsvvt3u~!J zN&8#H<+jP8ZE)kU_bqM0f6jV@up6*~7R>V4g6Y;gV6AZ3Gk2xq434TN!b!0AuPN~Na6~cgnAKHy z(cD=TL}1t3G?L7=H?liiW)=NRMoPJkl8rZC_=77K3Ruo>-D~dE9%)SrC`4}e84DG# zj`@_8&tSEkBWm2m=jXGMFKLwV?**nXc1tl-HfDt=+!Tth6lo{i3G|o#ZX=p}*0(pF zz~ae$E#X@Yp?m1e!*BqJEbJn8YPB(#)Ah%$0`yJ$%_c^c~7MM3~ z3Jqb-IhpMIoQ7Wf7HS?&4W{h{Y<1X3e}CTG2eRIlm+thQrEU-CnBE(C)^{+=V(p2K z!B)z2)g^1%raK2eQf0g}s-+sD0TImC(FTm2CEOdxy_m=FH66v8U)`P&vq;Xy{TVF>N*dY3g!IA7(>LO(J}j zdx0}U*P$5!K(dh|;B2RILRqSx1-~HIV?E9{W_^e!PrZ4;@rjMVS#{g?Uu>hv%Z?ch z9g0AfW$FUhSzT~HIu#q0C`mw#`_ySfWb~VlI;bp`P$E))#$R;egLi*HhDy6*LGa$7 z<@V@Z#5Q)nsqadp^XU$MGY8N?SLDRY*m5lA<+XsA=Ew=y;swWyQ-}gkLb3(aZs(gp zd4K1e73cItNBhFSmjRa$*R4?SVZVGm3XzTy`MAy}wp zCw#$@#>q}ZCRAfqhhMa57qjIFWsEX3LWNOQmmEKQb#G*UycTx-Co1NTK8UZ?cb7sn6k1`nkd(V!6Tze zE{UWIihEb z(0JPJ!Tx2tF(lV@bf6Jz?ShO(muZ>>%R{-;d&=n?183_)=eu{+ z5yEeLtCc5erPUVaulvZaIA&bnSJ`LO*53)AYnQuClLIo=25laS!bEJB$NVt&oT^LO zQm}uLj&4ZGyUdu}AwP?V*)AB{)wg>E=a}142^jwwT0Zh9TIdXUkT>~|)%^L7ANA=` z@x{`CYb71e0eSB-@m1#>bsg=-HzrQx+UM$4MvuKr2IY9klOl{Zdhh4`1yjpUzL5^sIoyWC|8ts;lyx1N+n{mX{<54|A8{ASu`)* z`9kyyd;Ozt`^fkEq(@-}-6=wY-9=jw6OYnyya|9gMyKqOl|6l7DeoxbsOrM#ybJ2e zfxno|iJaowo_Qq~0=z+X=gYgzNZGY6CU)5%gngj6-Q01qn=nU>D^Q6W{VaRE?VMw{ z$yyIRL}9Akq=4;Tu5)0Dfb#q@ABq2mfLJdgoL`sMdic%#Zgcgwg#^cuW{_mGtKHM?xxEXv| zi&f^D^O?`{d-`EcjobGm#@_fMrmdAjvwJq#Am54R%6iArh{W0o<-A;^ZJ-%7i4SOXI)?kDOoH)6nLQVU^m0B6BW8P{D<=P0kOR6YgDhk zKo$Z}Tbg*LYTs#ok)oI>PP>RS?a@%C4L;ds)KKybQlu9LU0}A%gYHq(!qtkPvp=b+ z+*~}yQNf;ZTr)l|UPZj)vVX7VrO=4daWBj`p}VifbxPO`LS!-jrGfPo$>0H)ay>bC zigPy~X4EZKl(Cjcfxx$f>>KJ zhN@f9p+Dg_%okYobblj`BKIvVX2S}(AG{3S-$Povk7E4I_c2~mHEt$Z2f5)RHme;` zg^L-RS|ihKr`VidG!(gfZoSI#ZTPjQrwv2n0wD6SGJQXRQEGdy-Oe#hQ$E$84B?p{ zES6!+`}SzUgivObd46tr;0}M-t|S@5WbZ)WWH_BAM)kNz0~VhROyvNTVqk)ng`ZjUzT3^4}&t9WD&2z zk2`)`;X`1KWMp;UTO3B3BNTlOo;{ly58JC~F0s-2&(-fE{u?3aA!Ga(M&`}hyA3uc)%+v#x#bGGFt?7iPx zPUKqqb}ZZ({fF*+4w;oY(MV11)(-CeBBmWfRrG5?sxEM?Te5;SBHjsY2YdsJ`pz)R z7dMZVZ-=2>&@g^%b{nsbc1>4|>rO=U??SSLh4?pSn3;MbFyo%wNcVf#H_1yz9BZE^l$l&lqgYBa1mP1y?OC}?Z{_(!J_okitL>wwo>W;(DBXCb4F>W zaJ_{^6WMM)s#5#OyJ}ULP%8!dQYXtPCTdY*D1dO3y}P@Q<~_$!lUN=YMd@rPRzluG zHpn(`GIWtyE1_l}G57Xa82Qi}c{zNF63R@@EWhvW+U|+)=Jn17Cet$rM0I6#9>b^w zs+&*0oljt|E0oJ1k!4AAv|L(o=MSHyGyHG<+*quTTNi1PtQ}rS4W#)!J3?*mAY-E^ zKJ;L1-*3Z(5!tf|;P%^H9Ci8lcnQBXsl0+U-hWKAze-uEvYu>oYzeTz`><=!=CR|l z-#b>eUj@reuLt|}H>f&)eEbLZhZdi8$V`wsa68ZFCjYiZKG z+eP#w4<98BV@=->mA$)%oH-Vz`Ut5^LFpO=u;jPHe=61QnZ%s(&(zZ*60a-}R&l}L zA}A%MvKY0~4xJR(xvf*RoXF{#>p3zKLGgje`pu^Hk|Qxop~G-ij8k64Cke&bfv@JS zAxi?N(Db$;X+hbg%rOD_KMg)3`&p!W8xZ>76EifhcbGC0sj@L|yFdHqC_ZZIho>*^ z9=D=wUdM=+IS}_qU-0ED{3}kceXf8!mF-#|Q|URPB+wvN{x#DTC^b~LQ!u{O*`b$6 zoa9dPB{}GHpRHM{HjrZ_v6^YeTHrm?M}P%FKC^GmsJhf(N_#t|{I(`zVhXn-hC6-s zp%d>hL**{8`n^uUZAR-=ZD#SC%YY#2B|E%6Ls)mA`Fwx&yh0OTdt~G%C;J${@<9b6 zDy;?6146{>eCoY_?9Ux0MJb)5UKMN~;2&=bWqJH#v-T@m9E}%1>XwF$&wTFDk015l zK;%`>{9;|(0=K$n;<)tcsJi~tvf#OylDBbDBnhnjhfqsfw;tP#;aPdG$Nl)xeAE8V zvs~8Y;HkZN#1%0>W>Pp&LpY&9(_sua3F8}?l0ACd6W+JgpqifL@2X&Pzx_Z3Q)721 zsWBV8Nb<_O9MaWN!)T_hb$>n)y0m8QKVi-w`H)mp4_vDxS9)(*7dkC$Ow6^a9r}uq ze5T&QWWuFk;-LP;X9oyCM|KM7qMcAr5Djj~wx{rHD)^ZAe<=UsmMIGigpmpVfd)s8 z>@C$y|Gyi#9@h8X=~pbkf2G_17sBEn@W7(&5t9b}*=9(KEt>#hy$Lv?!+yq(=$>9^ zheid^g1sjZ-|Z;Ic_A8X*ij8mhe-R1teRC8r`ObvOG;G(_N>R2E!l^CE;e` zK<6fQkA-?Jktna3A0d)GtZ1#gB>e(;w2er|!A#yF`&2;Mgx{9tLFybL$u&ZsWIVqI zWAxNHb<7n|jC%lwh}SFDHeykcOXZ%pyF=Vl3#x>76?htAq>sn2LT4x(=)7Q{neig~ zCpNggb=D}YF3FO3Olz_a5RUo0Vwr~r&qj0c_iLylBD`h_{5s!r`7`b926HI_Yh$8X z>qUka5lnNGtr*y|JOCk*7M0dFRJERQ;DP(!!YgVv+GNjK!-XnD|*<{Ft- z$GHaCE?K79`*h2bsXdEw%eW~Nr3b-fO5NWbNTA%{Tj>Au=w7eAoNR- zlaT`t+mykOOHgM6uZ-!Bc+kjAhqZ1+wb%_Dx}T^Or`Y$A&XwIKhhkYZ53&in@#ylf zFG>f5_v5F;Z7-kZdZjEkuH-vEd0sNOba1%)-5mBo8nPe_uIxAe@%2~!nqY*4o4C>+ z=1nJr)EYIbuO#PI!->1hY38&B(VAGZMHO)9S0;Kt3=8&NV{G85oc>|==pEF~m{saH zDm$_+E2hFB+otZPBw}p^%vGf}|87GpVd9?h(m7c7XDxquh3^gvDLkqG?>%<7_mDPc z#aZV7GgPUPy3;a=l9^J=Dz~zoH7_Y> z_rb#zH)%6G+di2!kOegMAaI1bt2?V?29<16S3;R!|e=pp7a z*cJ-z(DrEr@22U^)zf;!A@sv}@ItoA&SJ~!yx}}xAY~n#)J8E@7o|6PE$Q@{_VJHc z!((0uIo>}WMGXCww0h)k)&%v~Ak!yHQu{W= z%_?+_2S%M^Ib<-1eD0`=rVFKWUZ8l$-4+x?)44X%T)j{E(Ou1|uEMG01P~hX1QD!u zpb2fw2InwDd=j-Hqu|{$?>~R;Q7LnapNyQDY2_>}yt)(6=8WrRgmm19J{Yu{>#iDp zWu1Nj^hI4t{n72B;FsSBSAEAmEI*$%vz1KSS)FXWdyMIobE~O}4KB+$ph%e)|GSDl zK9s6%b7AA9AKd-@JkY=AinJwHOVGm&Fdty6H%Y!l$UG3eoe!ZygfE{D`)s z&Jg`@j%@Rcg2xDec*byBAFQr&f6#zO)*p5?*ZUh*YX~Sjq)fL_G@PK~28U-j2E9ux zIZCkl^k|ROWy!GMRe>>;(gtb9nSn248j`rT&T#|uo4BNY%$;Pm%0_WorMmZQtuLZ- z4n?=mcYVWd+Rl?n5=tNlE!uXPKx@QrT$7{}$3k{O&^9gSx}6i>6f+SQ0={UfUrO_M zxRNSEstb;a%U(rxD~sYedGVlKt@CD=?QRc42#1@-DnS!(FYg09ci2#to#xi{`@>~! zcA=9_;3+n4#zdQYvqyiGWg=pnBq+2OFKp-U06HO*y2i!%B#9&ln5SGpxOek5xfc}XWhcPl#rn_PXnLuLI06{ zM{b_{x6!KYy5}bYOw3+I-;$}}CHzg$X|d&%2t!*5-14Z9nAHS#xi#pto}Y<=42ND~ z+jL1Q4(_W`zhDvrt{tlj7YjZf4tBT^^I9*|)0EfMy$jiTaiY7@q;09H!TVxkq>UOf zG{!AgV=2UE;k~P1(6=v5&+yrtR2%UYC^=au&97^MZkcIC`Iqf!RpdH;-9^e0EjyMa z=I^bj{JA|$T2dUDHM$kZMEUQgRp{Nq2#GQ2SyK%*sxqRb7JXlqbKZz_Is3kA!^8b& z->Iv(z5SI|cI0uL!B!>P5w+8>f~2q@B&y3t#w;WM(_9{aao>C{L*6uMo?fDea;}8G zv9t=Qq;DzhuNj@SW=nN$ZT(g*WgJTYokQp^G#iPJjNmKkUY3tvOswr}5iA%>h^#N= zKW6daz6b@ahB6(uyM=^&?UxOoORmt{>3Qpzn|s0St}85Hbyk>Fm-q7QVj%5-VAWxX zabrgDnMMJN)&gExYsuQPoZBXJ`R!QiD^pZRwL@c7p`5uVeV3iE8MSuOaRkB%9S=e52lrWLRmYVQ zbPWSfbV}&OsP5%aEf}SCpz}$Sbv5&JMDB5oC|Xgn_ON^ZA{%RAh~f^5WC7Zgg5t&4 zSX(-yO!1$#7)OrW9b%ExctynsaO-jI$pgqsA$Q`E?-*^=hWF1zwm9jvwQ_wG2|d}_ zt3E!A4i#x*mJX4gucTFb?kuVzI%&V5qE1wK(wt}DLsseLBr*Y2U>xWLo8yG8^+ec} zPFdwCV9a8hxo6*7wRIU5@G=KpYH66NQxw{EZ@ao5l3FVsoFY+K<26dm6{xiX1y|*V zMfY|u&t*<4bpGNBM-JP$i$bngEUzYaD^o-XrJo98#g!(qGTTu{Hc+bw-#6j|x;)&vbH8bE>H&qz^rrTC)lCVP732Da{K)|=0SY~-D; z1WvMNOi-Vve+R}UE)}*jMn3Epr^tkk<2pE&ANkY%Ce5HnxLu z%$oqq932M_<{rZ4FB33>s9G!$Eu?(SpeY$qUtM8|gK9Y`?59;G%2uQ_9`D-&PV@*L z*fk@)G(Em4!YUo<(l1`FL>DDo-F zlz%+{nz4NSye^Pz_EQSIKc=N10BpiLe zNft6na|*;DW)9~6soHVmNWR{`ijMHxwC)%KtGe9A0$05$XH|>)S~USUtx~sKyI)K3 zIBMpN`4Nse*H*_fm#*uG`Vo*`T+=R>bOwp=^XtP(DbMniw4-&^&(nWDdi+F_*yESw zOHojOyZ`gOKgYUd*6D}&&MS{nV)F<>iUhOa-L!{4&DpHid@=gB(I09TvIk$lyM(+h ztKOZG{9MwKlXLQndWm6AAu4=`9T6&Yt6&UTv_*3c6B+vnn^|{OpYK;Xw(`~yx0N!$ zqu|ut#^KjEoS9ToZRGyWQMM@J?z-kl4pmlLxu3tky;zG>|5vG9-KOo!Xc8Jk z#(9wWs}I%>SkGU3!sciFA=KJdg(Wfe%6C@x%KmXdw%G)Bc1Zt!tJ<=+`r0np5 zEISo?xX1Hhno*=|@oR{W-SlgV)Q@~Im0RkDOh3_vOs6RvN`GW!0@)DoSo?C=o7B5r z*0Q&ROuIt0@x;bYB&cTk@pw52b{UQFJnmX%gALRtSL=5f@C2wJicNoS)#Ts6u zl1M|ixQ*$4k-&$EIMSpw17$PtRD2#}+BcP1c*7^f?3r<}UgCiU?l#mWIR%TwMnIuJ zMjWG3k(%vNQUvh1FGn!@2y)q@A*Y@%h8021Ldy>dg%(|BEuo`$#NOWC?7~9Pj!zu< zYjH>2zeb#W&9@5$9&T2~tE^49+5E*L!_CHY-yhs@Zky60f{X5%QsjUZDQ7(bo9<@B zPPr1WDlX?b!0nAlostikS0I;%zkm!-tXENo3SUr3*Yc(hFdOSx9$Nq|(Wrg1Kj5)l zWHRU&tF!w|(cy-e@Esr0+m9#3P2N|ElR^wNd$~&x+ct}2kJrcg4>w_BByo@Q4iL$y z)y59m1UmQAy2h%q)4`%<_d$jM2>mG>2Jjg;iPaICgW*JEZE4Dv1w4Ct_7fQ6Jk7^h+xam@dUcb(v)qHud^S2OM2RniB;&nyY zw|e%eUMOk}%q(I$RVh}AduZ<0dez~597^yf&XWQ^je)`6G<2ke5op)=XEt??OI5_8kbIQ7<4c^#*aX-3g22LHMIFqdxx`j+*=Iw z%51i^1&*}^lWMh8E2W*^JI?nFEYqR~)6aG68ol&^V}?$9O*PnoL6M~!SJXGY);DH- zl>EiQ9&>RwiP)uRC?-uY2YS3QYx6@ykKU@NZ7q~hCs)8edSTMpqE4+le?Rk5nEqMj?Em*B`qXU zQ``qFYUn~;C41U_wD#@0c7zVwR7v>8(B>4NI+R_{Y{%9GC}y0|e@92J-4`=)7TNuW z;p~I-8Fo95YcR30#$u`1%28ypib9W|Ub(**I>>O{gZ`A1;J^86bKm6o$bwE^F9o>( z=Btv3N&v8=>{O7j?}S^xhD$-P*n?Aw!eZ%ubG@k+)X#+{cn+Liv1>iOm_Y$_cM2k+U?h{G zBGYAU7N)E^h0umQdq%HHy|8dw1?y0Bn!L4+r^Y-;zJT_R(m~e>!K5X2{8Z*1`QJm) zX6N%f=Z9E*m^;^N`SE&B+LDFCtCdC<8rA>$t%={2dTRbRx9-BdnAmvBj4NG2dJCM~ zp>}IY(}MGh6W->)o!$F!m^3@H=}=_0?K`20-uaE%$ZgBVTv6NgS7kb`y%Y*}b93>N zEY)ayUYu67e?F@86~G@iE#q(o^VYiSnYmu~TXakq6sS!QvQtyhCHy%#oU)Ba*Tz`NALvW`JGd6*pzoNsEAuGkR%4 zdWyM3YJ}%B$`%b)aOs^gS|_s)8kD54{aVn3zXp!Jy_cL4D?v_kvB5DALSlaFclsuv z+KJAsb|Z72?{%k4*(C;q9ujv#q1DrlPMi{5ReL=uHDXq(GQxLjOXAE6Aw>AU^b!f- z!T@oo(fJPVPe83iM_llTF7<{ZhR&RM(0T{5@Kk0P-w^<(?rM_0lf}nBYl@)UpGA&c z$Wk&Q+sa530qjNEkoWRXG1lrvS8~Y0dM5_JfXa?Kn~OYA#7J{QEbqeMpkaz__ssk? zF-zU2MG*RH-rCE2f@u*JrO$!9&r-fUy~2H?Lms+x(ypxU9nKaCCsf{pU7S)04w_uB zW6s4?BskXo`tX}PI*($w;>8}IRW9~l1%b$vsB*=DL4y-wBY~adTiML@k>`)kiO(FO zCQpbJw_{!5LIpiQ2L`SJ@(9D4&%Ef#jOF)R8Xi(F0RqU0S5j-<@+L?34=i@e+bobe zY?y9aFuYuK)TSR`nCyXV@Vs-)tx@23JM*EWG$pXa!Y$1y;AN#Rn zpGDgq8#T#T3%F6c6*HPN-<&r>p=P+j5GhRa0v&v&Nj*VS}VQ7<95jbGOJRWv(a z7|}0y?R(bYz+Jkn5LbnNYIl`dVVQ2Eh5Ih62Eog;|3V^s+4P0G?XRq{iuxDL58t+8 z?qe_SYy0EVfw}o)bz1w<<-)`l2uPNf2jzo{eP$sorb5#LrcH74VYQ zc}8o{1(Qy==i4UH*|B`~mk*t_{C7nH4H_T2S!Ah&^6ROJ7zjhv)J)Vhi_uR7cx1uj z7V8sb)cIfrHm1N?@eDoau7IL=j`10`(YpE)`f!V?3*`6Rv!`LtevU{TH_gL#&&t11r8_T6HeDJjG#p~m$boy`SBun>ELK-n-@#x*T5OR- z8VegHK-hn;&Y1T9K(Q6!W4{0bYZ#A<;{FHXO<8v5I1=)gqM)V6N}Yp8Us{MYCaCrT zf2l-r3=beGX-{r_>-Xr7+~qqbq89aLKMZ?|jSs1GuZJuRdbDDOq10MV_-78J6-UJ{ z#gQ5~Jpwm1j8_QUWv1vLhj34=h6yn#aL|_%%->M{@$M~_J;P}HUEEkmCy=w}LXz?# zDwZ1PjeS}wqBl78K#sIKv3OE(SyVZWw1%A|pmT$!qwV_ z%gO(iQVsFIJq*K!Q7Mzg7l{a-hWk$%=q7g(FXWZ;XlPnhxJrGRUABa7eN`OG{!*gv zrHXbma)Z-$U>AFN3Q;e_kp}k{^EdA$M?glm0GwUUK6U4iNGRL2(yta$?1+N54vpr* zMPQn)leyI#qRuavVBcgo+Or8XdGuBVPR!APsusVRImY+d-;}fySrm+_LqF`nqq2Gy zEeZOW!vyaq;4|E+fDbM{`xB0+8u_?}2|wl)x8Hi=-(Ejl0-f5-XzXzO zixyJ#3P5UaQ=KH1p*u%8%$QCq1Wqj%m>InWZ-<3cFS-U)mTG)D*=MD#-Eqy6nA6Ic zRhSr71PY`OB6jY#o#3SqdLZk;?FU=hLErIG| zCsUK}15papbDr-vy7+iXaAJu6$jS%VFam0;h&aGB=adhh@J4Et9wxmW)vz#BEg4); z9l7?Y!=8LGaHNucgJaySDOq#ze%dAi;E@q|c~VEtEehq8zSFsTVqffOmuKSTbu^K-y;SJVjb3=|&~Z zEiz(xDUB+uX1InulOR^M98N?2pn(!;Wwfje2a5&@Nq&9J{HLGsks~Wtf6(SN%ewi0 zEq*KjQ)cXy)f|Qt-7MFW+l2Kl#%50EwQzF7t=b`dBIe)i=^Tb{FJRPP_KSEBCAoCN zGdWHf*G@kSd>7u!kFg5Al^6h!v>mu{_&iAz5c);v#h5*!x4*nPq_Yo_81UacCtbIN z=wq+Fcs7!}`#P?+t4%m_69X<9QasIlFVgo} zt^I?CZOPZr0aThLqzH_El3P>0bv`}5&hh~c7A0jitHOFgL7j(~InC5IK z`v+Xx|LPBonfd*!kj=j`rfUM{R<53*S{VaT!)7z>airoPFQ7Eok`u!AaEzgty}6P& ziyhTs0Hl&`zy86DU!oQQ-*(OBRxM;VI*AF+gO1JnCuIsZ+HCUV;6$g*mCtbjA-gpy zmH`GXgx#~qvuFH!nvjXE*?J+T>Az*7nmudbrC(q|Ky%$&pK4;VA=_hfC)wFYnpvsu z-ZYiC(kOE<`$Whc^~}L?7dC!Va4^hRY8Q$j>(9R(jj*oJB@Vt&mAt2ZV?h_AEHfHz zIkGD_WeQi@ObE5zVlX}kt5AmwndJwiHLPZ_?-jgtSmi1v$o#9n4AFta63bt)07s52 zb2N!%sh%)JlNxs~6?j~?nF^2;UiS*_3oE$g58u45p4ia;A_CO_J2Y7mbf@$8hH_rA z(l-TncuY1$VMG9%9^LY)02G}mq>~8I1$hGp@OGryAR|mxt%;jCLD07(If_{-;#Koq zDf&13e`NfF`0se77PNI_kluTdqFxlf9t_8sy+m;f>#_WshLPAOs1fh{$L)a@1^NbY z_Wn97Wq^1GAn{eiqbZs?h(fG|*LDw33zD@I#4QK^FUY<}Zgl zRs{&e_&P`#Gc9Y)Uf}I4CL?abI=mXBN(hyZL_Ald)|m?H1M`L1+OrUQTnyr0$5FRP zevr>}H@?OPd$dSppwET(n{Rlxe7jK>0lDSTpRfwUEbmdyqX5=ck)=d6n> z6m0Lg?@pN8lmV(xoISZz46WOpVIfs3^*xTX`92c$R`3~5oEk!@ka>Q%43Scvw5#uS z{{M&%Mt5RIMpo^BXo%Ob;|iv8<-lR5UJ<(+`Ok}|*`qpZ}aa;@$$bH2l9wr{}A7u^`y@qRd4v=CGHLggHe9?nIWwXPhDz5}~+9-%T(z zsqe?(bu{Kl?{0fM_UY4S!^jAB0F`n}`?Acpk6E#(ku6n%xm2w464~t!uYlgDq#v@o zf6#?zquY`&8SpATUzSTQ9AI7)Z9k?Go)KAhVEmmMnKr|Lv?HFBwf1(<+YSEgd`uxb zclsHZdsB(^NozSU-1xLfY_O%6!s0qvwuQ>^+_VHTb~5n-YG@cO>{`&k%{c4bDxrzINNvJYtui-3zB`Gvkm%qIPnvTJ=j41 zP`>oSQLsUKP9|htoti^1%D<*CiS} z@#=P;%0TYWYjX(xt`CC?cOW3u9=Fb4pT6k%3N-6zQvet>jUev+% z@k~`E`-3#bfpOab91O?0uC{&!>m#+b1n9O(+PmlZMJ~J<*;_Sm!HkQ`xwEwDpTz%r}j#&TtI zvv+>2Iy#CCAaM@+#rKwqN9(oOI$_ji5T8q}mQjQwLrgg=M51DG#1Is$SkwA^(IVvO z?Su>y5$vC9@)OF=8sW2*rCDI)0rr^q_3I|sE%re}0U71-t%nJaug#e@*WWil$fcMu z9IyLudryq=<=ihAzfwi@51RC` z-#@pKId(Nvcd9@g&AX(+I}d>D--@ik{o@tvza}8W@2B4aPR72HZGZl7>MISBCOD(s zBf2MXAK-YVV3%Q; z+qa~nkGn7IW%Af7(uVkXehus|38U4^YsISw#9XXt=P;lP?@1880!g0ST~jRZCt}7& zXfdx|)dmi|8t!^!70R^|={(c*RyC#m-dPpiAc_2XZeKIe!+tRtYVNeI{!>rQ)q?LOLwHmU>n^!w#4=i`wKNOA;>tJWatO&36{qdWuBLk^kwvBrpFXi+wtHy#)8@`E;Sz37c`gT zYDg)`Jwrhy!So7vhJl-}YIv0AQoXWrIXMGqYW-IM>EGrgVmC;|hI&^j-+6M({+5*W zE!{w;#(8gF^NB;XJZm7Yg+=rVCjz$wnHGkn+u9DgPt^vhKwI9PJ)g5O`vl+X=UYkM z{_u$6!A0^iH!!8SdgNgk>0e`+>j+-RNWm&!l~lt-%g7-%yn(nC($32p4Ow}*epo3I z+jc99pC2jkGlq>t86lEOn%lE5_(g75N_`7^Il~r z@GALOANMxlsp9j+XywK|hdyjQ_o3NJm6klD-RuQoy)&|dh>$-F-sem);`W~hzO(I5 z>@ws;bCMY6HHSZN8&p1|T1nM>DPo91QHg2v0Oz(~9%c73Jb#QyefhV7#1}~b=-TXZ zu`35}epeF>xBewGt^A^8nObT0*GSW%2~}il6svBEeE8?T4O1&@bdB~4VxuN3@J}PV zX?C}P`_uAsc?kgjQCY*oNJKwj(0Jyhq9uMqgq2uk>EibSGFU_ad`ss>aMfFXO)-K@ zH3^y>@Sw~}5vU8V2cI$yiz2fl(C=%SY;Y9Q$UvwIKuGk=?{0@cQQW_B*bqu2N4 z0+LKQ%2_${reF7SPtm;_p`Vv^y?6Dp-#8#!qAm|I>Y|25(P_ZS+rFIZwZ+HsGzL1R z?vci(h~oQwayC^n9Ckz-TT%>{c{?u+u!F_|0vC@L$b7yHM}vTEr#%4Qqck5jm4_nK zLNo2d{1c-qyF_1ph9VT4pFR!Fu*@)E=afj(4i}BebH73f*nuc;qHQf-=v&r!>~9S0 zZVbH=;4Gg@b(|}07PhPOXufL*E;oFTvXrzwmsqYTt0ySBpF3r&-7`>^ALPApX*Woe|nwJ`MTgYsD-|(ur49ipMQ?VSoVE2{K1=4D9>naafzi2 z?sBCtx?aM)w>14wJiBZixQ6AoaFYgist{-J;^<#bz`>yNjep#S;qL8UC&zj;M*)aX)Ln7eObMLJJUh0)_GyTq#_tYN*5m7 zM?caa$hlP&25-Xs%ZLFh(+{AgiZmD>Lp{|2nFl&mch}G49pVg}xV*}y3*40+=`zrj zuZ|8M#5L0`zd=G$@%_~NB{B9ULrw(T%o678CWYF;MeuKb&uY)u1Le|tAZN`1WMw68LYq5=(2|V*80q3xR#9D z-M#N3&ku9UZgnhM$zFlHZpen3#f9NvTXNsst12wXv(OwB->bHbeULoKF4^0H^NNAY zK)9IJQB3t_$g`S&WLBNP`gNI;>RhM_Q9Kg-`gB5sq;Zr{F0R_=JZ>lK#eu94Iaj^X zaa|TVkW;G9)h)=u!7(RgW8GI5So*A;~9_<%8JW$QrH2JlsLh`ek zuAQh>eEBD4!!pwr5AtP28kq*GonQSz)P%%A)dSCTZacH3ic7C;2En)S9%W-|b^?Zz zT+F2Qr1I0t_I_0w{qXn3K1PmPk>~>_yf`cQzlH>D{cGE$VOpw!$uj81p$en4plY<{{Ncjkb+UC?e6$J?k?KnXBC zR@CbELkZu0kJ~%tk`mj-idjz4Coku|pOP7OYb>hVqezG0suOG_{|Ox~oaCwb&hGZC z9m0@tSY$$kXT1ZHGTT^%g=7e}_+$;6M>f7MMUl$}*4~+N2m&lf_)?6GrWy94O#ayZ zh;Vc;nG0#DqndUZ3h-;c$=T(Zz2s@e#aXhRdHcMOpew8v-Y;CyB@_ACgbFZ)|01_T-LDpMv@WNy%LEw$kbYZ0T+oP!4UdAMi;rr$+$z+^p6SBclu4{0?4n?9z|tlmDAVAsIDkQK zuF4#uR2~}dEcLmvLdQEcM|=alvkfG7NTU<9z?hfZGBNy>W+ z!5`Xyi4*qAuC45>UY`E&{uwdW+>4@tS(Dm8OczKc@sZhiRg%fqX$)rsYvn6_Q?u`h zz#{%!$gjmlbA_nPze-2K?;;Ug==&3P|IXq!ws(m?wQ$6Itiigwo>b0?O%A7_52qo zfo;(K&^N38yfR>Qw!AhD-Nfa+8>LUa-uIBntiuk}F5GvT?F^1P89FG+gpHyr2kH$p zS!e#xfi?H*kqNwp0BH*U%&qy^(sPk=wHF@gQ;Pzy%&m-+*=#YQ%}gSpXv>XUef%{e zL9A-PV@G4A`889AY*rdABxv4b)V_0SRnK2JN4r12VLTRpeJA2Z^QD7GIYBIpzCGtz znHI7p?Qc8wZJ2TDKf{1%aKpwzYbIp${LgWzv=nE`5$Nm6lS&a0=>ru3u3Cxj3YOg3 zUw;SBQNJq*Ma~nbyygmga?)9+0LgI>N9VrrajRgtrs_zs@l?t-JU_CVY5U;0L#^|j ziMw|L9S)NW7!%|F9t&-+4v`-ItWrRD*RpIhrTU8pWSzFmm{v|U<_jCP5TQ6Wb_UMK zyY~Pogu8v0_;TL>RM2|-;d+BLZ*zA1NZfj6qdImLLmglePqK*!%=r$6od5o)*ycst z0WVa3=Dm=Z@f;pvdbQ3E9%{gc7WU?@kJ`Ms|B$8`wtJnq=Z7i1l;~g9z@u4Svldz_ z?ly3oI@Hjtp4B6XCTY({UA%#i?EbbajwXp$2oaA@49jQd<+sA|=E-)3C~h8EbFkbn zsBX-fp$<(*9S!gx6}XLRAg)u7GMD#pPmcqVwFcvod6+XMtm79V@ry~7j^BqTy;LCi zm1EcElxrl+rMKq~-J~;^`lgH|f#x479eCZn%m#jgQ}vwa9jSr$#AI5`mC5WQgS2o<8&iqn!_@|=bHtgV<8Xh2L}^c6Y8ML+MxEv_1M?#emTk2(RN-5JrM4#+$)gK+@cB=c znXtNL9pH-@u62Xu-*k8Dbft{GVeg)wyP{WamEqL{&R}OK=YPJiGuG($tp79q9CKw2 zPm=3VZLGr_!fcoZjvs2B`p@yj8cTw6Eu}(5cMCa}9-s#I-p(ohBLW=(Bf*Qml}nBxVv9^K4w{~l^;h@ghZ{0-)EVU?@T zr*3N)V1Ycv)Ou;VRl4?<dUux@m`zFt(=D^DF4_KXFs8Hm;Gso@DVY$Dm_e8; zz>CgfmA8@ha7jHnLXxzxAL9dTKj60z0Giq|8$Z2v0IE! zvZSSlh=CY$$qXRoe9}(CW}V%BXy?RvrrTtHSf0KPCG#OGikA|ZFeJmc@Qwt5>&7uQ zQlwgA2)ZF$XU}@?_TAzxNzSKPqJ@!%{$IZ)<_3O$GZr~~&p4Djrz;VRJzi7j5*I!rg_aR0n%+GTtauKc82k3Uza6w#+tP= z&IaiAWheg}IX5?8eKs<)77=nBNvKQlJD^phwe`$Qt_Nj9+jO=#nm!8u>dea(8=q3ervYvU33;*R6$yb2bi*ioyij&a`bbv!!W zGB~YZ*J^5OqhJe?yBrt&u%}XxbxRy?m357*5qWzs6DX6gPvE_KOmIGbGuqW+;AxtG zdmXVbLYzty!x*1#APP*Do>x>6>#+l^v{Q**;DSB)MxuxQmHXpf*vlKwk7}7~Fw@H{ z=%+*vJyYRL6Ix?ivpcfdK4wd>&Yvv>Sqm9*q=aSdB=*~YCXU945GLiqGm<;VK=R|+ zrsCN8`Vu|t>6%UoWfw0HjCP9IudWOK92q-=D+aGZEG+JsNzN~ReL#}{n~j>k)DqYj zF#D1lmPr-OJX`CS0bIa%()|r^s@{ z-^T5K;M1a2*Uu-9VF+Pc&FztA7~|k377WoV6m(u4-ZzlHQM2CFw^_e>_JKC+w{NjJu z{9`vZRwT#2;omR#tg3*pA>Itx$Tu> zRdPGwj?l&Hb0g?8tYTE+_~I;!ppLb_`D(Lj`Xuc%{c;60%A%Wg&o5@t?g7G`lDHzB5&jL9rtY8Ifk!3Q#| z3OrZkpkM@93THre+n%c?8@|#&<$XQTy<$88LLnXdTjSDYp1+{xN=d471$E;G2XD1` zScb|rA;q$JEc(;G*PSiJ(AX!-QVF_&011y%J6J4v{6!IC%z2G?)6=sYH@Y>N(q(1_ z*z<(8T^^|obYm>&{WCE~LazRZQ@%T1J92Sc>3?KAY`qi81W#%KjDsCj?Sb0FgjTZTnub*; z*g!*PQjo)TRY``{o)$^XEiFE~Cs5#S7>%W!HLc#;kW}Ct;SlL-Uy-jQ zwO!eOU>@GO)ntH^SGubiw`1yI+-lHW(BGnA#zKFI(zK!~D$?sPyYdn1o6oQh<_jE( ziEe+NHy5GkdTurQFk7A$AOtqh{XAQglyn=8B?x$cx2Q&IYOvZNc=5|}AcAuv{p$I1 z&s<@SNh@kgX+Nu1u=Z}*rH`ksx)T4+d!zzeHDh5+zvj#yq49sqJ$)G@i{k2WQcCZO zZfomX5u>^ddu-K|?R^H!wBz6Di1~oD66qbID`LB8WilFH@jC}?<-Gl_um=jNyc*gb zmwe)_T=tmR8hLNLZXJzt1;+5!lw8#EyI!)q9d`~6zBDDdJ@jgH{h4W>sDF9GRXYP5aw>sE(%JeHU+dkL>$z_hC;nG^?-|te_Vs;pioI}90YL!^f^_N96{LjTLJ3VlS}37~ zPOt$=??@L3Eujd6-b9*oDM{#vp(UY1C<%c(p8vt?T-V(5%yZ8?GtZm*6~lzz-kbH? zS$plhKkK`CS;qPE;Q>r3hbk0~U6!Tf7RWUKCcisl(m}#HjNxZUm>}s{K^#!od{NX{ z6oHH^&6~Oy_I&$LKWVOW&0U`S`1No@yxb0#)?>qK zx{v<~sN?Abt1ECYP0-K&C$~=q!nhfF1FmHN$Y?Ms@2L5z}__|UFt90PY;Ye zArETl`8%&OrT#HoVnRJsbR@}%>QWZHyrNH^968W^&U}Wjyu~KEtCdzKVI*L6_O{qB z$f5Sap9PQnt-bL$EH|;(CS$*qBh=9rvozXdxr-|Km3wt7v-dsb6fMUj7>pgAW7R{x z2X|;0uhT~_hrGiXzA1Zj;;iVV7-{&gg?kEQrjixY7_YK_ns~sVK%}15fkX9v{S`i| z?~D?+|DY8sM&D?J#VtC(lgB5S6z2a1E9g@Goucx2 z!B&&_`wJN;c?XXYJh2uXUCi|e;Z8V|;d9fsmf&OBC$-$CFR>>6*aakT{jCe=y)yPk zsoMFoXLT{frs#5;t}`&_lYz7UNxQ1JLf%^s9{F@m4WjR##jTYF=Qph=wChiEG)zK) z(Y!P<#CIDfi8C8&)=>s@YG(v38{g;W6mg=9O%0fZEHy`d-R{&{(M~^av7NneL4$aO zQ%)9o(wAG=|FnVc{F?!e4EH@iD6?1s08z)E(7O7t>0cKPnyQAD0OR?W={ z((}~02ZuU-mW%tVQdh|5KyiE`us|S8iIJ5RG0hixAKR+4^JUVBF&%U2Gt4)29EHe!Dk^j%xf#~u^jf6?l}Lu$%=i$ z(O8xnUYPkrV0`(pPNf3A;8}05aRxkcCTV9NB^&X0h(A0g<0CnV`TqRKl?h=Sb7the zmUlK6*}w0M|8TF?5`83^PcF=9cENe8-%7jx^+3S_{$yYqpCA0V7L&NKVX!5{cpvE* zQ4`M8opX1RzTg?~b5lamQzda8KQ*dZOEE)2iql@){SNh>?`r~(n08JmWto|@Tm6N# zE-<|sGW(&tX5> zy-~H_RV|IOt0~Q2ICo<8jTf}D*0?KdK|rJQ)p{2RGp>$_RYyYxH;Aco2MIICw*w4o zlzySk!{ zsB23hKYMlh%5QK5CNoR9kp(un5#@MmDB&}!*8aRj72!FyC$E)!mmJR&XsDEa#x#S{ z-U;Zi<~&gM=T9g$>V*b%c1KaiJ zIOB}L3RKR13J#p_rwn{|O=!5MA}p-3r9QQ5zvGurIPqh(f94Z9?&dwmRd3DghlG|K zY3LU#E(Pb8w-^~x^8JSE#lC0NZc)rhtNV@e7H(?6>FqA(gwKfk?c7NnPBOf&aT^WR zS99Wu#K!f$yL2S|01)IK?cUs+8tKPZ=zNrrCKHftea}L!(r9Gy&_8?d#Bsd{`=gbI zohLQCxv>1Mlfwev^`Bfh+5c?&p2@a0?A{jNXzg$-9#(P_s6ruL-$G%zeYcX2`qD zgAQ~p?U5)cjf^jqjjcSHR<~9k0EO74l+y!CleFY@h$1g2?TGmP(_m2LOG%e!-Zz#hqw+wpF}ZP9|00qzalN%ojgt$*sjCQkbhGf^IE@2E!LX&wZ8? zcKG(;T8&Pc*iXgSVVZkocT;|MSF6&l9}&$aBfnvD((}eGT4D2(hUnbDELgd~x=zld1EE)`@eEUc3h>q?NnU)~#g$K{`294#s zL>QMLV`Iq(qnyYdqhR2TfuH`a=&#%o8~;Mfru_5gigN4~$&S-SiNpa*RGg)YFY@wJivlwSWiy5Tdg4B!fjFJ>I3fU3Oon2M@iF`u<7Ke2DQZEB9f z$FiuNNc7Qy6O=dUwOE;o>u8T0^SW7gcc$e==tN8(ytQ%(yf!6H{)l-dH)&lMt?<2X z58TdM5&!Bwi_8zKRP!Tv>FXx9gIYaK(9bSF^PSe4bJ!pH9C6S1ha* zmTanY!VE=E`{?HsXAx>13QsluC9OP&Da#3UnLxNlg@bp;Ra6;ONw(9!^UgTO59G&= zvr+_I#sVJy_VQ@Jg(WDw@U0FCHtzk9(Vin$T#1!e=jqiTF=)!sl#G!;kAJV|r+1bP zdy(PuJs*pkmC=pI&-+=?#!RO+0p7rGvE!cNE)zGS*6~GY&661H89r%*z|3e z$3B?J>{-UC?mOy?zInnskXTR}Q}K}su(DlxvTMdV(Miz@K41>hWY5Y1 z+z?pX+6>g>ZoBImu+|AvoZMX;-HTXSZt~XQwJ%x{`xNAO&twZpEvQ5Zt)Q5i_0;&n z=!bK@2sus>tVom9&(1%5s6X3BX&WCzzXgKga&oeE)yY?%cAGyD3TjYP)#kvyofu-> z+(ey`4|;H7h)1B?50$0ob3&Dj9J(5BpXY(;ZHKnxkURb`Eb7&%F&{y7BAb8%!g8f$ zWi_Rp?aS+ir7!)POq9FE7@PM#30?;zbCW>pY0-PnB7?UGvtGOV=Yufvj~cjkMqg}lLx$yzD2u^411^&YVuTO^TVuBNW9(q` zqy62?G!x@f;b^_=XO(5?BKt;D#YU#n*0wQP&FIpxu|G{+s?*o)tx<=$68?@`kRG=P z?{~!&_YZzM8b9iE^#2uqn$s|=;U;HTmDtlQO;VXPdKVg??`$(Guh!o%U`*BFcABEq zTJaqW_ks3(#wZZJkPz*K~z%e3rvuS{jS^~VI2;GPWQ=R1rG+uKce z*k&A&6tRaS7_7aQXL~5vK{7Dl%lmIVcmH%C-_}VJKB~v%Xfs%|Q60CHj998cP6M=4 zM(y6A!SPq__L-GT6XhpQc+#zC_=^G{J^Avtt7q&SsGIwfyN=~ax~os=L)he3ro6?s z-tK1a8m4Z&l$;%`DW4&Xm_q`GlwwK_O|pd!_|Qw6Iy_nxAxz3qrZnf~CnYMy>l6{iu;zhEk z?3cQGl?7peD*;u3)26gMm^p^zY(NV{t=tULQvBzrC=bPE3sU6$bio7QE z(KA^UI?0;uAc??OOmT2yj;$8 z%^X$g3=A@2uA|PGwbyg}a+|Rj0|tHo4w4Ifo zX@Nc!K6b|P)Nt-t&}@-i>&*fCE>LmVWzP2~!3%qza{z<9!4)kRURp5GcA5W-A-43d zj60c9h-W40-83&GdQxjmz&;e;C>>bV557qO4jXAgFRwZuxx7fj` zp&-nCrY!+U(3k~*c7-XVqnifX!RRt(@*k_eP^B#! z?B^`yVdu|YG|cm#761TLr`zmJ()oqlKBQH=bb!Z2V*{oK}qe17rq6gQD zLRUkH*upr2<=}^ylwu4{L(WRVsV7R{;F{rlImM`#`g|o0q)?un zwJJ)?ioQR!J#sB1WjX&PM`N*lBO8_r+??NM(}N9b%>h81zJ-pw6KZ6|<8lNNaa@&u zP!5=lB#5umq#v#-mmRB7yVz@Wnnd+ZeEy*s?NDUi)@xq)+|Ui$wjzy}@%k+e9i1iV zkEb-Sdb957896hUA&VWuM?9Czj}!pIsa4d(uKtgar!0qFz6s_GHsX=@f6aA?kdLrE z|FzA|;PUZP606VmYJ*OdlIvyTszTh&pArq*Ft)Bk(%Ca6)jI2jJ@0=s#Ee^41&?1R zIq#xuq6|4sh3$;g8+5;(6-^_nTm=dkXu_H;w?2)}+g0-DJoT$u+fInK(3Y*?o5KOS z@8)_=kMX}`6F+Q_9IZb}*xDw=)8eGx^UR$37Avu6w|jxFPq$2|>71I6?@va}I1jy` z0}{(MJ3v^^=!@dJxoHnJOo2SfX6zE_A^~K4k-v}wSqQu5XR!E!OM3$8pQ)ILMdnLO z@GSe@)s9fn!EOf6PnT2VjdF?s#2@mDWUInl2S$% z(7tKmEAA22466&lf$ov^x-*=JTb&Lbd+`-qW-Zuh1(QFRj~8lT={x!hiXJ|d_ah*{ zD240U1PWz6V<^!W(c3j*kf+oJ6Y#ZsrR#NDnDumxe{*X>&w7%nL>F{b?CIR4RoZq$1A;;p z|8#p*oHLWJ@_Lc|_Iywt>pV->4S4t{TmCFA3NFfOE}Fp4gp`hw@$2f6EMQkNLX3GRZnNvJ#`?WUEh&<^v5oaLl;7WtamSp+E&&{t?2+pAX~hZOW$r+!azdynJj@f zQbU3{cT?T`a`EPOBDUqR>*C$Tvq9eu>uuv5ji#pkTxLK!1>4u%ySk*Zn<9^<`(B}~ z-m6QAJfti)+#hIHEfLGq@MQ&X=~Tt^ml_oGC+n^DT}^J6SXEu~+425zSW_Ld%>ohe znuRW1^$E-<+JBRM;D!#7X>LiWxg>T<)I`5lk|#p$5I9_w(6B8E72VXUETaTss9v)I zle71S5BHtjm*i4WM0!taqV7@^|B&6sgI+z4FAKnYEf{j!hw+S<9sU z4h?(H)-{l7@GPuu*6~Mz{hv`p9S}G6+^}yi+WQxe+lOhzm{h&L?3>&2zq-? zqpwvnaP+k34l-#mup)B#^IPeGr#yaNH}+n+4y?|g!`>^zFR9Z6KQy}Gv4YQ9{AwCi z^|cIK*#!Gn48NJ$s~o%9VO_o;gVotH7M-MrSPC=JjvGZE^jzByJr~tdtqM;S^(~h2 z62?F5{sT4t*kNU^ETgr^LvFozm4Jso)lEgdFN%)!9Ek<Lsq zARuRg8-zRGd-gWSRNIPNg50Q3PS2KR61ENC3{m5H)`DkDymsAcFH31l*T7PszaEJ# zeLdYrU|*i9Su2=?tcaRYJFjndMcQXBeYXgz6>Zmm1@RmC;E2r6K_v39W`-Pk=Y9%y z$Ksm3_vNC+T1cv9S9$bjg=Y!)d5CCyWI9CuRv`FjJNl9MCd+u-%8U!Ee!r~cZTvxV z0wtiJ)iH3i2kth!o5O9>7K?Inwz6&ke*#aT}D)0Z4(?%@)& zqCf9PE5vwj8>qHCuVQm|wc1eUS@P6v+}^rD=^p;dPmyW8?%5uwkEYZM$(j2^i5Lry z94|ErFK$)8%y(`Yif)lp?Q2-bb~{}FDGk?<-ZW(Ao7=j)t=L`60|Vgff=B)dRp@HE z44ZuaY{z#kZFPgo@2fs?397xXIZn;ePnQfH4()(^o!v}q@H4_VH8~yGNPm0YG`8wv z9C|N1urUF1KdMP9xO|Z?&9=_f7#KLen(Q0DS?=9|pE%5#IgiHzUz)}XzUZ*?Jc5zD zJuhhVe8&|Wnu>BZKk&AY=@?Os$1sw1NnY%OBW=P}pWf*{fARy>-oWJStr?L4kfH|O z14yJdTrtjzGFYBztI1g02e(^OYezrQO2REqs8?4c>J~)nNE`c1i9%2kSsJTklo7BH zG+ejd3=Qzwl@J}0XV^iVx7P~%G_Yb(M&;48a1t>@*G{)MbYjbpHkH-n<;g)q(eFAX z77T><>SzOb`DiEG?t(i9(=@df24a=BP9rCeajrbrYHhQ9TV7tNt$$VIsDQO!3#Mp_W&$SL8K>kfUVg6#oCRO zHYlg;5i1tWNxVzi}hh z?)m5I{M}N`ul30-i*ER0w#z6t$@8mS<^9Tnm_S+4*>42!yvil2+k3l`@sg{0L*WU) zm~oq7XSqil972G(cU2uvh=;ZSQ%GH}h4AjxG=)Q(AQ}4v#Wroe&Mbh$M%%FPWHsX1vD+j$6>5QmPbfPO}tpo$9NRh-Avo%x<+ z9M9gx=+nCn%3arahzj1H#u7X3I;Q(&Jv>oOE0qL-ec&57J(rK)^d1Z~@%owN;-X^5mW8|(`UQv+rH2zRg8=T1PY%iTL!R;aa{3 ztZ_azUiM$T*svoM!<0MT#L5FdrU#Xid`^)pKP2eIowqtlK5th``IZ-0!wlq6Jq>W5 z5VOgOwved*OWI5ULN$m_Y?wfTP087_BVXYY>q5h{;%=zDjb|YtAw2y2nlha=nydh_ z&f*Idov9)ML=a2+{AjZh_%k{YlPwFmxlrJ@{AU2YT4VNPT>QRN!X$T&ob#;y4YZ;RbDr2`T4X%!ImJAME-z-1zS$mgTdCV18nx#NHPCp7u zieExU>nvx)O4YUy(;X~tyH~J{_?D@U&|*x`xKegM702Y_Ixr^bGiIBDt6~ht*Gnu!8Xce&wsoIvWTG5M?&RvD=`mcln}uY~^VBdH5+>ffm|3 z!cyVa04a>^?&aGcSu5L4Cir=(SP$KHg1`_6L zzm5CnwCPd_@3XL7OA{M?4=p0KSzq2{bMcAh9Y{*Mow4`X=HNo_q_99``D9Z663v4Q zVvWZI^aWcEE2IbZ6z4aC87B&MO-utDlSYKue7v5&8NlDx$@s^S+{(Q5Fht`)d3m__ zyE(;IvIgQkJD{^3=&Xk!#3s9PxspNE#==+8darMU?Tyx?A2$PtqS810wOQ7@$^&-4 z1kus4z<#m-U!PmAcL6v_rEHaUj!Sv|pe^h@i^#XS&qrxL&Y}<38;X1@o^EzaGm_pY z$NxYDOk*ZXxzi4>BVlQ|d^%P7t1l9^&3?Ll8FGh=8=XUt3n0#;KxyMA_l3fKf7 zO-3^vI?Zac?aK(Cy@;3u9lL9|sN+irRT0uzEz58o$9@2tl)Gj8JZ(FK*02hObz}tr zE^XaggU+=x_|*{dSYxb?mng8A**D8T|7s6l)yl9ou*V3y|GI`A9^rGWzmD||e0&?6 zbGlLfAphBqj;m*OS~LN7gEU4$`@UVqdDGbIzR0$18Id%qowkUPR|wq zk?(DGoOdisX1#?wyt9C;%lM3UgEOfvqE#X*OxUkd@bS_&&qWPr?*a3n$Deo5CLX>& zLd&^x17o*O?oX#^a=`g#Mzp+D@{6xb0}PQiMbU#8EvJ`=Sv znmjJWgcOp<8w-2l;6vXi4h40qAX27ERCaDu*nrgJB6!LQBF=jYAlnb)&&>Pf5QVaI{nov9Dj;fSisb@;csl7sf66`)~oq1zy%hA{d2nKL-$Z57L#)|y|u;u2kklB*c z;xKNY#c|WS;jY(-ObR^wx8Wp*ZG)OCH2Qugh9d$!ndm4wbeI&ol{M$oB6g|g&+9%& z^m9@-qz8?6kf-dJ8a#O2Q5PH~nIZ$4?9akO($YFSQZ?ht{pKZgFhL#s6l&~2g}YwD zbiCKp&6m@CJ(Zhw+Z&-=5W5J^0H*9)+eaT244_8Fxr+UV>$498h(NKBAUQ3rZj)jo z96n+88ht4^&1cQF`MS&`jl|lV*A}|*RDIu0+k5RmDQE$%|U2B|kKLjqau=bNs$a3fvb>hc59mi(eadiOPieEZFPoc&1g{?+c%@&+D^ zSELjphikKoDgSt#y@=HB+2R+D-%2he?}_r6%tK|TZJM`dIL$9hdY*xbifeGs z(5Hvh@aXc6iLD&IGu@?h+$bGfaN$UMWy%le5||j~m%0&bkv`UDqZK7<3VB$azGPrG zU&_a|w*SyzNbdEvB{Saw#ABkk(;6yS7hxo9pt;PJbi3|Kd21j^U&&y2w%3Gh z$?7ABPt5WwI4n7pl z_sJ#RPYNE-vqE$%HbQpK8Mh~vV0s$Gy>q3qV-L-_Q?p#M-C@x#><3z#ThLOZj6C@D z!3XLHS9U@F!6B5RuGR0TNX7^?E>V*0Xp-|{J8eQX{Cz#N*W5?$d{wHi>fVXEym@NJ z^|l`lbZh#>)sYT4TusdQvUI9)dD3X1May!?4;~#_0dtFU9A)6g)2M~OM*g}i^uV!} z;kcPrxzpuBAAC4#^E#wk-$coq-Eg*og~Pn{B<%S9(i&k&=Of@jT_R8pSP!bRa$2rL zM%L}xU^i{cik_a1xOO8qfq}jY6xr$8(gt<1v27tae>-{nRwhk_+0Cc#w;(csc_NT} zE~If|kfd6&MIyzc?{tf*>735*cNj^8qpE+&Jh)l($(;MXX0pXoYv}4Y%RtK}ew0*! zGOnW&VhM)=-C~D#g}UZ?vx3w4 zCYE>dylAjn;YK~(`70``UR~yL2wpSNf(hB+ac~Bq8Nx-tP?SRwyW{Lg9U-tXR-KlQ z{r0%bNq)>BfjrGi9jp2lCG*~bpod6SS&z}K9lm7bnnVLvFC-p?gjql#;w`w6FnTwqR&jg7_Rofw?1 z_RoLd!RhdK?}k3V{G-ZY=uSs_I~*$IL(^8>CGU*rEYK7b{{6_spU9xJ*Ybys2<8GP z%bsR~_o|r3{8yCV3F$)j+4d(H_I+XD9w1E`ir_UYjO|1p2AI{j0%?CdKR1_y>xwCW zO8pv#;Op<#V%|8lI+~c6uo`>p{r>y!oB%yNJxB9j4CM==n+EY@Ye0MORtZhOngME= zP;V3)Jd~Ce_)xwi;^aNjeO8L5wFu|qW_IW)N3iY;?TP1+3vpn*%v zC^>(1w8BEv)?7w!Qt+O@f=rpuCvfvl$P69>ied!<-Q1L#6o~IR@q})7gk@o!JY`O+ z%3)Nk3XzBvWm9pmLdEsnZX#)#HpzRvSL96`zM#OK;AJV(*^*CQ(BOIa&?N8G4Z?JH zz%@QcV!gIsBB$%pUXpPo8{9p+QQ(!}0=8Hb)0 z%(F{%HfvpFt}6=KzGb`N2v2?)AZEL1xXZxlrZNHVB3t1YFuDFn7lT#;2@NcAhGiEGt!WvxPnsioZrfb>CQ~ zHJeS=K(L+N0w^kVjkSCglH`JZqO+2UtYSD9{bko-hHzFKKc#CrQ1;EqB8<{NB$OImr7Li1w9&=S^d7(uh;)p-44HvD9oXZ^{f3;C%JHZmJjk|e z2mqzhbdW2UU5hR+2!W@E@Aeqh#<8AZAO3R8r{l2GM#92NN#nh3 z<0M5}`=n9Jp9L^P*hCWIk%TcaGsv|1q2-itaG=V#&7c_~~-Ic=8k z_9-0k*Zh4k`m~(};+3}uew+W~5As;4r6nX}Dr{Slz9pU`G7=q_Jr^UleXa&r3NwrJ zPT%$X5n6kl%9tP~E6%mp$iDp*SsaZ8%u0OfX=}4cv9Znw0xXf1vW*k(mKAVzm$_bG zq}2gTkAYw!lbEU;&1j)MKGR@4+zYcJM?pbmtFqh3wwMchxQbu1@Vf*pZ9~me#=W9g zT+vWAVxfN3HcbN4k#`j(!0X;TZIGVtIKJ6l|242%;y(wLirRn0T2&P9OqhCtIUd!^ zoiY=*b+>+C3RjV0BSSoamj&|(5OzakYc?V|1&50*LiH@9BDJAN-iNiRM*hvk$Gd_F zKQ?9gnsVK#K_rXxaGEZhQOOb$qG!owv}v@;y=VbBlGcrlTdMaP#e8y? z+i8J-?%%(!_W1Foosl9BFjEF`#_AU|N;j2EW5Zo%FZ$N{`S}6jztDt*CcRCM&$ALF z4148~UTp!!+S*2K_ArdoqP-4&lUk;q{Ai9+*kw?ouRAEkeGuh@)xN75tx5b{o?pNH&8Nd{+*O$fxD+s-ee$g+ z`Rmz;pva&Wm8O};E{X(Vhx-}!dh)L)hJUAR%kq$M$em|lrxBJ-r9TN2P>2Yx85QXA zFom`<$?2wsi|HTB3nVJ!-b)H-2kW&gp&rhjNt0M*W0d0%Nd3dV4AGK>Z<)JYc#;-J zcgux#hfftgv!jA5mDczBlw4wwU+0B8XG~VDnvDwd>t3y+zea)T zmVN~B9ppT_?9dwSwy?KN%iTs<5-m(CeEA*}!qxxu&x1mL4cjr4MiBl-zIIF_srO6| zVTzCwI^A&p8=w1gT+&7nVVnG1`TZzoK{;r*gUDL5FcpN@KdKm8doH%*r>xS&_qspX zIlX2gxbjh3Lvk%P4^ThyFRPrUS%poKp1!Vuiimq%Kp0zqKOL7S{{)y%y@h}W= z`*NOb@3fv7E-I2*Xm@c9u4pQUhHNqdhC{yb_*cU&i93%NML8K7Rt`!?BOW4zh)lYh zv?4|lA$_}jyV;{h0HL2q#RZl6E6w_^vyg}tQzs{U-JhAW{{u_h(znx5ph;WiTC)XR zr|3lm(7Uqy|yBOB^L!mdS^_VpYgkOui?{(Tu&2;zjQd|f>`g{ShxVo7ZUP*QPhCotGo~ps5AA1Vkr!9Xyl%Bx0A--Y3yo`t zF`R0Ds)UJnI1qioCEH`yzMRpB2wMf-2{_%AESfg9F3=&KTBtly;Mhul7>a8YR|c&J zsMPtNQ9$k{d2l1ElKopul-Z*nr>^B?jeOqiu%#i2)6Ev}y9IYq_=H>iH$>E&2JYQ= zj~X`eU&4aZ>itvmwbDNb(@J?)hdLB5HjY15unc^>ouY|t%)4qhH}# zqwPg8{en#+^)aQWw8DFy1x`aKcIn;mO4jCub;HqLj%UN|&i@NkQc%PC#S)QNf)o}e zQ>-q%i3EB@TFeE|b4OR8WQ(x=Sg%H7lRr|z@ZU3{d)%2#bJF{b{hCBHDhLX|*T7|U z_`s7JEEF*H!L}rNWz_VDC(u7CD8Wwf;>bt|k*DEadDeG*sVc}c=9=o~8FeNdtA#%2 zxI`T&kqMjk){W}INr@f@dGSArBFx8e+V1ca#UC-Fo>`2!5dUj7nEajA5#04REa{QZ z%*xoTKYMWnxyMa|CBbO1a;N5N6J6lhLk6-(5A7J%zegZ20xojA^&es~Xg06(d~r$M zWjRa3GUoK6M~f6u#IQBg=Ljag^+wyWFaII2FOqp|)8As~H4kA}0+vm6R;lP8S zH7yr+k$PX62%Y&w?Aw${M{m0bxJhPztd-7~s};wIuld{-PV+UE5-lLs2Y&g2nSmft z%xaweS?$%yGev&bB=Ykv)+(AZ88fpt7QVV09P~F-se2%pCgFl=<`~v9un#PxX#vmd z=_Pl%rqBQ$R0RTd+)YNb+VFU}?TMQdGx2ybkZ#BTwLHv&zqsch4fOuidsw$v+!`&B zrVQIGLyjq#+1jq^nHF7$i!M;~Ax8Sds!u(xNc}6I(#M*Sfg#Ts1{^Ip=5zr`KhnAR zK8MtWGZ^0pBfHtbYIA+6!+W4fybrY$rdy)Xj7O$`fBD_=ZI(lKLf?TC`E z&#uP?-d*MV)!b-=o@EIeDURu8!~E2sYs(oWEwcgZbRRH3rUYj8;vRnwd)vpxE zefo+e?5w&6k@eRGOXATDPBx+DJrJw1)<2l>#p;Eyur29_h;nr!CxozBDX(V|yFc<} zdb6{x8V%|hnl#w$j_1+t4W%C@8;mE;RvWHnIsUy+Y({8|y0z<~17x8b1^R51B0R2l zwDyspoM}^o%4)dY;+wl<@b_zM@{Nxa_|J_Td^%v2a!L7^yXVxi+#&7BqwlFIFvNo^ z8Mq76P5GTTTKm-XC-7zw=BnqcSlbEP$-HdkVO-gouroI@=bxv~PGi@Mqx@aO?0QXE zD0*7H0r1P_weSV2z0or`34F0sF5}gip~OtR@G)r>j>t*473K)u(UOVYEV(M~(rapQ z)}j!ycoUQNmX!98YTPm2gO5m^MQk0c8w&df(z>?1G}(+SDY(KpT`GUY@WkL7ZWY&U z)cz~Rp{o^B`EOYl{}wg=KjIHhdZ7EiB - - - - diff --git a/common/web_controller/package.xml b/common/web_controller/package.xml deleted file mode 100644 index b1cdadf43238e..0000000000000 --- a/common/web_controller/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - web_controller - 0.1.0 - The web_controller package - Yukihiro Saito - Apache License 2.0 - - ament_cmake - - autoware_cmake - - autoware_auto_system_msgs - rosbridge_server - std_msgs - tier4_control_msgs - tier4_planning_msgs - - - ament_cmake - - diff --git a/common/web_controller/www/web_controller/css/web_controller_style.css b/common/web_controller/www/web_controller/css/web_controller_style.css deleted file mode 100644 index e103a66d694ce..0000000000000 --- a/common/web_controller/www/web_controller/css/web_controller_style.css +++ /dev/null @@ -1,62 +0,0 @@ -.menu { - position: absolute; - float: left; - left: 3%; - top: 50pt; -} - -.time { - font-size: small; -} - -.contents { - position: absolute; - left: 200pt; - right: 10%; - float: right; -} - -* { - margin: 0; - padding: 0; -} - -#wrapper { - width: 80%; - margin: 0 auto 0 20px; -} - -h1 { - height: auto; - margin: 40px 0 10px 20px; - padding: 0; - color: #444444; - font-size: 30px; - font-weight: bold; - border-top: 0; - line-height: 1.5; -} - -h2 { - margin: 60px 0 10px 0; - padding: 0 0 0 15px; - font-size: 26px; - font-weight: bold; - border-width: 0 0 0 4px; - border-style: solid; - border-color: #0f5296; -} - -h3 { - padding: 20px 0 5px 0px; - font-size: 22px; - font-weight: bold; - border-width: 0 0 1px 0; - border-style: solid; - border-color: #e6e6e6; -} - -form { - display: inline-block; - margin: 0 5px; -} diff --git a/common/web_controller/www/web_controller/icon/autoware.ico b/common/web_controller/www/web_controller/icon/autoware.ico deleted file mode 100644 index ed943dd488e1ed0beff7fc95df3313cebb361a77..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1150 zcmaLXTS!xJ90%~5C()gp#Rf_jj9y}Ned(d-$!({^Y2;)TlOjnl#6(gmyp;23 z8E9lCMLw8N5^7MaFj7)POel;Lv#T$Xz6JjJ{&qt~_8*_~|NnoN|IQiPhzxis3gK#_ zLL-rxh_(Ylh=k&ph>%N43<>T-m-XTG2FzO^2~b6AAQjeu4J59@W{})kko-wVhGHlO z4>%zkF2O};fC?xB9W=NAx8MYvhJ4rq(pLwkK;p7@!13^2&MSd)AZvNtgCO@TV-?6V z$!vh-Lt?@dp5vA$!|P3$3&0n{i@oIjH{|`U0I%R0;<5LHP;~6ZY8F=N|B9SFS{+_! znnqr)H{N72bwqmYb~{&ARk77-4W5x z=~tAE-21GD7k^H1O?5SAW@hro_oF=bWs>tf9;tD}#f3h)dZ$WfEtD8&E$tf|c*Pb= z8vA^OJUP_Q-Ip~UA9>4NkGpxarRb!Q5X^|tZi{2V_jIm|b+(|97##rN9p-$B30Xf%>0I?rq+-KCbb zcy=r`yRF>lGIOJ*vW{7ICI{EpFEu#L<-AZw75i2Pdm|Ub$?r6$>aLWbMlIAU{W0D( zxKs;7+M;t^mYJF~Rqi22*VNB26JrWFchfPIYEk>IaUIVA@pzsL*cKy0a#FM6C?J~0 Wzrb{SNC3YSQHgJ?kaZ*{b>c7FtO^nU diff --git a/common/web_controller/www/web_controller/index.html b/common/web_controller/www/web_controller/index.html deleted file mode 100644 index 110481ae298fe..0000000000000 --- a/common/web_controller/www/web_controller/index.html +++ /dev/null @@ -1,105 +0,0 @@ - - - - - Autoware WEB Controller - - - - - - - - - - - - - - - - - -

-
-

Status：

-
-

Autoware

- -

Autoware State : -

- - -

Autoware Engage : -

-
-
-
-
-
-
-
- Status: - - -
-

Vehicle Engage

-
-
-
-
-
-
-
- Status: - - -
-
- Control Mode: - - -
-

Velocity Limit : -

-
- -
-
-
- Status: - - - km/h -
-

Path Change Approval : -

-
-
-
-
- Status: - - -
-
-
-

Vehicle

-
-
-
- - - diff --git a/common/web_controller/www/web_controller/js/autoware_engage.js b/common/web_controller/www/web_controller/js/autoware_engage.js deleted file mode 100644 index dda18ac322313..0000000000000 --- a/common/web_controller/www/web_controller/js/autoware_engage.js +++ /dev/null @@ -1,94 +0,0 @@ -if (!AutowareEngagePublisher) { - var AutowareEngagePublisher = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("autoware_engage_info").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("autoware_engage_info").innerHTML = "Connected"; - }); - this.ros.on("close", function (error) { - document.getElementById("autoware_engage_info").innerHTML = "Closed"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - }, - send: function (value) { - var pub = new ROSLIB.Topic({ - ros: this.ros, - name: "/autoware/engage", - messageType: "autoware_auto_vehicle_msgs/Engage", - latch: "true", - }); - - if (value == "Engage") { - var str = new ROSLIB.Message({ - stamp: { - sec: 0, - nanosec: 0, - }, - engage: true, - }); - pub.publish(str); - } else { - var str = new ROSLIB.Message({ - stamp: { - sec: 0, - nanosec: 0, - }, - engage: false, - }); - pub.publish(str); - } - }, - }; - AutowareEngagePublisher.init(); - - window.onload = function () {}; - window.onunload = function () { - AutowareEngagePublisher.ros.close(); - }; -} -if (!AutowareEngageStatusSubscriber) { - var AutowareEngageStatusSubscriber = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("state").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("state").innerHTML = "Connect"; - }); - this.ros.on("close", function (error) { - document.getElementById("state").innerHTML = "Close"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - - var sub = new ROSLIB.Topic({ - ros: this.ros, - name: "/autoware/engage", - messageType: "autoware_auto_vehicle_msgs/Engage", - }); - sub.subscribe(function (message) { - const div = document.getElementById("autoware_engage_status"); - if (div.hasChildNodes()) { - div.removeChild(div.firstChild); - } - var res = message.engage; - var el = document.createElement("span"); - el.innerHTML = res; - document.getElementById("autoware_engage_status").appendChild(el); - }); - }, - }; - AutowareEngageStatusSubscriber.init(); - - window.onload = function () {}; - window.onunload = function () { - AutowareEngageStatusSubscriber.ros.close(); - }; -} diff --git a/common/web_controller/www/web_controller/js/autoware_state.js b/common/web_controller/www/web_controller/js/autoware_state.js deleted file mode 100644 index 8c9a0ef05d33e..0000000000000 --- a/common/web_controller/www/web_controller/js/autoware_state.js +++ /dev/null @@ -1,45 +0,0 @@ -if (!AutowareStateSubscriber) { - var AutowareStateSubscriber = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.connect("ws://" + location.hostname + ":9090"); - - var sub = new ROSLIB.Topic({ - ros: this.ros, - name: "/autoware/state", - messageType: "autoware_auto_system_msgs/AutowareState", - }); - - sub.subscribe(function (message) { - var res = ""; - if (message.state == 1) { - res = "INITIALIZING"; - } else if (message.state == 2) { - res = "WAITING_FOR_ROUTE"; - } else if (message.state == 3) { - res = "PLANNING"; - } else if (message.state == 4) { - res = "WAITING_FOR_ENGAGE"; - } else if (message.state == 5) { - res = "DRIVING"; - } else if (message.state == 6) { - res = "ARRIVED_GOAL"; - } else if (message.state == 7) { - res = "FINALIZING"; - } else { - res = "Undefined"; - } - - document.getElementById("autoware_state").innerHTML = res; - }); - }, - }; - AutowareStateSubscriber.init(); - - window.onload = function () {}; - window.onunload = function () { - AutowareStateSubscriber.ros.close(); - }; -} diff --git a/common/web_controller/www/web_controller/js/lane_change_approval.js b/common/web_controller/www/web_controller/js/lane_change_approval.js deleted file mode 100644 index 413d893bb7a43..0000000000000 --- a/common/web_controller/www/web_controller/js/lane_change_approval.js +++ /dev/null @@ -1,81 +0,0 @@ -if (!PathChangeApprovalPublisher) { - var PathChangeApprovalPublisher = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("path_change_approval_info").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("path_change_approval_info").innerHTML = "Connected"; - }); - this.ros.on("close", function (error) { - document.getElementById("path_change_approval_info").innerHTML = "Closed"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - }, - send: function () { - var pub = new ROSLIB.Topic({ - ros: this.ros, - name: "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/path_change_approval", - messageType: "tier4_planning_msgs/msg/Approval", - }); - var str = new ROSLIB.Message({ - stamp: { - sec: 0, - nanosec: 0, - }, - approval: true, - }); - pub.publish(str); - }, - }; - PathChangeApprovalPublisher.init(); - - window.onload = function () {}; - window.onunload = function () { - PathChangeApprovalPublisher.ros.close(); - }; -} -if (!PathChangeApprovalStateSubscriber) { - var PathChangeApprovalStateSubscriber = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("state").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("state").innerHTML = "Connect"; - }); - this.ros.on("close", function (error) { - document.getElementById("state").innerHTML = "Close"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - - var sub = new ROSLIB.Topic({ - ros: this.ros, - name: "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/path_change_approval", - messageType: "tier4_planning_msgs/msg/Approval", - }); - sub.subscribe(function (message) { - const div = document.getElementById("path_change_approval_status"); - if (div.hasChildNodes()) { - div.removeChild(div.firstChild); - } - var res = message.command; - var el = document.createElement("span"); - el.innerHTML = res; - document.getElementById("path_change_approval_status").appendChild(el); - }); - }, - }; - PathChangeApprovalStateSubscriber.init(); - - window.onload = function () {}; - window.onunload = function () { - PathChangeApprovalStateSubscriber.ros.close(); - }; -} diff --git a/common/web_controller/www/web_controller/js/menu.js b/common/web_controller/www/web_controller/js/menu.js deleted file mode 100644 index d3a575f517aef..0000000000000 --- a/common/web_controller/www/web_controller/js/menu.js +++ /dev/null @@ -1,56 +0,0 @@ -var lastTabName = "#autoware"; - -$(function () { - $("#menu").menu(); - - // run the currently selected effect - function runEffect(tabName) { - if (lastTabName == tabName) { - return false; - } - - // most effect types need no options passed by default - var options = {}; - // run the effect - $(lastTabName).hide( - "clip", - { - direction: "horizontal", - }, - 1000, - callback(tabName) - ); - lastTabName = tabName; - } - - // callback function to bring a hidden box back - function callback(tabName) { - setTimeout(function () { - $(tabName).removeAttr("style").hide().show( - "clip", - { - direction: "horizontal", - }, - 1000 - ); - }, 1100); - } - - // set effect from select menu value - $("#menu_autoware").click(function () { - runEffect("#autoware"); - return false; - }); - $("#menu_vehicle").click(function () { - runEffect("#vehicle"); - return false; - }); -}); - -function ChangeTab(tabName) { - document.getElementById("autoware").style.display = "none"; - document.getElementById("vehicle").style.display = "none"; - document.getElementById(tabName).style.display = "block"; -} - -ChangeTab("autoware"); diff --git a/common/web_controller/www/web_controller/js/path_change_approval.js b/common/web_controller/www/web_controller/js/path_change_approval.js deleted file mode 100644 index 413d893bb7a43..0000000000000 --- a/common/web_controller/www/web_controller/js/path_change_approval.js +++ /dev/null @@ -1,81 +0,0 @@ -if (!PathChangeApprovalPublisher) { - var PathChangeApprovalPublisher = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("path_change_approval_info").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("path_change_approval_info").innerHTML = "Connected"; - }); - this.ros.on("close", function (error) { - document.getElementById("path_change_approval_info").innerHTML = "Closed"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - }, - send: function () { - var pub = new ROSLIB.Topic({ - ros: this.ros, - name: "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/path_change_approval", - messageType: "tier4_planning_msgs/msg/Approval", - }); - var str = new ROSLIB.Message({ - stamp: { - sec: 0, - nanosec: 0, - }, - approval: true, - }); - pub.publish(str); - }, - }; - PathChangeApprovalPublisher.init(); - - window.onload = function () {}; - window.onunload = function () { - PathChangeApprovalPublisher.ros.close(); - }; -} -if (!PathChangeApprovalStateSubscriber) { - var PathChangeApprovalStateSubscriber = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("state").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("state").innerHTML = "Connect"; - }); - this.ros.on("close", function (error) { - document.getElementById("state").innerHTML = "Close"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - - var sub = new ROSLIB.Topic({ - ros: this.ros, - name: "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/path_change_approval", - messageType: "tier4_planning_msgs/msg/Approval", - }); - sub.subscribe(function (message) { - const div = document.getElementById("path_change_approval_status"); - if (div.hasChildNodes()) { - div.removeChild(div.firstChild); - } - var res = message.command; - var el = document.createElement("span"); - el.innerHTML = res; - document.getElementById("path_change_approval_status").appendChild(el); - }); - }, - }; - PathChangeApprovalStateSubscriber.init(); - - window.onload = function () {}; - window.onunload = function () { - PathChangeApprovalStateSubscriber.ros.close(); - }; -} diff --git a/common/web_controller/www/web_controller/js/vehicle_engage.js b/common/web_controller/www/web_controller/js/vehicle_engage.js deleted file mode 100644 index 4304fbe9816c4..0000000000000 --- a/common/web_controller/www/web_controller/js/vehicle_engage.js +++ /dev/null @@ -1,188 +0,0 @@ -if (!VehicleEngagePublisher) { - var VehicleEngagePublisher = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("state").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("state").innerHTML = "Connected"; - }); - this.ros.on("close", function (error) { - document.getElementById("state").innerHTML = "Closed"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - }, - send: function (value) { - var pub = new ROSLIB.Topic({ - ros: this.ros, - name: "/vehicle/engage", - messageType: "autoware_auto_vehicle_msgs/Engage", - }); - - if (value == "Engage") { - var str = new ROSLIB.Message({ - stamp: { - sec: 0, - nanosec: 0, - }, - engage: true, - }); - pub.publish(str); - } else { - var str = new ROSLIB.Message({ - stamp: { - sec: 0, - nanosec: 0, - }, - engage: false, - }); - pub.publish(str); - } - }, - }; - VehicleEngagePublisher.init(); - - window.onload = function () {}; - window.onunload = function () { - VehicleEngagePublisher.ros.close(); - }; -} -if (!VehicleDisengagePublisher) { - var VehicleDisengagePublisher = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("state").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("state").innerHTML = "Connected"; - }); - this.ros.on("close", function (error) { - document.getElementById("state").innerHTML = "Closed"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - }, - send: function () { - var pub = new ROSLIB.Topic({ - ros: this.ros, - name: "/vehicle/engage", - messageType: "autoware_auto_vehicle_msgs/Engage", - }); - - var str = new ROSLIB.Message({ - stamp: { - sec: 0, - nanosec: 0, - }, - engage: false, - }); - pub.publish(str); - }, - }; - VehicleDisengagePublisher.init(); - - window.onload = function () {}; - window.onunload = function () { - VehicleDisengagePublisher.ros.close(); - }; -} -if (!VehicleEngageStatusSubscriber) { - var VehicleEngageStatusSubscriber = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("state").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("state").innerHTML = "Connect"; - }); - this.ros.on("close", function (error) { - document.getElementById("state").innerHTML = "Close"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - - var sub = new ROSLIB.Topic({ - ros: this.ros, - name: "/vehicle/engage", - messageType: "autoware_auto_vehicle_msgs/Engage", - }); - sub.subscribe(function (message) { - const div = document.getElementById("vehicle_engage_status"); - if (div.hasChildNodes()) { - div.removeChild(div.firstChild); - } - var res = message.engage; - var el = document.createElement("span"); - el.innerHTML = res; - document.getElementById("vehicle_engage_status").appendChild(el); - }); - }, - }; - VehicleEngageStatusSubscriber.init(); - - window.onload = function () {}; - window.onunload = function () { - VehicleEngageStatusSubscriber.ros.close(); - }; -} -if (!VehicleControlModeStatusSubscriber) { - var VehicleControlModeStatusSubscriber = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("state").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("state").innerHTML = "Connect"; - }); - this.ros.on("close", function (error) { - document.getElementById("state").innerHTML = "Close"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - - var sub = new ROSLIB.Topic({ - ros: this.ros, - name: "/vehicle/status/control_mode", - messageType: "autoware_auto_vehicle_msgs/ControlModeReport", - }); - sub.subscribe(function (message) { - const div = document.getElementById("vehicle_control_mode_status"); - if (div.hasChildNodes()) { - div.removeChild(div.firstChild); - } - var res = ""; - if (message.mode == 0) { - res = "No command"; - } else if (message.mode == 1) { - res = "Autonomous"; - } else if (message.mode == 2) { - res = "Manual"; - } else if (message.mode == 3) { - res = "Disengaged"; - } else if (message.mode == 4) { - res = "Not ready"; - } else { - res = "Undefined"; - } - var el = document.createElement("span"); - el.innerHTML = res; - document.getElementById("vehicle_control_mode_status").appendChild(el); - }); - }, - }; - VehicleControlModeStatusSubscriber.init(); - - window.onload = function () {}; - window.onunload = function () { - VehicleControlModeStatusSubscriber.ros.close(); - }; -} diff --git a/common/web_controller/www/web_controller/js/velocity_limit.js b/common/web_controller/www/web_controller/js/velocity_limit.js deleted file mode 100644 index 3b96f558b4c67..0000000000000 --- a/common/web_controller/www/web_controller/js/velocity_limit.js +++ /dev/null @@ -1,81 +0,0 @@ -if (!VelocityLimitPublisher) { - var VelocityLimitPublisher = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("velocity_limit_info").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("velocity_limit_info").innerHTML = "Connected"; - }); - this.ros.on("close", function (error) { - document.getElementById("velocity_limit_info").innerHTML = "Closed"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - }, - send: function () { - var pub = new ROSLIB.Topic({ - ros: this.ros, - name: "/planning/scenario_planning/max_velocity_default", - messageType: "tier4_planning_msgs/VelocityLimit", - }); - var str = new ROSLIB.Message({ - stamp: { - sec: 0, - nanosec: 0, - }, - max_velocity: parseFloat(velocity_limit_form.velocity_limit.value) / 3.6, - }); - pub.publish(str); - }, - }; - VelocityLimitPublisher.init(); - - window.onload = function () {}; - window.onunload = function () { - VelocityLimitPublisher.ros.close(); - }; -} -if (!VelocityLimitSubscriber) { - var VelocityLimitSubscriber = { - ros: null, - name: "", - init: function () { - this.ros = new ROSLIB.Ros(); - this.ros.on("error", function (error) { - document.getElementById("state").innerHTML = "Error"; - }); - this.ros.on("connection", function (error) { - document.getElementById("state").innerHTML = "Connect"; - }); - this.ros.on("close", function (error) { - document.getElementById("state").innerHTML = "Close"; - }); - this.ros.connect("ws://" + location.hostname + ":9090"); - - var sub = new ROSLIB.Topic({ - ros: this.ros, - name: "/planning/scenario_planning/max_velocity_default", - messageType: "tier4_planning_msgs/VelocityLimit", - }); - sub.subscribe(function (message) { - const div = document.getElementById("velocity_limit_status"); - if (div.hasChildNodes()) { - div.removeChild(div.firstChild); - } - var res = message.max_velocity; - var el = document.createElement("span"); - el.innerHTML = res * 3.6; - document.getElementById("velocity_limit_status").appendChild(el); - }); - }, - }; - VelocityLimitSubscriber.init(); - - window.onload = function () {}; - window.onunload = function () { - VelocityLimitSubscriber.ros.close(); - }; -} diff --git a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml index 45014428d0bd1..a0ddef6e363cc 100644 --- a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml +++ b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml @@ -2,6 +2,7 @@ ros__parameters: transition_timeout: 10.0 frequency_hz: 10.0 + check_engage_condition: false # set false if you do not want to care about the engage condition. engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. dist_threshold: 1.5 diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 11c4fd070343f..775083908cdfd 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -42,6 +42,8 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) sub_trajectory_ = node->create_subscription( "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; }); + check_engage_condition_ = node->declare_parameter("check_engage_condition"); + // params for mode change available { auto & p = engage_acceptable_param_; @@ -80,9 +82,21 @@ void AutonomousMode::update(bool transition) bool AutonomousMode::isModeChangeCompleted() { + if (!check_engage_condition_) { + return true; + } + constexpr auto dist_max = 5.0; constexpr auto yaw_max = M_PI_4; + const auto current_speed = kinematics_.twist.twist.linear.x; + const auto & param = engage_acceptable_param_; + + // Engagement completes quickly if the vehicle is stopped. + if (param.allow_autonomous_in_stopped && std::abs(current_speed) < 0.01) { + return true; + } + const auto unstable = [this]() { stable_start_time_.reset(); return false; @@ -180,6 +194,11 @@ std::pair AutonomousMode::hasDangerLateralAcceleration() bool AutonomousMode::isModeChangeAvailable() { + if (!check_engage_condition_) { + debug_info_.is_all_ok = true; + return true; + } + constexpr auto dist_max = 100.0; constexpr auto yaw_max = M_PI_4; diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index bb8665e44c7d5..dc72d57aed231 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -72,6 +72,7 @@ class AutonomousMode : public ModeChangeBase rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; + bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks. EngageAcceptableParam engage_acceptable_param_; StableCheckParam stable_check_param_; AckermannControlCommand control_cmd_; diff --git a/control/trajectory_follower/design/mpc_lateral_controller-design.md b/control/trajectory_follower/design/mpc_lateral_controller-design.md index ee79cc88f206b..0883d160cb89b 100644 --- a/control/trajectory_follower/design/mpc_lateral_controller-design.md +++ b/control/trajectory_follower/design/mpc_lateral_controller-design.md @@ -93,6 +93,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | path_smoothing_times | int | number of times of applying path smoothing filter | 1 | | curvature_smoothing_num_ref_steer | double | index distance of points used in curvature calculation for reference steer command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 35 | | curvature_smoothing_num_traj | double | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 1 | +| extend_trajectory_for_end_yaw_control | bool | trajectory extending flag for end yaw control. | true | | steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 | | admissible_position_error | double | stop vehicle when following position error is larger than this value [m]. | 5.0 | | admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad]. | 1.57 | diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp index 8ae6dc5449fc3..c64c231144b80 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc.hpp @@ -400,7 +400,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg, const double traj_resample_dist, const bool enable_path_smoothing, const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj, - const int curvature_smoothing_num_ref_steer); + const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control); /** * @brief set the vehicle model of this MPC */ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index d38312505fed7..8ed6d44b85258 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -97,6 +97,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController //!< @brief path resampling interval [m] double m_traj_resample_dist; + //!< @brief flag of traj extending for terminal yaw + bool m_extend_trajectory_for_end_yaw_control; + /* parameters for stop state */ double m_stop_state_entry_ego_speed; double m_stop_state_entry_target_speed; diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp index 9f394ff9baa6f..f9f58deadd6bb 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp @@ -179,6 +179,21 @@ TRAJECTORY_FOLLOWER_PUBLIC bool calcNearestPoseInterp( // */ TRAJECTORY_FOLLOWER_PUBLIC double calcStopDistance( const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int origin); + +/** + * @brief extend terminal points + * Note: The current MPC does not properly take into account the attitude angle at the end of the + * path. By extending the end of the path in the attitude direction, the MPC can consider the + * attitude angle well, resulting in improved control performance. If the trajectory is + * well-defined considering the end point attitude angle, this feature is not necessary. + * @param [in] terminal yaw + * @param [in] extend interval + * @param [in] flag of forward shift + * @param [out] extended trajectory + */ +void extendTrajectoryInYawDirection( + const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj); + } // namespace MPCUtils } // namespace trajectory_follower } // namespace control diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp index 050727c40a50d..ed6472700e14a 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/control/trajectory_follower/src/mpc.cpp @@ -180,7 +180,7 @@ void MPC::setReferenceTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg, const double traj_resample_dist, const bool enable_path_smoothing, const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj, - const int curvature_smoothing_num_ref_steer) + const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control) { trajectory_follower::MPCTrajectory mpc_traj_raw; // received raw trajectory trajectory_follower::MPCTrajectory mpc_traj_resampled; // resampled trajectory @@ -217,6 +217,17 @@ void MPC::setReferenceTrajectory( } } + /* extend terminal points + * Note: The current MPC does not properly take into account the attitude angle at the end of the + * path. By extending the end of the path in the attitude direction, the MPC can consider the + * attitude angle well, resulting in improved control performance. If the trajectory is + * well-defined considering the end point attitude angle, this feature is not necessary. + */ + if (extend_trajectory_for_end_yaw_control) { + trajectory_follower::MPCUtils::extendTrajectoryInYawDirection( + mpc_traj_raw.yaw.back(), traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed); + } + /* calculate yaw angle */ trajectory_follower::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift); trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw); diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index a90fa0fd61da6..b2e0ac18753a5 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -65,6 +65,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} m_mpc.m_admissible_yaw_error_rad = node_->declare_parameter("admissible_yaw_error_rad"); m_mpc.m_use_steer_prediction = node_->declare_parameter("use_steer_prediction"); m_mpc.m_param.steer_tau = node_->declare_parameter("vehicle_model_steer_tau"); + m_extend_trajectory_for_end_yaw_control = + node_->declare_parameter("extend_trajectory_for_end_yaw_control"); /* stop state parameters */ m_stop_state_entry_ego_speed = node_->declare_parameter("stop_state_entry_ego_speed"); @@ -234,6 +236,7 @@ bool MpcLateralController::isSteerConverged( // wait for a while to propagate the trajectory shape to the output command when the trajectory // shape is changed. if (!m_has_received_first_trajectory || isTrajectoryShapeChanged()) { + RCLCPP_DEBUG(node_->get_logger(), "trajectory shaped is changed"); return false; } @@ -301,7 +304,8 @@ void MpcLateralController::setTrajectory( m_mpc.setReferenceTrajectory( *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num, - m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer); + m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer, + m_extend_trajectory_for_end_yaw_control); // update trajectory buffer to check the trajectory shape change. m_trajectory_buffer.push_back(*m_current_trajectory_ptr); diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index aece2545c388e..6e5b31d340e6b 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -230,7 +230,7 @@ bool convertToMPCTrajectory( for (const autoware_auto_planning_msgs::msg::TrajectoryPoint & p : input.points) { const double x = p.pose.position.x; const double y = p.pose.position.y; - const double z = 0.0; + const double z = p.pose.position.z; const double yaw = tf2::getYaw(p.pose.orientation); const double vx = p.longitudinal_velocity_mps; const double k = 0.0; @@ -265,10 +265,9 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj) traj.relative_time.clear(); traj.relative_time.push_back(t); for (size_t i = 0; i < traj.x.size() - 1; ++i) { - const double dx = traj.x.at(i + 1) - traj.x.at(i); - const double dy = traj.y.at(i + 1) - traj.y.at(i); - const double dz = traj.z.at(i + 1) - traj.z.at(i); - const double dist = std::sqrt(dx * dx + dy * dy + dz * dz); + const double dist = std::hypot( + traj.x.at(i + 1) - traj.x.at(i), traj.y.at(i + 1) - traj.y.at(i), + traj.z.at(i + 1) - traj.z.at(i)); const double v = std::max(std::fabs(traj.vx.at(i)), 0.1); t += (dist / v); traj.relative_time.push_back(t); @@ -404,6 +403,31 @@ double calcStopDistance( return stop_dist; } +void extendTrajectoryInYawDirection( + const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj) +{ + // set terminal yaw + traj.yaw.back() = yaw; + + // get terminal pose + autoware_auto_planning_msgs::msg::Trajectory autoware_traj; + autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory( + traj, autoware_traj); + auto extended_pose = autoware_traj.points.back().pose; + + constexpr double extend_dist = 10.0; + constexpr double extend_vel = 10.0; + const double x_offset = is_forward_shift ? interval : -interval; + const double dt = interval / extend_vel; + const size_t num_extended_point = static_cast(extend_dist / interval); + for (size_t i = 0; i < num_extended_point; ++i) { + extended_pose = tier4_autoware_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0); + traj.push_back( + extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(), + extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt); + } +} + } // namespace MPCUtils } // namespace trajectory_follower } // namespace control diff --git a/control/trajectory_follower/test/test_mpc.cpp b/control/trajectory_follower/test/test_mpc.cpp index 909133dcd65bd..6aa767289c1fd 100644 --- a/control/trajectory_follower/test/test_mpc.cpp +++ b/control/trajectory_follower/test/test_mpc.cpp @@ -84,6 +84,7 @@ class MPCTest : public ::testing::Test int curvature_smoothing_num_ref_steer = 35; bool enable_path_smoothing = true; bool use_steer_prediction = true; + bool extend_trajectory_for_end_yaw_control = true; void SetUp() override { @@ -175,7 +176,8 @@ class MPCTest : public ::testing::Test // Init trajectory mpc.setReferenceTrajectory( dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); } }; // class MPCTest @@ -233,7 +235,8 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) initializeMPC(mpc); mpc.setReferenceTrajectory( dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); // Calculate MPC AckermannLateralCommand ctrl_cmd; @@ -251,7 +254,8 @@ TEST_F(MPCTest, OsqpCalculate) initializeMPC(mpc); mpc.setReferenceTrajectory( dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); const std::string vehicle_model_type = "kinematics"; std::shared_ptr vehicle_model_ptr = @@ -282,7 +286,8 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) initializeMPC(mpc); mpc.setReferenceTrajectory( dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); const std::string vehicle_model_type = "kinematics"; std::shared_ptr vehicle_model_ptr = @@ -327,7 +332,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) // Init trajectory mpc.setReferenceTrajectory( dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; @@ -344,7 +350,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) initializeMPC(mpc); mpc.setReferenceTrajectory( dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); const std::string vehicle_model_type = "kinematics_no_delay"; std::shared_ptr vehicle_model_ptr = diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp index 346fe9a92d56a..f8493c7a1cb28 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp @@ -83,6 +83,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_accel_; rclcpp::Publisher::SharedPtr control_cmd_pub_; + rclcpp::Publisher::SharedPtr debug_marker_pub_; enum class LateralControllerMode { INVALID = 0, @@ -106,6 +107,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; LongitudinalControllerMode getLongitudinalControllerMode( const std::string & algorithm_name) const; + void publishDebugMarker() const; }; } // namespace trajectory_follower_nodes } // namespace control diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml index a6d98fa682d74..92d15da9e49e6 100644 --- a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml @@ -12,6 +12,9 @@ curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) + # -- trajectory extending -- + extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control + # -- mpc optimization -- qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) mpc_prediction_horizon: 50 # prediction horizon step diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp index 0bd1a694f1bd7..8b168afc08416 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/control/trajectory_follower_nodes/src/controller_node.cpp @@ -77,6 +77,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control "~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1)); control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); + debug_marker_pub_ = + create_publisher("~/output/debug_marker", rclcpp::QoS{1}); // Timer { @@ -168,6 +170,34 @@ void Controller::callbackTimerControl() out.lateral = lateral_output_->control_cmd; out.longitudinal = longitudinal_output_->control_cmd; control_cmd_pub_->publish(out); + + publishDebugMarker(); +} + +void Controller::publishDebugMarker() const +{ + visualization_msgs::msg::MarkerArray debug_marker_array{}; + + // steer converged marker + { + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), + tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); + marker.pose = input_data_.current_odometry_ptr->pose.pose; + + std::stringstream ss; + const double current = input_data_.current_steering_ptr->steering_tire_angle; + const double cmd = lateral_output_->control_cmd.steering_tire_angle; + const double diff = current - cmd; + ss << "current:" << current << " cmd:" << cmd << " diff:" << diff + << (lateral_output_->sync_data.is_steer_converged ? " (converged)" : " (not converged)"); + marker.text = ss.str(); + + debug_marker_array.markers.push_back(marker); + } + + debug_marker_pub_->publish(debug_marker_array); } } // namespace trajectory_follower_nodes diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 09592c3fffac6..4bae58e22fa93 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -21,7 +21,6 @@ | `~/input/external/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from external | | `~/input/external_emergency_stop_heartbeat` | `tier4_external_api_msgs::msg::Heartbeat` | heartbeat | | `~/input/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | -| `~/input/emergency_state` | `autoware_auto_system_msgs::msg::EmergencyState` | used to detect the emergency situation of the vehicle | | `~/input/emergency/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from emergency handler | | `~/input/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | | `~/input/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | @@ -48,7 +47,7 @@ | ------------------------------------------- | ------ | --------------------------------------------------------------------------- | | `update_period` | double | update period | | `use_emergency_handling` | bool | true when emergency handler is used | -| `use_external_emergency_stop` | bool | true when external emergency stop information is used | +| `check_external_emergency_heartbeat` | bool | true when checking heartbeat for emergency stop | | `system_emergency_heartbeat_timeout` | double | timeout for system emergency | | `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency | | `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop | @@ -66,6 +65,6 @@ ## Assumptions / Known limits -The parameter `use_external_emergency_stop` (true by default) enables an emergency stop request from external modules. +The parameter `check_external_emergency_heartbeat` (true by default) enables an emergency stop request from external modules. This feature requires a `~/input/external_emergency_stop_heartbeat` topic for health monitoring of the external module, and the vehicle_cmd_gate module will not start without the topic. -The `use_external_emergency_stop` parameter must be false when the "external emergency stop" function is not used. +The `check_external_emergency_heartbeat` parameter must be false when the "external emergency stop" function is not used. diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index 47aedbae68430..644e937e0718d 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -1,7 +1,7 @@ - + @@ -22,7 +22,6 @@ - @@ -48,7 +47,7 @@ - + diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 2634949b8b577..36e12ab28a3a6 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -128,7 +128,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) // Parameter update_period_ = 1.0 / declare_parameter("update_rate", 10.0); use_emergency_handling_ = declare_parameter("use_emergency_handling", false); - use_external_emergency_stop_ = declare_parameter("use_external_emergency_stop", false); + check_external_emergency_heartbeat_ = + declare_parameter("check_external_emergency_heartbeat", false); system_emergency_heartbeat_timeout_ = declare_parameter("system_emergency_heartbeat_timeout", 0.5); external_emergency_stop_heartbeat_timeout_ = @@ -224,7 +225,7 @@ bool VehicleCmdGate::isDataReady() } } - if (use_external_emergency_stop_) { + if (check_external_emergency_heartbeat_) { if (!external_emergency_stop_heartbeat_received_time_) { RCLCPP_WARN(get_logger(), "external_emergency_stop_heartbeat_received_time_ is false"); return false; @@ -322,7 +323,7 @@ void VehicleCmdGate::onTimer() } // Check external emergency stop heartbeat - if (use_external_emergency_stop_) { + if (check_external_emergency_heartbeat_) { is_external_emergency_stop_heartbeat_timeout_ = isHeartbeatTimeout( external_emergency_stop_heartbeat_received_time_, external_emergency_stop_heartbeat_timeout_); diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 2813af0261641..756e8e122b13f 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include @@ -160,7 +159,7 @@ class VehicleCmdGate : public rclcpp::Node // Parameter double update_period_; bool use_emergency_handling_; - bool use_external_emergency_stop_; + bool check_external_emergency_heartbeat_; double system_emergency_heartbeat_timeout_; double external_emergency_stop_heartbeat_timeout_; double stop_hold_acceleration_; diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml index 1eff5a49add48..55b57dbda5a73 100644 --- a/launch/tier4_autoware_api_launch/package.xml +++ b/launch/tier4_autoware_api_launch/package.xml @@ -21,6 +21,7 @@ awapi_awiv_adapter default_ad_api path_distance_calculator + rosbridge_server topic_tools ament_lint_auto diff --git a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml index 45014428d0bd1..a0ddef6e363cc 100644 --- a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml +++ b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml @@ -2,6 +2,7 @@ ros__parameters: transition_timeout: 10.0 frequency_hz: 10.0 + check_engage_condition: false # set false if you do not want to care about the engage condition. engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. dist_threshold: 1.5 diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index 76b5da140bfaa..f3819f155edfe 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -13,6 +13,9 @@ curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) + # -- trajectory extending -- + extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control + # -- mpc optimization -- qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) mpc_prediction_horizon: 50 # prediction horizon step diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index e5754fce856e9..4cb4c54dd5ddb 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -171,7 +171,6 @@ def launch_setup(context, *args, **kwargs): plugin="vehicle_cmd_gate::VehicleCmdGate", name="vehicle_cmd_gate", remappings=[ - ("input/emergency_state", "/system/emergency/emergency_state"), ("input/steering", "/vehicle/status/steering_status"), ("input/operation_mode", "/system/operation_mode/state"), ("input/auto/control_cmd", "/control/trajectory_follower/control_cmd"), @@ -209,7 +208,9 @@ def launch_setup(context, *args, **kwargs): vehicle_info_param, { "use_emergency_handling": LaunchConfiguration("use_emergency_handling"), - "use_external_emergency_stop": LaunchConfiguration("use_external_emergency_stop"), + "check_external_emergency_heartbeat": LaunchConfiguration( + "check_external_emergency_heartbeat" + ), "use_start_request": LaunchConfiguration("use_start_request"), }, ], @@ -374,7 +375,7 @@ def add_launch_arg(name: str, default_value=None, description=None): # vehicle cmd gate add_launch_arg("use_emergency_handling", "false", "use emergency handling") - add_launch_arg("use_external_emergency_stop", "true", "use external emergency stop") + add_launch_arg("check_external_emergency_heartbeat", "true", "use external emergency stop") add_launch_arg("use_start_request", "false", "use start request service") # external cmd selector diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml index fa94afbf12a0b..8f3ccbff00360 100644 --- a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml +++ b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml @@ -3,6 +3,7 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: false + enable_differential_load: false # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 153b879cd96ba..b4e992ab56916 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -50,6 +50,9 @@ min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] max_avoidance_acceleration: 0.5 # [m/ss] + # bound clipping for objects + enable_bound_clipping: false + # for debug publish_debug_marker: false print_debug_info: false diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml index b9de5cc4f2e13..71dc2ac600685 100644 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml +++ b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml @@ -36,6 +36,7 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default @@ -46,5 +47,6 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml index bf40c334f6498..9708456df4fe7 100644 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml +++ b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml @@ -36,6 +36,7 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default @@ -46,5 +47,6 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 49682a9daba54..2400b8b449a6a 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -13,7 +13,7 @@ - + @@ -21,9 +21,15 @@ + + + + + + diff --git a/launch/tier4_system_launch/system_launch.drawio.svg b/launch/tier4_system_launch/system_launch.drawio.svg index 334012a85b351..07a33aff8e30f 100644 --- a/launch/tier4_system_launch/system_launch.drawio.svg +++ b/launch/tier4_system_launch/system_launch.drawio.svg @@ -51,7 +51,7 @@ >
- system_monitor.launch.py + system_monitor.launch.xml

package: system_monitor
@@ -59,7 +59,7 @@
- system_monitor.launch.py... + system_monitor.launch.xml... diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index 4dc60ff2e785f..0d53b64f16e36 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -9,6 +9,8 @@ ament_auto_add_executable(${PROJECT_NAME} src/gyro_odometer_core.cpp ) +target_link_libraries(${PROJECT_NAME} fmt) + ament_auto_package(INSTALL_TO_SHARE launch ) diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index ce74644ad3586..c83447369855a 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,6 +15,9 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "tier4_autoware_utils/ros/transform_listener.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + #include #include @@ -27,27 +30,28 @@ #else #include #endif -#include -#include +#include +#include #include class GyroOdometer : public rclcpp::Node { +private: + using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + public: GyroOdometer(); ~GyroOdometer(); private: - void callbackTwistWithCovariance( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr); + void callbackVehicleTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr); void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); - bool getTransform( - const std::string & target_frame, const std::string & source_frame, - const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr); + void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); rclcpp::Subscription::SharedPtr - vehicle_twist_with_cov_sub_; + vehicle_twist_sub_; rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Publisher::SharedPtr twist_raw_pub_; @@ -58,14 +62,15 @@ class GyroOdometer : public rclcpp::Node rclcpp::Publisher::SharedPtr twist_with_covariance_pub_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; + std::shared_ptr transform_listener_; std::string output_frame_; double message_timeout_sec_; - geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr_; - sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr_; + bool vehicle_twist_arrived_; + bool imu_arrived_; + std::deque vehicle_twist_queue_; + std::deque gyro_queue_; }; #endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index f9de1ac2086f2..aeb505270b2cc 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index e96c6ef7dfc5c..43717dbd49352 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -12,12 +12,14 @@ autoware_cmake + fmt geometry_msgs sensor_msgs std_msgs tf2 tf2_geometry_msgs tf2_ros + tier4_autoware_utils rclcpp rclcpp_components diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index c03d0cf3a0027..b4a3c7118e051 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -19,21 +19,98 @@ #else #include #endif +#include #include #include #include +std::array transformCovariance(const std::array & cov) +{ + using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + + double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); + + std::array cov_transformed; + cov_transformed.fill(0.); + cov_transformed[COV_IDX::X_X] = max_cov; + cov_transformed[COV_IDX::Y_Y] = max_cov; + cov_transformed[COV_IDX::Z_Z] = max_cov; + return cov_transformed; +} + +geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( + const std::deque & vehicle_twist_queue, + const std::deque & gyro_queue) +{ + using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + + double vx_mean = 0; + geometry_msgs::msg::Vector3 gyro_mean{}; + double vx_covariance_original = 0; + geometry_msgs::msg::Vector3 gyro_covariance_original{}; + for (const auto & vehicle_twist : vehicle_twist_queue) { + vx_mean += vehicle_twist.twist.twist.linear.x; + vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; + } + vx_mean /= vehicle_twist_queue.size(); + vx_covariance_original /= vehicle_twist_queue.size(); + + for (const auto & gyro : gyro_queue) { + gyro_mean.x += gyro.angular_velocity.x; + gyro_mean.y += gyro.angular_velocity.y; + gyro_mean.z += gyro.angular_velocity.z; + gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; + gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; + gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; + } + gyro_mean.x /= gyro_queue.size(); + gyro_mean.y /= gyro_queue.size(); + gyro_mean.z /= gyro_queue.size(); + gyro_covariance_original.x /= gyro_queue.size(); + gyro_covariance_original.y /= gyro_queue.size(); + gyro_covariance_original.z /= gyro_queue.size(); + + geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; + const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue.back().header.stamp); + const auto latest_imu_stamp = rclcpp::Time(gyro_queue.back().header.stamp); + if (latest_vehicle_twist_stamp < latest_imu_stamp) { + twist_with_cov.header.stamp = latest_imu_stamp; + } else { + twist_with_cov.header.stamp = latest_vehicle_twist_stamp; + } + twist_with_cov.twist.twist.linear.x = vx_mean; + twist_with_cov.twist.twist.angular = gyro_mean; + + // From a statistical point of view, here we reduce the covariances according to the number of + // observed data + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = + vx_covariance_original / vehicle_twist_queue.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = + gyro_covariance_original.x / gyro_queue.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = + gyro_covariance_original.y / gyro_queue.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = + gyro_covariance_original.z / gyro_queue.size(); + + return twist_with_cov; +} + GyroOdometer::GyroOdometer() : Node("gyro_odometer"), - tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_), output_frame_(declare_parameter("base_link", "base_link")), - message_timeout_sec_(declare_parameter("message_timeout_sec", 0.2)) + message_timeout_sec_(declare_parameter("message_timeout_sec", 0.2)), + vehicle_twist_arrived_(false), + imu_arrived_(false) { - vehicle_twist_with_cov_sub_ = create_subscription( + transform_listener_ = std::make_shared(this); + + vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometer::callbackTwistWithCovariance, this, std::placeholders::_1)); + std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1)); imu_sub_ = create_subscription( "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1)); @@ -51,90 +128,131 @@ GyroOdometer::GyroOdometer() GyroOdometer::~GyroOdometer() {} -void GyroOdometer::callbackTwistWithCovariance( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr) +void GyroOdometer::callbackVehicleTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) { - // TODO(YamatoAndo) trans from twist_with_cov_msg_ptr->header to base_frame - twist_with_cov_msg_ptr_ = twist_with_cov_msg_ptr; - - if (!imu_msg_ptr_) { + vehicle_twist_arrived_ = true; + if (!imu_arrived_) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed"); + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); + return; + } + + const double twist_dt = std::abs((this->now() - vehicle_twist_ptr->header.stamp).seconds()); + if (twist_dt > message_timeout_sec_) { + const std::string error_msg = fmt::format( + "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); return; } - const double imu_dt = std::abs((this->now() - imu_msg_ptr_->header.stamp).seconds()); + + vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + + if (gyro_queue_.empty()) return; + const double imu_dt = std::abs((this->now() - gyro_queue_.back().header.stamp).seconds()); if (imu_dt > message_timeout_sec_) { - RCLCPP_ERROR_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, - "Imu msg is timeout. imu_dt: %lf[sec], tolerance %lf[sec]", imu_dt, message_timeout_sec_); + const std::string error_msg = fmt::format( + "Imu msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); return; } + + const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = + concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); + publishData(twist_with_cov_raw); + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); } void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { - imu_msg_ptr_ = imu_msg_ptr; - - if (!twist_with_cov_msg_ptr_) { + imu_arrived_ = true; + if (!vehicle_twist_arrived_) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed"); + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); return; } - const double twist_dt = std::abs((this->now() - twist_with_cov_msg_ptr_->header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - RCLCPP_ERROR_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, - "Twist msg is timeout. twist_dt: %lf[sec], tolerance %lf[sec]", twist_dt, - message_timeout_sec_); + const double imu_dt = std::abs((this->now() - imu_msg_ptr->header.stamp).seconds()); + if (imu_dt > message_timeout_sec_) { + const std::string error_msg = fmt::format( + "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); return; } - geometry_msgs::msg::TransformStamped::SharedPtr tf_imu2base_ptr = - std::make_shared(); - getTransform(output_frame_, imu_msg_ptr_->header.frame_id, tf_imu2base_ptr); + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = + transform_listener_->getLatestTransform(imu_msg_ptr->header.frame_id, output_frame_); + if (!tf_imu2base_ptr) { + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), + (imu_msg_ptr->header.frame_id).c_str()); + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); + return; + } geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.header = imu_msg_ptr_->header; - angular_velocity.vector = imu_msg_ptr_->angular_velocity; + angular_velocity.header = imu_msg_ptr->header; + angular_velocity.vector = imu_msg_ptr->angular_velocity; geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; transformed_angular_velocity.header = tf_imu2base_ptr->header; tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); - // TODO(YamatoAndo) move code - geometry_msgs::msg::TwistStamped twist; - twist.header.stamp = imu_msg_ptr_->header.stamp; - twist.header.frame_id = output_frame_; - twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear; - twist.twist.angular.x = transformed_angular_velocity.vector.x; - twist.twist.angular.y = transformed_angular_velocity.vector.y; - twist.twist.angular.z = transformed_angular_velocity.vector.z; - twist_raw_pub_->publish(twist); - - geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance; - twist_with_covariance.header.stamp = imu_msg_ptr_->header.stamp; - twist_with_covariance.header.frame_id = output_frame_; - twist_with_covariance.twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear; - twist_with_covariance.twist.twist.angular.x = transformed_angular_velocity.vector.x; - twist_with_covariance.twist.twist.angular.y = transformed_angular_velocity.vector.y; - twist_with_covariance.twist.twist.angular.z = transformed_angular_velocity.vector.z; - - // NOTE - // linear.y and linear.z are not measured values. - // Therefore, they should be assigned large variance values. - twist_with_covariance.twist.covariance[0] = twist_with_cov_msg_ptr_->twist.covariance[0]; - twist_with_covariance.twist.covariance[7] = 10000.0; - twist_with_covariance.twist.covariance[14] = 10000.0; - twist_with_covariance.twist.covariance[21] = imu_msg_ptr_->angular_velocity_covariance[0]; - twist_with_covariance.twist.covariance[28] = imu_msg_ptr_->angular_velocity_covariance[4]; - twist_with_covariance.twist.covariance[35] = imu_msg_ptr_->angular_velocity_covariance[8]; - - twist_with_covariance_raw_pub_->publish(twist_with_covariance); + sensor_msgs::msg::Imu gyro_base_link; + gyro_base_link.header = imu_msg_ptr->header; + gyro_base_link.angular_velocity = transformed_angular_velocity.vector; + gyro_base_link.angular_velocity_covariance = + transformCovariance(imu_msg_ptr->angular_velocity_covariance); + + gyro_queue_.push_back(gyro_base_link); + + if (vehicle_twist_queue_.empty()) return; + const double twist_dt = + std::abs((this->now() - vehicle_twist_queue_.back().header.stamp).seconds()); + if (twist_dt > message_timeout_sec_) { + const std::string error_msg = fmt::format( + "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); + return; + } + + const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = + concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); + publishData(twist_with_cov_raw); + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); +} + +void GyroOdometer::publishData( + const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) +{ + geometry_msgs::msg::TwistStamped twist_raw; + twist_raw.header = twist_with_cov_raw.header; + twist_raw.twist = twist_with_cov_raw.twist.twist; + + twist_raw_pub_->publish(twist_raw); + twist_with_covariance_raw_pub_->publish(twist_with_cov_raw); + + geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance = twist_with_cov_raw; + geometry_msgs::msg::TwistStamped twist = twist_raw; // clear imu yaw bias if vehicle is stopped if ( - std::fabs(transformed_angular_velocity.vector.z) < 0.01 && - std::fabs(twist_with_cov_msg_ptr_->twist.twist.linear.x) < 0.01) { + std::fabs(twist_with_cov_raw.twist.twist.angular.z) < 0.01 && + std::fabs(twist_with_cov_raw.twist.twist.linear.x) < 0.01) { twist.twist.angular.x = 0.0; twist.twist.angular.y = 0.0; twist.twist.angular.z = 0.0; @@ -146,44 +264,3 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m twist_pub_->publish(twist); twist_with_covariance_pub_->publish(twist_with_covariance); } - -bool GyroOdometer::getTransform( - const std::string & target_frame, const std::string & source_frame, - const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr) -{ - if (target_frame == source_frame) { - transform_stamped_ptr->header.stamp = this->get_clock()->now(); - transform_stamped_ptr->header.frame_id = target_frame; - transform_stamped_ptr->child_frame_id = source_frame; - transform_stamped_ptr->transform.translation.x = 0.0; - transform_stamped_ptr->transform.translation.y = 0.0; - transform_stamped_ptr->transform.translation.z = 0.0; - transform_stamped_ptr->transform.rotation.x = 0.0; - transform_stamped_ptr->transform.rotation.y = 0.0; - transform_stamped_ptr->transform.rotation.z = 0.0; - transform_stamped_ptr->transform.rotation.w = 1.0; - return true; - } - - try { - *transform_stamped_ptr = - tf_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "%s", ex.what()); - RCLCPP_ERROR( - this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - - transform_stamped_ptr->header.stamp = this->get_clock()->now(); - transform_stamped_ptr->header.frame_id = target_frame; - transform_stamped_ptr->child_frame_id = source_frame; - transform_stamped_ptr->transform.translation.x = 0.0; - transform_stamped_ptr->transform.translation.y = 0.0; - transform_stamped_ptr->transform.translation.z = 0.0; - transform_stamped_ptr->transform.rotation.x = 0.0; - transform_stamped_ptr->transform.rotation.y = 0.0; - transform_stamped_ptr->transform.rotation.z = 0.0; - transform_stamped_ptr->transform.rotation.w = 1.0; - return false; - } - return true; -} diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index e94f86c8d6c7e..86b0382527201 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -22,3 +22,7 @@ 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, ] + + # use partial map loading for large maps + # Note: You also need to enable partial_map_loading interface in pointcloud_map_loader to use this functionality + enable_partial_map_load: false diff --git a/localization/pose_initializer/docs/map_height_fitter.md b/localization/pose_initializer/docs/map_height_fitter.md index ee7fdfb44264d..2715ab7f91ac3 100644 --- a/localization/pose_initializer/docs/map_height_fitter.md +++ b/localization/pose_initializer/docs/map_height_fitter.md @@ -7,8 +7,21 @@ Use this service as preprocessing for `pose_initializer` when using a initial po This service replaces the Z value of the input pose with the lowest point of the map point cloud within a cylinder of XY-radius. If no point is found in this range, returns the input pose without changes. +Note that this package supports partial map loading interface, which is disabled by default. The interface is intended to be enabled when +the pointcloud map is too large to handle. By using the interface, the node will request for a partial map around the requested position +instead of loading whole map by subscription interface. To use this interface, + +1. Set `enable_partial_map_load` in this node to `true` +2. Set `enable_partial_load` in `pointcloud_map_loader` to `true` + ## Interfaces +### Parameters + +| Name | Type | Description | | +| ------------------------- | ---- | ------------------------------------------------------------------------------------- | ----- | +| `enable_partial_map_load` | bool | If true, use partial map load interface. If false, use topic subscription interaface. | false | + ### Services | Name | Type | Description | @@ -20,3 +33,9 @@ If no point is found in this range, returns the input pose without changes. | Name | Type | Description | | --------------------- | ----------------------------- | -------------- | | `/map/pointcloud_map` | sensor_msgs::msg::PointCloud2 | pointcloud map | + +### Clients + +| Name | Type | Description | +| --------------------------------- | ----------------------------------------------- | -------------------------------------------- | +| `/map/get_partial_pointcloud_map` | autoware_map_msgs::srv::GetPartialPointCloudMap | client for requesting partial pointcloud map | diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 732457dfaac54..9a29fdd60fe02 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -5,8 +5,10 @@ - + + + diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 2f39f74efa09c..48d3a203b8deb 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -14,6 +14,7 @@ autoware_cmake + autoware_map_msgs component_interface_specs component_interface_utils geometry_msgs diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp index 0e2bded754f61..44a9360d65d68 100644 --- a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp +++ b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp @@ -23,17 +23,33 @@ #endif #include +#include MapHeightFitter::MapHeightFitter() : Node("map_height_fitter"), tf2_listener_(tf2_buffer_) { + enable_partial_map_load_ = declare_parameter("enable_partial_map_load", false); + const auto durable_qos = rclcpp::QoS(1).transient_local(); using std::placeholders::_1; using std::placeholders::_2; - sub_map_ = create_subscription( - "pointcloud_map", durable_qos, std::bind(&MapHeightFitter::on_map, this, _1)); srv_fit_ = create_service( "fit_map_height", std::bind(&MapHeightFitter::on_fit, this, _1, _2)); + + if (!enable_partial_map_load_) { + sub_map_ = create_subscription( + "pointcloud_map", durable_qos, std::bind(&MapHeightFitter::on_map, this, _1)); + } else { + callback_group_service_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + cli_get_partial_pcd_ = create_client( + "client_partial_map_load", rmw_qos_profile_default, callback_group_service_); + while (!cli_get_partial_pcd_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO( + this->get_logger(), + "Cannot find partial map loading interface. Please check the setting in " + "pointcloud_map_loader to see if the interface is enabled."); + } + } } void MapHeightFitter::on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) @@ -72,15 +88,62 @@ double MapHeightFitter::get_ground_height(const tf2::Vector3 & point) const return std::isfinite(height) ? height : point.getZ(); } +void MapHeightFitter::get_partial_point_cloud_map(const geometry_msgs::msg::Point & point) +{ + if (!cli_get_partial_pcd_) { + throw std::runtime_error{"Partial map loading in pointcloud_map_loader is not enabled"}; + } + const auto req = std::make_shared(); + req->area.center = point; + req->area.radius = 50; + + RCLCPP_INFO(this->get_logger(), "Send request to map_loader"); + auto res{cli_get_partial_pcd_->async_send_request( + req, [](rclcpp::Client::SharedFuture) {})}; + + std::future_status status = res.wait_for(std::chrono::seconds(0)); + while (status != std::future_status::ready) { + RCLCPP_INFO(this->get_logger(), "waiting response"); + if (!rclcpp::ok()) { + return; + } + status = res.wait_for(std::chrono::seconds(1)); + } + + RCLCPP_INFO( + this->get_logger(), "Loaded partial pcd map from map_loader (grid size: %d)", + static_cast(res.get()->new_pointcloud_with_ids.size())); + + sensor_msgs::msg::PointCloud2 pcd_msg; + for (const auto & pcd_with_id : res.get()->new_pointcloud_with_ids) { + if (pcd_msg.width == 0) { + pcd_msg = pcd_with_id.pointcloud; + } else { + pcd_msg.width += pcd_with_id.pointcloud.width; + pcd_msg.row_step += pcd_with_id.pointcloud.row_step; + pcd_msg.data.insert( + pcd_msg.data.end(), pcd_with_id.pointcloud.data.begin(), pcd_with_id.pointcloud.data.end()); + } + } + + map_frame_ = res.get()->header.frame_id; + map_cloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud); + pcl::fromROSMsg(pcd_msg, *map_cloud_); +} + void MapHeightFitter::on_fit( const RequestHeightFitting::Request::SharedPtr req, - const RequestHeightFitting::Response::SharedPtr res) const + const RequestHeightFitting::Response::SharedPtr res) { const auto & position = req->pose_with_covariance.pose.pose.position; tf2::Vector3 point(position.x, position.y, position.z); std::string req_frame = req->pose_with_covariance.header.frame_id; res->success = false; + if (enable_partial_map_load_) { + get_partial_point_cloud_map(req->pose_with_covariance.pose.pose.position); + } + if (map_cloud_) { try { const auto stamped = tf2_buffer_.lookupTransform(map_frame_, req_frame, tf2::TimePointZero); diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp index 8845b77a78095..5efa97989b95f 100644 --- a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp +++ b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -40,12 +41,17 @@ class MapHeightFitter : public rclcpp::Node std::string map_frame_; pcl::PointCloud::Ptr map_cloud_; rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Client::SharedPtr cli_get_partial_pcd_; rclcpp::Service::SharedPtr srv_fit_; + bool enable_partial_map_load_; + rclcpp::CallbackGroup::SharedPtr callback_group_service_; + void on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void get_partial_point_cloud_map(const geometry_msgs::msg::Point & point); void on_fit( const RequestHeightFitting::Request::SharedPtr req, - const RequestHeightFitting::Response::SharedPtr res) const; + const RequestHeightFitting::Response::SharedPtr res); double get_ground_height(const tf2::Vector3 & point) const; }; diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp index 296a50e0cd3ab..ca4e75671243f 100644 --- a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp +++ b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp @@ -19,7 +19,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::executors::MultiThreadedExecutor executor; auto node = std::make_shared(); executor.add_node(node); executor.spin(); diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index 12e0729cc268b..111d460be737e 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -22,12 +22,6 @@ #include #include -// clang-format off -#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl -#define DEBUG_INFO(...) {if (show_debug_info_) {RCLCPP_INFO(__VA_ARGS__);}} -#define DEBUG_PRINT_MAT(X) {if (show_debug_info_) {std::cout << #X << ": " << X << std::endl;}} - -// clang-format on using std::placeholders::_1; StopFilter::StopFilter(const std::string & node_name, const rclcpp::NodeOptions & node_options) diff --git a/localization/twist2accel/src/twist2accel.cpp b/localization/twist2accel/src/twist2accel.cpp index 85427abce5ec7..a8a7d46f57056 100644 --- a/localization/twist2accel/src/twist2accel.cpp +++ b/localization/twist2accel/src/twist2accel.cpp @@ -22,12 +22,6 @@ #include #include -// clang-format off -#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl -#define DEBUG_INFO(...) {if (show_debug_info_) {RCLCPP_INFO(__VA_ARGS__);}} -#define DEBUG_PRINT_MAT(X) {if (show_debug_info_) {std::cout << #X << ": " << X << std::endl;}} - -// clang-format on using std::placeholders::_1; Twist2Accel::Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & node_options) diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 6bfcffde71d60..4c3b418239a5c 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_node.cpp src/pointcloud_map_loader/pointcloud_map_loader_module.cpp src/pointcloud_map_loader/partial_map_loader_module.cpp + src/pointcloud_map_loader/differential_map_loader_module.cpp src/pointcloud_map_loader/utils.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 1683fd472e74a..c8f5bed19bb3d 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -12,6 +12,7 @@ Currently, it supports the following two types: - Publish raw pointcloud map - Publish downsampled pointcloud map - Send partial pointcloud map loading via ROS 2 service +- Send differential pointcloud map loading via ROS 2 service #### Publish raw pointcloud map (ROS 2 topic) @@ -28,6 +29,13 @@ Here, we assume that the pointcloud maps are divided into grids. Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area. Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. +#### Send differential pointcloud map (ROS 2 service) + +Here, we assume that the pointcloud maps are divided into grids. + +Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs. +Please see [the description of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details. + ### Parameters | Name | Type | Description | Default value | @@ -35,6 +43,7 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com | enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | | enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | | enable_partial_load | bool | A flag to enable partial pointcloud map server | false | +| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | | leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | ### Interfaces @@ -42,6 +51,7 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com - `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map - `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled pointcloud map - `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map +- `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map --- diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index fa94afbf12a0b..8f3ccbff00360 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -3,6 +3,7 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: false + enable_differential_load: false # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp new file mode 100644 index 0000000000000..b42b919edb59d --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -0,0 +1,85 @@ +// Copyright 2022 The Autoware Contributors +// +// 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 "differential_map_loader_module.hpp" + +DifferentialMapLoaderModule::DifferentialMapLoaderModule( + rclcpp::Node * node, const std::map & pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) +{ + get_differential_pcd_maps_service_ = node->create_service( + "service/get_differential_pcd_map", + std::bind( + &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this, + std::placeholders::_1, std::placeholders::_2)); +} + +void DifferentialMapLoaderModule::differentialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area, const std::vector & cached_ids, + GetDifferentialPointCloudMap::Response::SharedPtr & response) const +{ + // iterate over all the available pcd map grids + std::vector should_remove(static_cast(cached_ids.size()), true); + for (const auto & ele : all_pcd_file_metadata_dict_) { + std::string path = ele.first; + PCDFileMetadata metadata = ele.second; + + // assume that the map ID = map path (for now) + std::string map_id = path; + + // skip if the pcd file is not within the queried area + if (!isGridWithinQueriedArea(area, metadata)) continue; + + auto id_in_cached_list = std::find(cached_ids.begin(), cached_ids.end(), map_id); + if (id_in_cached_list != cached_ids.end()) { + int index = id_in_cached_list - cached_ids.begin(); + should_remove[index] = false; + } else { + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = + loadPointCloudMapCellWithID(path, map_id); + response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); + } + } + + for (int i = 0; i < static_cast(cached_ids.size()); ++i) { + if (should_remove[i]) { + response->ids_to_remove.push_back(cached_ids[i]); + } + } +} + +bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( + GetDifferentialPointCloudMap::Request::SharedPtr req, + GetDifferentialPointCloudMap::Response::SharedPtr res) +{ + auto area = req->area; + std::vector cached_ids = req->cached_ids; + differentialAreaLoad(area, cached_ids, res); + res->header.frame_id = "map"; + return true; +} + +autoware_map_msgs::msg::PointCloudMapCellWithID +DifferentialMapLoaderModule::loadPointCloudMapCellWithID( + const std::string path, const std::string map_id) const +{ + sensor_msgs::msg::PointCloud2 pcd; + if (pcl::io::loadPCDFile(path, pcd) == -1) { + RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); + } + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id; + pointcloud_map_cell_with_id.pointcloud = pcd; + pointcloud_map_cell_with_id.cell_id = map_id; + return pointcloud_map_cell_with_id; +} diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp new file mode 100644 index 0000000000000..5d6188c0b1a1f --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -0,0 +1,59 @@ +// Copyright 2022 The Autoware Contributors +// +// 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 POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ +#define POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ + +#include "utils.hpp" + +#include + +#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +class DifferentialMapLoaderModule +{ + using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap; + +public: + explicit DifferentialMapLoaderModule( + rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + +private: + rclcpp::Logger logger_; + + std::map all_pcd_file_metadata_dict_; + rclcpp::Service::SharedPtr get_differential_pcd_maps_service_; + + bool onServiceGetDifferentialPointCloudMap( + GetDifferentialPointCloudMap::Request::SharedPtr req, + GetDifferentialPointCloudMap::Response::SharedPtr res); + void differentialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area_info, const std::vector & cached_ids, + GetDifferentialPointCloudMap::Response::SharedPtr & response) const; + autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + const std::string path, const std::string map_id) const; +}; + +#endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index b42be7308b67b..ecbe481345381 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -52,6 +52,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); + bool enable_differential_load = declare_parameter("enable_differential_load"); if (enable_whole_load) { std::string publisher_name = "output/pointcloud_map"; @@ -65,10 +66,18 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load) { + if (enable_partial_load | enable_differential_load) { pcd_metadata_dict_ = generatePCDMetadata(pcd_paths); + } + + if (enable_partial_load) { partial_map_loader_ = std::make_unique(this, pcd_metadata_dict_); } + + if (enable_differential_load) { + differential_map_loader_ = + std::make_unique(this, pcd_metadata_dict_); + } } std::vector PointCloudMapLoaderNode::getPcdPaths( diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index a7c289dd0f9a8..d321902131a81 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -15,6 +15,7 @@ #ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ #define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#include "differential_map_loader_module.hpp" #include "partial_map_loader_module.hpp" #include "pointcloud_map_loader_module.hpp" @@ -43,6 +44,7 @@ class PointCloudMapLoaderNode : public rclcpp::Node std::unique_ptr pcd_map_loader_; std::unique_ptr downsampled_pcd_map_loader_; std::unique_ptr partial_map_loader_; + std::unique_ptr differential_map_loader_; std::vector getPcdPaths( const std::vector & pcd_paths_or_directory) const; diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index fcf96f828fc9b..f6a8ba4c8a89a 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -86,8 +86,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter float radius_avg; float height_avg; float height_max; + float height_min; uint32_t point_num; uint16_t grid_id; + pcl::PointIndices pcl_indices; + std::vector height_list; PointsCentroid() : radius_sum(0.0f), height_sum(0.0f), radius_avg(0.0f), height_avg(0.0f), point_num(0) @@ -101,8 +104,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter radius_avg = 0.0f; height_avg = 0.0f; height_max = 0.0f; + height_min = 10.0f; point_num = 0; grid_id = 0; + pcl_indices.indices.clear(); + height_list.clear(); } void addPoint(const float radius, const float height) @@ -113,6 +119,13 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter radius_avg = radius_sum / point_num; height_avg = height_sum / point_num; height_max = height_max < height ? height : height_max; + height_min = height_min > height ? height : height_min; + } + void addPoint(const float radius, const float height, const uint index) + { + pcl_indices.indices.push_back(index); + height_list.push_back(height); + addPoint(radius, height); } float getAverageSlope() { return std::atan2(height_avg, radius_avg); } @@ -123,7 +136,12 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter float getMaxHeight() { return height_max; } + float getMinHeight() { return height_min; } + uint16_t getGridId() { return grid_id; } + + pcl::PointIndices getIndices() { return pcl_indices; } + std::vector getHeightList() { return height_list; } }; void filter( @@ -193,18 +211,24 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter void initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids); - void checkContinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list, PointsCentroid & gnd_cluster); - void checkDiscontinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list, PointsCentroid & gnd_cluster); - void checkBreakGndGrid( - PointRef & p, const std::vector & gnd_grids_list, PointsCentroid & gnd_cluster); + void checkContinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); + void checkDiscontinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); + void checkBreakGndGrid(PointRef & p, const std::vector & gnd_grids_list); void classifyPointCloud( std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); void classifyPointCloudGridScan( std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); + /*! + * Re-classifies point of ground cluster based on their height + * @param gnd_cluster Input ground cluster for re-checking + * @param non_ground_threshold Height threshold for ground and non-ground points classification + * @param non_ground_indices Output non-ground PointCloud indices + */ + void recheckGroundCluster( + PointsCentroid & gnd_cluster, const float non_ground_threshold, + pcl::PointIndices & non_ground_indices); /*! * Returns the resulting complementary PointCloud, one with the points kept * and the other removed as indicated in the indices diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 16a6859d09ad9..fa9d3d6d83665 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -178,7 +178,7 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids) { GridCenter curr_gnd_grid; - for (int ind_grid = id - 1 - gnd_grid_buffer_size_; ind_grid < id - 1; ind_grid++) { + for (int ind_grid = id - 1 - gnd_grid_buffer_size_; ind_grid < id - 1; ++ind_grid) { float ind_gnd_z = ind_grid - id + 1 + gnd_grid_buffer_size_; ind_gnd_z *= h / static_cast(gnd_grid_buffer_size_); @@ -194,7 +194,7 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( } void ScanGroundFilterComponent::checkContinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list, PointsCentroid & gnd_cluster) + PointRef & p, const std::vector & gnd_grids_list) { float next_gnd_z = 0.0f; float curr_gnd_slope_rad = 0.0f; @@ -203,7 +203,7 @@ void ScanGroundFilterComponent::checkContinuousGndGrid( float gnd_buff_radius = 0.0f; for (auto it = gnd_grids_list.end() - gnd_grid_buffer_size_ - 1; it < gnd_grids_list.end() - 1; - it++) { + ++it) { gnd_buff_radius += it->radius; gnd_buff_z_mean += it->avg_height; gnd_buff_z_max += it->max_height; @@ -234,14 +234,13 @@ void ScanGroundFilterComponent::checkContinuousGndGrid( if ( abs(p.orig_point->z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || abs(local_slope) <= local_slope_max_angle_rad_) { - gnd_cluster.addPoint(p.radius, p.orig_point->z); p.point_state = PointLabel::GROUND; } else if (p.orig_point->z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { p.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::checkDiscontinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list, PointsCentroid & gnd_cluster) + PointRef & p, const std::vector & gnd_grids_list) { float tmp_delta_max_z = p.orig_point->z - gnd_grids_list.back().max_height; float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; @@ -252,7 +251,6 @@ void ScanGroundFilterComponent::checkDiscontinuousGndGrid( abs(local_slope) < local_slope_max_angle_rad_ || abs(tmp_delta_avg_z) < non_ground_height_threshold_ || abs(tmp_delta_max_z) < non_ground_height_threshold_) { - gnd_cluster.addPoint(p.radius, p.orig_point->z); p.point_state = PointLabel::GROUND; } else if (local_slope > global_slope_max_angle_rad_) { p.point_state = PointLabel::NON_GROUND; @@ -260,24 +258,36 @@ void ScanGroundFilterComponent::checkDiscontinuousGndGrid( } void ScanGroundFilterComponent::checkBreakGndGrid( - PointRef & p, const std::vector & gnd_grids_list, PointsCentroid & gnd_cluster) + PointRef & p, const std::vector & gnd_grids_list) { float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius); if (abs(local_slope) < global_slope_max_angle_rad_) { - gnd_cluster.addPoint(p.radius, p.orig_point->z); p.point_state = PointLabel::GROUND; } else if (local_slope > global_slope_max_angle_rad_) { p.point_state = PointLabel::NON_GROUND; } } +void ScanGroundFilterComponent::recheckGroundCluster( + PointsCentroid & gnd_cluster, const float non_ground_threshold, + pcl::PointIndices & non_ground_indices) +{ + const float min_gnd_height = gnd_cluster.getMinHeight(); + pcl::PointIndices gnd_indices = gnd_cluster.getIndices(); + std::vector height_list = gnd_cluster.getHeightList(); + for (size_t i = 0; i < height_list.size(); ++i) { + if (height_list.at(i) >= min_gnd_height + non_ground_threshold) { + non_ground_indices.indices.push_back(gnd_indices.indices.at(i)); + } + } +} void ScanGroundFilterComponent::classifyPointCloudGridScan( std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); - for (size_t i = 0; i < in_radial_ordered_clouds.size(); i++) { + for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) { PointsCentroid ground_cluster; ground_cluster.initialize(); std::vector gnd_grids; @@ -296,7 +306,7 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( bool initialized_first_gnd_grid = false; bool prev_list_init = false; - for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); j++) { + for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { p = &in_radial_ordered_clouds[i][j]; float global_slope_p = std::atan(p->orig_point->z / p->radius); float non_ground_height_threshold_local = non_ground_height_threshold_; @@ -317,7 +327,7 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( if ( !initialized_first_gnd_grid && abs(global_slope_p) < global_slope_max_angle_rad_ && abs(p->orig_point->z) < non_ground_height_threshold_local) { - ground_cluster.addPoint(p->radius, p->orig_point->z); + ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); p->point_state = PointLabel::GROUND; initialized_first_gnd_grid = static_cast(p->grid_id - prev_p->grid_id); prev_p = p; @@ -346,6 +356,7 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( // move to new grid if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) { // check if the prev grid have ground point cloud + recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices); curr_gnd_grid.radius = ground_cluster.getAverageRadius(); curr_gnd_grid.avg_height = ground_cluster.getAverageHeight(); curr_gnd_grid.max_height = ground_cluster.getMaxHeight(); @@ -382,17 +393,17 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( if ( p->grid_id < next_gnd_grid_id_thresh && p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { - checkContinuousGndGrid(*p, gnd_grids, ground_cluster); + checkContinuousGndGrid(*p, gnd_grids); - } else if ( - p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size || - p->radius < grid_mode_switch_radius_ * 2.0f) { - checkDiscontinuousGndGrid(*p, gnd_grids, ground_cluster); + } else if (p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { + checkDiscontinuousGndGrid(*p, gnd_grids); } else { - checkBreakGndGrid(*p, gnd_grids, ground_cluster); + checkBreakGndGrid(*p, gnd_grids); } if (p->point_state == PointLabel::NON_GROUND) { out_no_ground_indices.indices.push_back(p->orig_index); + } else if (p->point_state == PointLabel::GROUND) { + ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); } prev_p = p; } @@ -411,7 +422,7 @@ void ScanGroundFilterComponent::classifyPointCloud( // point classification algorithm // sweep through each radial division - for (size_t i = 0; i < in_radial_ordered_clouds.size(); i++) { + for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) { float prev_gnd_radius = 0.0f; float prev_gnd_slope = 0.0f; float points_distance = 0.0f; @@ -420,7 +431,7 @@ void ScanGroundFilterComponent::classifyPointCloud( PointLabel prev_point_label = PointLabel::INIT; pcl::PointXYZ prev_gnd_point(0, 0, 0); // loop through each point in the radial div - for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); j++) { + for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { const float global_slope_max_angle = global_slope_max_angle_rad_; const float local_slope_max_angle = local_slope_max_angle_rad_; auto * p = &in_radial_ordered_clouds[i][j]; diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index 6b07c73e93b3d..ed3bfb9a040b6 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -8,7 +8,8 @@ traffic_light_classifier is a package for classifying traffic light labels using ### cnn_classifier -Traffic light labels are classified by MobileNetV2. +Traffic light labels are classified by MobileNetV2. +Totally 37600 (26300 for training, 6800 for evaluation and 4500 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. ### hsv_classifier diff --git a/planning/behavior_path_planner/behavior_path_planner_side-shift.md b/planning/behavior_path_planner/behavior_path_planner_side-shift.md new file mode 100644 index 0000000000000..22c55ddc7370b --- /dev/null +++ b/planning/behavior_path_planner/behavior_path_planner_side-shift.md @@ -0,0 +1,92 @@ +# Side shift Module + +(For remote control) Shift the path to left or right according to an external instruction. + +## Flowchart + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title callback function of lateral offset input +start + +partition onLateralOffset { +:**INPUT** double new_lateral_offset; + +if (abs(inserted_lateral_offset_ - new_lateral_offset) < 1e-4) then ( true) + stop +else ( false) + if (interval from last request is too short) then ( no) + else ( yes) + :requested_lateral_offset_ = new_lateral_offset \n lateral_offset_change_request_ = true; + endif +stop +@enduml +``` + +```plantuml --> +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title path generation + +start +partition plan { +if (lateral_offset_change_request_ == true \n && \n (shifting_status_ == BEFORE_SHIFT \n || \n shifting_status_ == AFTER_SHIFT)) then ( true) + partition replace-shift-line { + if ( shift line is inserted in the path ) then ( yes) + :erase left shift line; + else ( no) + endif + :calcShiftLines; + :add new shift lines; + :inserted_lateral_offset_ = requested_lateral_offset_ \n inserted_shift_lines_ = new_shift_line; + } +else( false) +endif +stop +@enduml +``` + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title update state + +start +partition updateState { + :last_sp = path_shifter_.getLastShiftLine(); + note left + get furthest shift lines + end note + :calculate max_planned_shift_length; + note left + calculate furthest shift length of previous shifted path + end note + if (abs(inserted_lateral_offset_ - inserted_shift_line_.end_shift_length) < 1e-4 \n && \n abs(max_planned_shift_length) < 1e-4 \n && \n abs(requested_lateral_offset_) < 1e-4) then ( true) + :current_state_ = BT::NodeStatus::SUCCESS; + else (false) + if (ego's position is behind of shift line's start point) then( yes) + :shifting_status_ = BEFORE_SHIFT; + else ( no) + if ( ego's position is between shift line's start point and end point) then (yes) + :shifting_status_ = SHIFTING; + else( no) + :shifting_status_ = AFTER_SHIFT; + endif + endif + :current_state_ = BT::NodeStatus::RUNNING; + endif + stop +} + +@enduml +``` diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 43be10146c7ac..5e2e4673166da 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -50,6 +50,9 @@ min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] max_avoidance_acceleration: 0.5 # [m/s2] + # bound clipping for objects + enable_bound_clipping: false + # for debug publish_debug_marker: false print_debug_info: false diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 30dc2eb73489a..68cc14a8299a1 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -6,6 +6,7 @@ backward_length_buffer_for_end_of_pull_over: 5.0 backward_length_buffer_for_end_of_pull_out: 5.0 minimum_lane_change_length: 12.0 + minimum_lane_change_prepare_distance: 2.0 # [m] minimum_pull_over_length: 16.0 drivable_area_resolution: 0.1 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 366ecc095d0b9..48ac09b77e803 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -80,6 +81,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; using tier4_planning_msgs::msg::PathChangeModule; using tier4_planning_msgs::msg::Scenario; +using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; class BehaviorPathPlannerNode : public rclcpp::Node @@ -99,6 +101,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr path_candidate_publisher_; rclcpp::Publisher::SharedPtr turn_signal_publisher_; rclcpp::Publisher::SharedPtr hazard_signal_publisher_; + rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::TimerBase::SharedPtr timer_; std::shared_ptr planner_data_; @@ -169,6 +172,13 @@ class BehaviorPathPlannerNode : public rclcpp::Node bool skipSmoothGoalConnection( const std::vector> & statuses) const; + /** + * @brief skip smooth goal connection + */ + void computeTurnSignal( + const std::shared_ptr planner_data, const PathWithLaneId & path, + const BehaviorModuleOutput & output); + // debug rclcpp::Publisher::SharedPtr debug_drivable_area_lanelets_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; @@ -184,6 +194,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node */ void publish_steering_factor(const TurnIndicatorsCommand & turn_signal); + /** + * @brief publish left and right bound + */ + void publish_bounds(const PathWithLaneId & path); + /** * @brief publish debug messages */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 79039e606252d..0543c9319f47d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index f599deab7bb24..132e01f5d91d3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -25,6 +25,8 @@ struct BehaviorPathPlannerParameters double backward_length_buffer_for_end_of_pull_over; double backward_length_buffer_for_end_of_pull_out; double minimum_lane_change_length; + double minimum_lane_change_prepare_distance; + double minimum_pull_over_length; double minimum_pull_out_length; double drivable_area_resolution; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index ddd20f09aa5d7..a4783a445b177 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -275,7 +275,7 @@ class AvoidanceModule : public SceneModuleInterface // -- path generation -- ShiftedPath generateAvoidancePath(PathShifter & shifter) const; - void generateExtendedDrivableArea(ShiftedPath * shifted_path) const; + void generateExtendedDrivableArea(PathWithLaneId & path) const; // -- velocity planning -- std::shared_ptr ego_velocity_starting_avoidance_ptr_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 91018cb619747..25b29352973c1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -179,6 +179,9 @@ struct AvoidanceParameters double drivable_area_right_bound_offset; double drivable_area_left_bound_offset; + // clip left and right bounds for objects + bool enable_bound_clipping{false}; + // debug bool publish_debug_marker = false; bool print_debug_info = false; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp index 530238eb0ae67..ce04323a775dc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp @@ -25,6 +25,14 @@ namespace behavior_path_planner { using behavior_path_planner::PlannerData; + +struct PolygonPoint +{ + geometry_msgs::msg::Point point; + double lat_dist_to_bound; + double lon_dist; +}; + bool isOnRight(const ObjectData & obj); double calcShiftLength( @@ -73,6 +81,26 @@ std::vector getUuidStr(const ObjectDataArray & objs); Polygon2d createEnvelopePolygon( const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer); + +void getEdgePoints( + const Polygon2d & object_polygon, const double threshold, std::vector & edge_points); + +void getEdgePoints( + const std::vector & bound, const std::vector & edge_points, + const double lat_dist_to_path, std::vector & edge_points_data, + size_t & start_segment_idx, size_t & end_segment_idx); + +void sortPolygonPoints( + const std::vector & points, std::vector & sorted_points); + +std::vector updateBoundary( + const std::vector & original_bound, const std::vector & points, + const size_t start_segment_idx, const size_t end_segment_idx); + +void generateDrivableArea( + PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + const std::shared_ptr planner_data, const ObjectDataArray & objects, + const bool enable_bound_clipping); } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index f9ce032760e45..be14bb120aa42 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -27,7 +27,6 @@ struct LaneChangeParameters { double lane_change_prepare_duration{2.0}; double lane_changing_duration{4.0}; - double minimum_lane_change_prepare_distance{4.0}; double lane_change_finish_judge_buffer{3.0}; double minimum_lane_change_velocity{5.6}; double prediction_time_resolution{0.5}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 9ce619c189146..1e15ee93a7d19 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -110,8 +110,9 @@ ShiftLine getLaneChangeShiftLine( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double & prepare_distance, - const double & lane_changing_distance, const double & forward_path_length); + const Pose & lane_changing_start_pose, const double prepare_distance, + const double lane_changing_distance, const double forward_path_length, + const double lane_changing_speed); PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index c6703a589e7d6..52073c7f50cbe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -34,6 +34,8 @@ using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; using tier4_planning_msgs::msg::LateralOffset; +enum class SideShiftStatus { STOP = 0, BEFORE_SHIFT, SHIFTING, AFTER_SHIFT }; + struct SideShiftParameters { double time_to_start_shifting; @@ -87,7 +89,7 @@ class SideShiftModule : public SceneModuleInterface ShiftLine calcShiftLine() const; - bool addShiftLine(); + void replaceShiftLine(); // const methods void publishPath(const PathWithLaneId & path) const; @@ -100,8 +102,17 @@ class SideShiftModule : public SceneModuleInterface lanelet::ConstLanelets current_lanelets_; SideShiftParameters parameters_; - // Current lateral offset to shift the reference path. - double lateral_offset_{0.0}; + // Requested lateral offset to shift the reference path. + double requested_lateral_offset_{0.0}; + + // Inserted lateral offset to shift the reference path. + double inserted_lateral_offset_{0.0}; + + // Inserted shift lines in the path + ShiftLine inserted_shift_line_; + + // Shift status + SideShiftStatus shift_status_; // Flag to check lateral offset change is requested bool lateral_offset_change_request_{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 09ea9630983b4..3697ad5576b1c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -66,7 +66,6 @@ using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; -using nav_msgs::msg::OccupancyGrid; using route_handler::RouteHandler; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; @@ -289,16 +288,13 @@ std::vector generateDrivableLanes(const lanelet::ConstLanelets & std::vector generateDrivableLanesWithShoulderLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); -void occupancyGridToImage(const OccupancyGrid & occupancy_grid, cv::Mat * cv_image); +boost::optional getOverlappedLaneletId(const std::vector & lanes); +std::vector cutOverlappedLanes( + PathWithLaneId & path, const std::vector & lanes); -void imageToOccupancyGrid(const cv::Mat & cv_image, OccupancyGrid * occupancy_grid); - -cv::Point toCVPoint( - const Point & geom_point, const double width_m, const double height_m, const double resolution); - -OccupancyGrid generateDrivableArea( - const PathWithLaneId & path, const std::vector & lanes, const double resolution, - const double vehicle_length, const std::shared_ptr planner_data); +void generateDrivableArea( + PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + const std::shared_ptr planner_data); lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( const std::shared_ptr & planner_data); @@ -481,6 +477,12 @@ bool isSafeInFreeSpaceCollisionCheck( const double front_decel, const double rear_decel, CollisionCheckDebug & debug); bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); + +double calcTotalLaneChangeDistanceWithBuffer(const BehaviorPathPlannerParameters & common_param); + +double calcLaneChangeBuffer( + const BehaviorPathPlannerParameters & common_param, const int num_lane_change, + const double length_to_intersection); } // namespace behavior_path_planner::util #endif // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index ab9f506de00a7..4da5933afa7af 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -75,6 +75,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod create_publisher("~/drivable_area_boundary", 1); } + bound_publisher_ = create_publisher("~/debug/bound", 1); + // subscriber velocity_subscriber_ = create_subscription( "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1), @@ -205,6 +207,9 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.backward_length_buffer_for_end_of_pull_out = declare_parameter("backward_length_buffer_for_end_of_pull_out", 5.0); p.minimum_lane_change_length = declare_parameter("minimum_lane_change_length", 8.0); + p.minimum_lane_change_prepare_distance = + declare_parameter("minimum_lane_change_prepare_distance", 2.0); + p.minimum_pull_over_length = declare_parameter("minimum_pull_over_length", 15.0); p.drivable_area_resolution = declare_parameter("drivable_area_resolution"); p.drivable_lane_forward_length = declare_parameter("drivable_lane_forward_length"); @@ -332,6 +337,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); + p.enable_bound_clipping = dp("enable_bound_clipping", false); + p.avoidance_execution_lateral_threshold = dp("avoidance_execution_lateral_threshold", 0.499); return p; @@ -359,7 +366,6 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() LaneChangeParameters p{}; p.lane_change_prepare_duration = dp("lane_change_prepare_duration", 2.0); p.lane_changing_duration = dp("lane_changing_duration", 4.0); - p.minimum_lane_change_prepare_distance = dp("minimum_lane_change_prepare_distance", 4.0); p.lane_change_finish_judge_buffer = dp("lane_change_finish_judge_buffer", 3.0); p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 5.6); p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); @@ -612,7 +618,11 @@ void BehaviorPathPlannerNode::run() // update planner data planner_data_->self_pose = self_pose_listener_.getCurrentPose(); - const auto planner_data = planner_data_; + const auto planner_data = std::make_shared(*planner_data_); + + // unlock planner data + mutex_pd_.unlock(); + // run behavior planner const auto output = bt_manager_->run(planner_data); @@ -621,7 +631,12 @@ void BehaviorPathPlannerNode::run() // update planner data planner_data_->prev_output_path = path; - mutex_pd_.unlock(); + + // compute turn signal + computeTurnSignal(planner_data, *path, output); + + // publish drivable bounds + publish_bounds(*path); const size_t target_idx = findEgoIndex(path->points); util::clipPathLength(*path, target_idx, planner_data_->parameters); @@ -636,26 +651,6 @@ void BehaviorPathPlannerNode::run() const auto path_candidate = getPathCandidate(output, planner_data); path_candidate_publisher_->publish(util::toPath(*path_candidate)); - // for turn signal - { - TurnIndicatorsCommand turn_signal; - HazardLightsCommand hazard_signal; - if (output.turn_signal_info.hazard_signal.command == HazardLightsCommand::ENABLE) { - turn_signal.command = TurnIndicatorsCommand::DISABLE; - hazard_signal.command = output.turn_signal_info.hazard_signal.command; - } else { - turn_signal = - turn_signal_decider_.getTurnSignal(planner_data, *path, output.turn_signal_info); - hazard_signal.command = HazardLightsCommand::DISABLE; - } - turn_signal.stamp = get_clock()->now(); - hazard_signal.stamp = get_clock()->now(); - turn_signal_publisher_->publish(turn_signal); - hazard_signal_publisher_->publish(hazard_signal); - - publish_steering_factor(turn_signal); - } - publishSceneModuleDebugMsg(); if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { @@ -668,6 +663,27 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } +void BehaviorPathPlannerNode::computeTurnSignal( + const std::shared_ptr planner_data, const PathWithLaneId & path, + const BehaviorModuleOutput & output) +{ + TurnIndicatorsCommand turn_signal; + HazardLightsCommand hazard_signal; + if (output.turn_signal_info.hazard_signal.command == HazardLightsCommand::ENABLE) { + turn_signal.command = TurnIndicatorsCommand::DISABLE; + hazard_signal.command = output.turn_signal_info.hazard_signal.command; + } else { + turn_signal = turn_signal_decider_.getTurnSignal(planner_data, path, output.turn_signal_info); + hazard_signal.command = HazardLightsCommand::DISABLE; + } + turn_signal.stamp = get_clock()->now(); + hazard_signal.stamp = get_clock()->now(); + turn_signal_publisher_->publish(turn_signal); + hazard_signal_publisher_->publish(hazard_signal); + + publish_steering_factor(turn_signal); +} + void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsCommand & turn_signal) { const auto [intersection_flag, approaching_intersection_flag] = @@ -698,6 +714,39 @@ void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsComman steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now()); } +void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path) +{ + constexpr double scale_x = 0.1; + constexpr double scale_y = 0.1; + constexpr double scale_z = 0.1; + constexpr double color_r = 0.0 / 256.0; + constexpr double color_g = 148.0 / 256.0; + constexpr double color_b = 205.0 / 256.0; + constexpr double color_a = 0.999; + + const auto current_time = path.header.stamp; + auto left_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "left_bound", 0L, Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + for (const auto lb : path.left_bound) { + left_marker.points.push_back(lb); + } + + auto right_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "right_bound", 0L, Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + for (const auto rb : path.right_bound) { + right_marker.points.push_back(rb); + } + + MarkerArray msg; + msg.markers.push_back(left_marker); + msg.markers.push_back(right_marker); + bound_publisher_->publish(msg); +} + void BehaviorPathPlannerNode::publishSceneModuleDebugMsg() { { @@ -886,8 +935,18 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection( const auto goal = planner_data_->route_handler->getGoalPose(); const auto goal_lane_id = planner_data_->route_handler->getGoalLaneId(); + Pose refined_goal{}; + { + lanelet::ConstLanelet goal_lanelet; + if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) { + refined_goal = util::refineGoal(goal, goal_lanelet); + } else { + refined_goal = goal; + } + } + auto refined_path = util::refinePathForGoal( - planner_data_->parameters.refine_goal_search_radius_range, M_PI * 0.5, path, goal, + planner_data_->parameters.refine_goal_search_radius_range, M_PI * 0.5, path, refined_goal, goal_lane_id); refined_path.header.frame_id = "map"; refined_path.header.stamp = this->now(); diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 110df193abfde..20d9cfc0fc3b8 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -123,7 +123,8 @@ Path toPath(const PathWithLaneId & input) { Path output{}; output.header = input.header; - output.drivable_area = input.drivable_area; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; output.points.resize(input.points.size()); for (size_t i = 0; i < input.points.size(); ++i) { output.points.at(i) = input.points.at(i).point; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 2893c1af72999..2c7ecf6356eb9 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1902,7 +1902,7 @@ double AvoidanceModule::getLeftShiftBound() const // TODO(murooka) freespace during turning in intersection where there is no neighbor lanes // NOTE: Assume that there is no situation where there is an object in the middle lane of more than // two lanes since which way to avoid is not obvious -void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) const +void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const { const auto has_same_lane = [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) { @@ -2007,15 +2007,17 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c drivable_lanes.push_back(current_drivable_lanes); } - drivable_lanes = util::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, + const auto shorten_lanes = util::cutOverlappedLanes(path, drivable_lanes); + + const auto extended_lanes = util::expandLanelets( + shorten_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset, {"road_border"}); { const auto & p = planner_data_->parameters; - shifted_path->path.drivable_area = util::generateDrivableArea( - shifted_path->path, drivable_lanes, p.drivable_area_resolution, p.vehicle_length, - planner_data_); + generateDrivableArea( + path, drivable_lanes, p.vehicle_length, planner_data_, avoidance_data_.target_objects, + parameters_->enable_bound_clipping); } } @@ -2259,9 +2261,6 @@ BehaviorModuleOutput AvoidanceModule::plan() auto avoidance_path = generateAvoidancePath(path_shifter_); debug_data_.output_shift = avoidance_path.shift_length; - // Drivable area generation. - generateExtendedDrivableArea(&avoidance_path); - // modify max speed to prevent acceleration in avoidance maneuver. modifyPathVelocityToPreventAccelerationOnAvoidance(avoidance_path); @@ -2291,6 +2290,9 @@ BehaviorModuleOutput AvoidanceModule::plan() const size_t ego_idx = findEgoIndex(output.path->points); util::clipPathLength(*output.path, ego_idx, planner_data_->parameters); + // Drivable area generation. + generateExtendedDrivableArea(*output.path); + DEBUG_PRINT("exit plan(): set prev output (back().lat = %f)", prev_output_.shift_length.back()); updateRegisteredRTCStatus(avoidance_path.path); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index 31f3ce7fa32db..ffe2d0a4b83a2 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -342,4 +342,158 @@ Polygon2d createEnvelopePolygon( return offset_polygons.front(); } +void getEdgePoints( + const Polygon2d & object_polygon, const double threshold, std::vector & edge_points) +{ + if (object_polygon.outer().size() < 2) { + return; + } + + const size_t num_points = object_polygon.outer().size(); + for (size_t i = 0; i < num_points - 1; ++i) { + const auto & curr_p = object_polygon.outer().at(i); + const auto & next_p = object_polygon.outer().at(i + 1); + const auto & prev_p = + i == 0 ? object_polygon.outer().at(num_points - 2) : object_polygon.outer().at(i - 1); + const Eigen::Vector2d current_to_next(next_p.x() - curr_p.x(), next_p.y() - curr_p.y()); + const Eigen::Vector2d current_to_prev(prev_p.x() - curr_p.x(), prev_p.y() - curr_p.y()); + const double inner_val = current_to_next.dot(current_to_prev); + if (std::fabs(inner_val) > threshold) { + continue; + } + + const auto edge_point = tier4_autoware_utils::createPoint(curr_p.x(), curr_p.y(), 0.0); + edge_points.push_back(edge_point); + } +} + +void sortPolygonPoints( + const std::vector & points, std::vector & sorted_points) +{ + sorted_points = points; + if (points.size() <= 2) { + // sort data based on longitudinal distance to the boundary + std::sort( + sorted_points.begin(), sorted_points.end(), + [](const PolygonPoint & a, const PolygonPoint & b) { return a.lon_dist < b.lon_dist; }); + return; + } + + // sort data based on lateral distance to the boundary + std::sort( + sorted_points.begin(), sorted_points.end(), [](const PolygonPoint & a, const PolygonPoint & b) { + return std::fabs(a.lat_dist_to_bound) > std::fabs(b.lat_dist_to_bound); + }); + PolygonPoint first_point; + PolygonPoint second_point; + if (sorted_points.at(0).lon_dist < sorted_points.at(1).lon_dist) { + first_point = sorted_points.at(0); + second_point = sorted_points.at(1); + } else { + first_point = sorted_points.at(1); + second_point = sorted_points.at(0); + } + + for (size_t i = 2; i < sorted_points.size(); ++i) { + const auto & next_point = sorted_points.at(i); + if (next_point.lon_dist < first_point.lon_dist) { + sorted_points = {next_point, first_point, second_point}; + return; + } else if (second_point.lon_dist < next_point.lon_dist) { + sorted_points = {first_point, second_point, next_point}; + return; + } + } + + sorted_points = {first_point, second_point}; +} + +void getEdgePoints( + const std::vector & bound, const std::vector & edge_points, + const double lat_dist_to_path, std::vector & edge_points_data, + size_t & start_segment_idx, size_t & end_segment_idx) +{ + for (const auto & edge_point : edge_points) { + const size_t segment_idx = motion_utils::findNearestSegmentIndex(bound, edge_point); + start_segment_idx = std::min(start_segment_idx, segment_idx); + end_segment_idx = std::max(end_segment_idx, segment_idx); + + PolygonPoint edge_point_data; + edge_point_data.point = edge_point; + edge_point_data.lat_dist_to_bound = motion_utils::calcLateralOffset(bound, edge_point); + edge_point_data.lon_dist = motion_utils::calcSignedArcLength(bound, 0, edge_point); + if (lat_dist_to_path >= 0.0 && edge_point_data.lat_dist_to_bound > 0.0) { + continue; + } else if (lat_dist_to_path < 0.0 && edge_point_data.lat_dist_to_bound < 0.0) { + continue; + } + + edge_points_data.push_back(edge_point_data); + } +} + +std::vector updateBoundary( + const std::vector & original_bound, const std::vector & points, + const size_t start_segment_idx, const size_t end_segment_idx) +{ + if (start_segment_idx >= end_segment_idx) { + return original_bound; + } + + std::vector updated_bound; + std::copy( + original_bound.begin(), original_bound.begin() + start_segment_idx + 1, + std::back_inserter(updated_bound)); + for (size_t i = 0; i < points.size(); ++i) { + updated_bound.push_back(points.at(i).point); + } + std::copy( + original_bound.begin() + end_segment_idx + 1, original_bound.end(), + std::back_inserter(updated_bound)); + return updated_bound; +} + +void generateDrivableArea( + PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + const std::shared_ptr planner_data, const ObjectDataArray & objects, + const bool enable_bound_clipping) +{ + util::generateDrivableArea(path, lanes, vehicle_length, planner_data); + if (objects.empty() || !enable_bound_clipping) { + return; + } + + path.left_bound = motion_utils::resamplePointVector(path.left_bound, 1.0, true); + path.right_bound = motion_utils::resamplePointVector(path.right_bound, 1.0, true); + + for (const auto & object : objects) { + const auto & obj_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto & obj_poly = object.envelope_poly; + constexpr double threshold = 0.01; + + // get edge points + std::vector edge_points; + getEdgePoints(obj_poly, threshold, edge_points); + + // get boundary + const double lat_dist_to_path = motion_utils::calcLateralOffset(path.points, obj_pose.position); + auto & bound = lat_dist_to_path < 0.0 ? path.right_bound : path.left_bound; + + size_t start_segment_idx = (bound.size() == 1 ? 0 : (bound.size() - 2)); + size_t end_segment_idx = 0; + + // get edge points data + std::vector edge_points_data; + getEdgePoints( + bound, edge_points, lat_dist_to_path, edge_points_data, start_segment_idx, end_segment_idx); + + // sort points + std::vector sorted_points; + sortPolygonPoints(edge_points_data, sorted_points); + + // update boundary + bound = updateBoundary(bound, sorted_points, start_segment_idx, end_segment_idx); + } +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 17199c68fd895..dc9a19e4da7d4 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -143,8 +143,7 @@ BT::NodeStatus LaneChangeModule::updateState() BehaviorModuleOutput LaneChangeModule::plan() { - constexpr double resample_interval{1.0}; - auto path = util::resamplePathWithSpline(status_.lane_change_path.path, resample_interval); + auto path = status_.lane_change_path.path; if (!isValidPath(path)) { status_.is_safe = false; @@ -264,23 +263,21 @@ PathWithLaneId LaneChangeModule::getReferencePath() const *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, common_parameters.forward_path_length, common_parameters, optional_lengths); } - const double & buffer = - common_parameters.backward_length_buffer_for_end_of_lane; // buffer for min_lane_change_length + const double lane_change_buffer = - num_lane_change * (common_parameters.minimum_lane_change_length + buffer) + optional_lengths; + util::calcLaneChangeBuffer(common_parameters, num_lane_change, optional_lengths); reference_path = util::setDecelerationVelocity( *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, lane_change_buffer); const auto drivable_lanes = util::generateDrivableLanes(current_lanes); + const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); const auto expanded_lanes = util::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, + shorten_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset); - - reference_path.drivable_area = util::generateDrivableArea( - reference_path, expanded_lanes, common_parameters.drivable_area_resolution, - common_parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); return reference_path; } @@ -415,9 +412,7 @@ bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const bool LaneChangeModule::isNearEndOfLane() const { const auto & current_pose = getEgoPose(); - const auto minimum_lane_change_length = planner_data_->parameters.minimum_lane_change_length; - const auto end_of_lane_buffer = planner_data_->parameters.backward_length_buffer_for_end_of_lane; - const double threshold = end_of_lane_buffer + minimum_lane_change_length; + const double threshold = util::calcTotalLaneChangeDistanceWithBuffer(planner_data_->parameters); return std::max(0.0, util::getDistanceToEndOfLane(current_pose, status_.current_lanes)) < threshold; @@ -620,13 +615,11 @@ void LaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path) const auto & route_handler = planner_data_->route_handler; const auto drivable_lanes = lane_change_utils::generateDrivableLanes( *route_handler, status_.current_lanes, status_.lane_change_lanes); + const auto shorten_lanes = util::cutOverlappedLanes(path, drivable_lanes); const auto expanded_lanes = util::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, + shorten_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset); - - const double & resolution = common_parameters.drivable_area_resolution; - path.drivable_area = util::generateDrivableArea( - path, expanded_lanes, resolution, common_parameters.vehicle_length, planner_data_); + util::generateDrivableArea(path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 9b3e65ec92a87..ffad3231eee05 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/scene_module/lane_change/util.hpp" +#include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" #include "behavior_path_planner/utilities.hpp" @@ -177,7 +178,7 @@ LaneChangePaths getLaneChangePaths( const auto & lane_change_prepare_duration = parameter.lane_change_prepare_duration; const auto & lane_changing_duration = parameter.lane_changing_duration; const auto & minimum_lane_change_prepare_distance = - parameter.minimum_lane_change_prepare_distance; + common_parameter.minimum_lane_change_prepare_distance; const auto & minimum_lane_change_length = common_parameter.minimum_lane_change_length; const auto & minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; const auto & maximum_deceleration = parameter.maximum_deceleration; @@ -237,7 +238,7 @@ LaneChangePaths getLaneChangePaths( const PathWithLaneId target_lane_reference_path = getReferencePathFromTargetLane( route_handler, target_lanelets, lane_changing_start_pose, prepare_distance, - lane_changing_distance, forward_path_length); + lane_changing_distance, forward_path_length, lane_changing_speed); const ShiftLine shift_line = getLaneChangeShiftLine( prepare_segment_reference, lane_changing_segment_reference, target_lanelets, @@ -490,8 +491,9 @@ bool isLaneChangePathSafe( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double & prepare_distance, - const double & lane_changing_distance, const double & forward_path_length) + const Pose & lane_changing_start_pose, const double prepare_distance, + const double lane_changing_distance, const double forward_path_length, + const double lane_changing_speed) { const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -502,7 +504,14 @@ PathWithLaneId getReferencePathFromTargetLane( lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); s_end = std::min(s_end, goal_arc_coordinates.length); } - return route_handler.getCenterLinePath(target_lanes, s_start, s_end); + const auto lane_changing_reference_path = + route_handler.getCenterLinePath(target_lanes, s_start, s_end); + + constexpr auto min_resampling_points{30.0}; + constexpr auto resampling_dt{0.2}; + const auto resample_interval = + std::max(lane_changing_distance / min_resampling_points, lane_changing_speed * resampling_dt); + return util::resamplePathWithSpline(lane_changing_reference_path, resample_interval); } PathWithLaneId getReferencePathFromTargetLane( diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index 3c2c0da5f0634..6d958cfc9810c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -128,21 +128,21 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const p, optional_lengths); } - const double buffer = p.backward_length_buffer_for_end_of_lane; const double lane_change_buffer = - num_lane_change * (p.minimum_lane_change_length + buffer) + optional_lengths; + util::calcLaneChangeBuffer(p, num_lane_change, optional_lengths); reference_path = util::setDecelerationVelocity( *route_handler, reference_path, current_lanes, parameters_.lane_change_prepare_duration, lane_change_buffer); } + const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); + const auto expanded_lanes = util::expandLanelets( - drivable_lanes, parameters_.drivable_area_left_bound_offset, + shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - reference_path.drivable_area = util::generateDrivableArea( - reference_path, expanded_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); + util::generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, planner_data_); return reference_path; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 0dca71c1a895f..5ebbf2e415470 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -185,12 +185,12 @@ BehaviorModuleOutput PullOutModule::plan() path = status_.backward_path; } + const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); const auto expanded_lanes = util::expandLanelets( - status_.lanes, parameters_.drivable_area_left_bound_offset, + shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - path.drivable_area = util::generateDrivableArea( - path, expanded_lanes, planner_data_->parameters.drivable_area_resolution, - planner_data_->parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); output.path = std::make_shared(path); output.turn_signal_info = calcTurnSignalInfo(); @@ -286,9 +286,8 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() parameters_.drivable_area_right_bound_offset); auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path; - candidate_path.drivable_area = util::generateDrivableArea( - candidate_path, expanded_lanes, planner_data_->parameters.drivable_area_resolution, - planner_data_->parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + candidate_path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); auto stop_path = candidate_path; for (auto & p : stop_path.points) { p.point.longitudinal_velocity_mps = 0.0; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index 77e6899609711..b72558260ed75 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -91,9 +91,9 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) } // Generate drivable area - const double resolution = common_parameters.drivable_area_resolution; - shift_path.drivable_area = util::generateDrivableArea( - shift_path, drivable_lanes, resolution, common_parameters.vehicle_length, planner_data_); + const auto shorten_lanes = util::cutOverlappedLanes(shift_path, drivable_lanes); + util::generateDrivableArea( + shift_path, shorten_lanes, common_parameters.vehicle_length, planner_data_); shift_path.header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 55b1a5ebe17c1..29fcf8e12233c 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -438,11 +438,11 @@ BehaviorModuleOutput PullOverModule::plan() for (size_t i = status_.current_path_idx; i < status_.pull_over_path.partial_paths.size(); ++i) { auto & path = status_.pull_over_path.partial_paths.at(i); const auto p = planner_data_->parameters; + const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); const auto lane = util::expandLanelets( - status_.lanes, parameters_.drivable_area_left_bound_offset, + shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - path.drivable_area = util::generateDrivableArea( - path, lane, p.drivable_area_resolution, p.vehicle_length, planner_data_); + util::generateDrivableArea(path, lane, p.vehicle_length, planner_data_); } BehaviorModuleOutput output; @@ -610,13 +610,12 @@ PathWithLaneId PullOverModule::getReferencePath() const -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); + const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); const auto lanes = util::expandLanelets( - drivable_lanes, parameters_.drivable_area_left_bound_offset, + shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - - reference_path.drivable_area = util::generateDrivableArea( - reference_path, lanes, common_parameters.drivable_area_resolution, - common_parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + reference_path, lanes, common_parameters.vehicle_length, planner_data_); return reference_path; } @@ -659,13 +658,12 @@ PathWithLaneId PullOverModule::generateStopPath() const } const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); + const auto shorten_lanes = util::cutOverlappedLanes(stop_path, drivable_lanes); const auto lanes = util::expandLanelets( - drivable_lanes, parameters_.drivable_area_left_bound_offset, + shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - stop_path.drivable_area = util::generateDrivableArea( - stop_path, lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length, - planner_data_); + util::generateDrivableArea(stop_path, lanes, common_parameters.vehicle_length, planner_data_); return stop_path; } diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 6ed656acddf05..f6c980871f866 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -46,7 +46,10 @@ void SideShiftModule::initVariables() { reference_path_ = std::make_shared(); start_pose_reset_request_ = false; - lateral_offset_ = 0.0; + requested_lateral_offset_ = 0.0; + inserted_lateral_offset_ = 0.0; + inserted_shift_line_ = ShiftLine{}; + shift_status_ = SideShiftStatus{}; prev_output_ = ShiftedPath{}; prev_shift_line_ = ShiftLine{}; path_shifter_ = PathShifter{}; @@ -79,7 +82,7 @@ bool SideShiftModule::isExecutionRequested() const // If the desired offset has a non-zero value, return true as we want to execute the plan. - const bool has_request = !isAlmostZero(lateral_offset_); + const bool has_request = !isAlmostZero(requested_lateral_offset_); RCLCPP_DEBUG_STREAM( getLogger(), "ESS::isExecutionRequested() : " << std::boolalpha << has_request); @@ -114,7 +117,7 @@ BT::NodeStatus SideShiftModule::updateState() const auto last_sp = path_shifter_.getLastShiftLine(); if (last_sp) { const auto length = std::fabs(last_sp.get().end_shift_length); - const auto lateral_offset = std::fabs(lateral_offset_); + const auto lateral_offset = std::fabs(requested_lateral_offset_); const auto offset_diff = lateral_offset - length; if (!isAlmostZero(offset_diff)) { lateral_offset_change_request_ = true; @@ -125,7 +128,7 @@ BT::NodeStatus SideShiftModule::updateState() }(); const bool no_offset_diff = isOffsetDiffAlmostZero; - const bool no_request = isAlmostZero(lateral_offset_); + const bool no_request = isAlmostZero(requested_lateral_offset_); const auto no_shifted_plan = [&]() { if (prev_output_.shift_length.empty()) { @@ -145,6 +148,22 @@ BT::NodeStatus SideShiftModule::updateState() if (no_request && no_shifted_plan && no_offset_diff) { current_state_ = BT::NodeStatus::SUCCESS; } else { + const auto & current_lanes = util::getCurrentLanes(planner_data_); + const auto & current_pose = planner_data_->self_pose->pose; + const auto & inserted_shift_line_start_pose = inserted_shift_line_.start; + const auto & inserted_shift_line_end_pose = inserted_shift_line_.end; + const double self_to_shift_line_start_arc_length = + behavior_path_planner::util::getSignedDistance( + current_pose, inserted_shift_line_start_pose, current_lanes); + const double self_to_shift_line_end_arc_length = behavior_path_planner::util::getSignedDistance( + current_pose, inserted_shift_line_end_pose, current_lanes); + if (self_to_shift_line_start_arc_length >= 0) { + shift_status_ = SideShiftStatus::BEFORE_SHIFT; + } else if (self_to_shift_line_start_arc_length < 0 && self_to_shift_line_end_arc_length > 0) { + shift_status_ = SideShiftStatus::SHIFTING; + } else { + shift_status_ = SideShiftStatus::AFTER_SHIFT; + } current_state_ = BT::NodeStatus::RUNNING; } @@ -179,103 +198,42 @@ void SideShiftModule::updateData() path_shifter_.removeBehindShiftLineAndSetBaseOffset(nearest_idx); } -bool SideShiftModule::addShiftLine() +void SideShiftModule::replaceShiftLine() { auto shift_lines = path_shifter_.getShiftLines(); - - const auto calcLongitudinal_to_shift_start = [this](const auto & sp) { - return motion_utils::calcSignedArcLength( - reference_path_->points, getEgoPose().pose.position, sp.start.position); - }; - const auto calcLongitudinal_to_shift_end = [this](const auto & sp) { - return motion_utils::calcSignedArcLength( - reference_path_->points, getEgoPose().pose.position, sp.end.position); - }; - - // remove shift points on a far position. - const auto remove_far_iter = std::remove_if( - shift_lines.begin(), shift_lines.end(), - [this, calcLongitudinal_to_shift_start](const ShiftLine & sp) { - const auto dist_to_start = calcLongitudinal_to_shift_start(sp); - constexpr double max_remove_threshold_time = 1.0; // [s] - constexpr double max_remove_threshold_dist = 2.0; // [m] - const auto ego_current_speed = planner_data_->self_odometry->twist.twist.linear.x; - const auto remove_threshold = - std::max(ego_current_speed * max_remove_threshold_time, max_remove_threshold_dist); - return (dist_to_start > remove_threshold); - }); - - shift_lines.erase(remove_far_iter, shift_lines.end()); - - // check if the new_shift_lines overlap with existing shift points. - const auto new_sp = calcShiftLine(); - // check if the new_shift_lines is same with lately inserted shift_lines. - if (new_sp.end_shift_length == prev_shift_line_.end_shift_length) { - return false; + if (shift_lines.size() > 0) { + shift_lines.clear(); } - const auto new_sp_longitudinal_to_shift_start = calcLongitudinal_to_shift_start(new_sp); - const auto new_sp_longitudinal_to_shift_end = calcLongitudinal_to_shift_end(new_sp); - - const auto remove_overlap_iter = std::remove_if( - shift_lines.begin(), shift_lines.end(), - [this, calcLongitudinal_to_shift_start, calcLongitudinal_to_shift_end, - new_sp_longitudinal_to_shift_start, new_sp_longitudinal_to_shift_end](const ShiftLine & sp) { - const bool check_with_prev_sp = (sp.end_shift_length == prev_shift_line_.end_shift_length); - const auto old_sp_longitudinal_to_shift_start = calcLongitudinal_to_shift_start(sp); - const auto old_sp_longitudinal_to_shift_end = calcLongitudinal_to_shift_end(sp); - const bool sp_overlap_front = - ((new_sp_longitudinal_to_shift_start <= old_sp_longitudinal_to_shift_start) && - (old_sp_longitudinal_to_shift_start <= new_sp_longitudinal_to_shift_end)); - const bool sp_overlap_back = - ((new_sp_longitudinal_to_shift_start <= old_sp_longitudinal_to_shift_end) && - (old_sp_longitudinal_to_shift_end <= new_sp_longitudinal_to_shift_end)); - const bool sp_new_contain_old = - ((new_sp_longitudinal_to_shift_start <= old_sp_longitudinal_to_shift_start) && - (old_sp_longitudinal_to_shift_end <= new_sp_longitudinal_to_shift_end)); - const bool sp_old_contain_new = - ((old_sp_longitudinal_to_shift_start <= new_sp_longitudinal_to_shift_start) && - (new_sp_longitudinal_to_shift_end <= old_sp_longitudinal_to_shift_end)); - const bool overlap_with_new_sp = - (sp_overlap_front || sp_overlap_back || sp_new_contain_old || sp_old_contain_new); - - return (overlap_with_new_sp && !check_with_prev_sp); - }); - - shift_lines.erase(remove_overlap_iter, shift_lines.end()); - - // check if the new_shift_line has conflicts with existing shift points. - for (const auto & sp : shift_lines) { - if (calcLongitudinal_to_shift_start(sp) >= new_sp_longitudinal_to_shift_start) { - RCLCPP_WARN( - getLogger(), - "try to add shift point, but shift point already exists behind the proposed point. " - "Ignore the current proposal."); - return false; - } - } + const auto new_sl = calcShiftLine(); // if no conflict, then add the new point. - shift_lines.push_back(new_sp); - const bool new_sp_is_same_with_previous = - new_sp.end_shift_length == prev_shift_line_.end_shift_length; + shift_lines.push_back(new_sl); + const bool new_sl_is_same_with_previous = + new_sl.end_shift_length == prev_shift_line_.end_shift_length; - if (!new_sp_is_same_with_previous) { - prev_shift_line_ = new_sp; + if (!new_sl_is_same_with_previous) { + prev_shift_line_ = new_sl; } // set to path_shifter path_shifter_.setShiftLines(shift_lines); lateral_offset_change_request_ = false; + inserted_lateral_offset_ = requested_lateral_offset_; + inserted_shift_line_ = new_sl; - return true; + return; } BehaviorModuleOutput SideShiftModule::plan() { - // Update shift point - if (lateral_offset_change_request_) { - addShiftLine(); + // Replace shift line + if ( + lateral_offset_change_request_ && ((shift_status_ == SideShiftStatus::BEFORE_SHIFT) || + (shift_status_ == SideShiftStatus::AFTER_SHIFT))) { + replaceShiftLine(); + } else if (shift_status_ != SideShiftStatus::BEFORE_SHIFT) { + RCLCPP_DEBUG(getLogger(), "ego is shifting"); } else { RCLCPP_DEBUG(getLogger(), "change is not requested"); } @@ -340,11 +298,11 @@ void SideShiftModule::onLateralOffset(const LateralOffset::ConstSharedPtr latera const double new_lateral_offset = lateral_offset_msg->lateral_offset; RCLCPP_DEBUG( - getLogger(), "onLateralOffset start : lateral offset current = %f, new = &%f", lateral_offset_, - new_lateral_offset); + getLogger(), "onLateralOffset start : lateral offset current = %f, new = &%f", + requested_lateral_offset_, new_lateral_offset); // offset is not changed. - if (std::abs(lateral_offset_ - new_lateral_offset) < 1e-4) { + if (std::abs(inserted_lateral_offset_ - new_lateral_offset) < 1e-4) { return; } @@ -355,8 +313,7 @@ void SideShiftModule::onLateralOffset(const LateralOffset::ConstSharedPtr latera // new offset is requested. if (isReadyForNextRequest(parameters_.shift_request_time_limit)) { lateral_offset_change_request_ = true; - - lateral_offset_ = new_lateral_offset; + requested_lateral_offset_ = new_lateral_offset; } } @@ -369,7 +326,7 @@ ShiftLine SideShiftModule::calcShiftLine() const std::max(p.min_distance_to_start_shifting, ego_speed * p.time_to_start_shifting); const double dist_to_end = [&]() { - const double shift_length = lateral_offset_ - getClosestShiftLength(); + const double shift_length = requested_lateral_offset_ - getClosestShiftLength(); const double jerk_shifting_distance = path_shifter_.calcLongitudinalDistFromJerk( shift_length, p.shifting_lateral_jerk, std::max(ego_speed, p.min_shifting_speed)); const double shifting_distance = std::max(jerk_shifting_distance, p.min_shifting_distance); @@ -382,7 +339,7 @@ ShiftLine SideShiftModule::calcShiftLine() const const size_t nearest_idx = findEgoIndex(reference_path_->points); ShiftLine shift_line; - shift_line.end_shift_length = lateral_offset_; + shift_line.end_shift_length = requested_lateral_offset_; shift_line.start_idx = util::getIdxByArclength(*reference_path_, nearest_idx, dist_to_start); shift_line.start = reference_path_->points.at(shift_line.start_idx).point.pose; shift_line.end_idx = util::getIdxByArclength(*reference_path_, nearest_idx, dist_to_end); @@ -416,12 +373,12 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const -parameters_.drivable_area_right_bound_offset); const auto drivable_lanes = util::generateDrivableLanes(current_lanelets_); - const auto expanded_lanes = util::expandLanelets(drivable_lanes, left_offset, right_offset); + const auto shorten_lanes = util::cutOverlappedLanes(path->path, drivable_lanes); + const auto expanded_lanes = util::expandLanelets(shorten_lanes, left_offset, right_offset); { const auto & p = planner_data_->parameters; - path->path.drivable_area = util::generateDrivableArea( - path->path, expanded_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); + util::generateDrivableArea(path->path, expanded_lanes, p.vehicle_length, planner_data_); } } diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 89933bb020a3d..500eb5f90c774 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -21,6 +21,8 @@ #include "autoware_auto_perception_msgs/msg/predicted_object.hpp" #include "autoware_auto_perception_msgs/msg/predicted_path.hpp" +#include + #include #include #include @@ -57,17 +59,10 @@ double calcInterpolatedVelocity( const double interpolated_vel = std::abs(seg_dist) < 1e-06 ? next_vel : closest_vel; return interpolated_vel; } -} // namespace - -namespace drivable_area_utils -{ -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using geometry_msgs::msg::Pose; -using route_handler::RouteHandler; template size_t findNearestSegmentIndex( - const std::vector & points, const Pose & pose, const double dist_threshold, + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold) { const auto nearest_idx = @@ -79,277 +74,43 @@ size_t findNearestSegmentIndex( return motion_utils::findNearestSegmentIndex(points, pose.position); } -double quantize(const double val, const double resolution) -{ - return std::round(val / resolution) * resolution; -} - -void updateMinMaxPosition( - const Eigen::Vector2d & point, boost::optional & min_x, boost::optional & min_y, - boost::optional & max_x, boost::optional & max_y) -{ - min_x = min_x ? std::min(min_x.get(), point.x()) : point.x(); - min_y = min_y ? std::min(min_y.get(), point.y()) : point.y(); - max_x = max_x ? std::max(max_x.get(), point.x()) : point.x(); - max_y = max_y ? std::max(max_y.get(), point.y()) : point.y(); -} - -bool sumLengthFromTwoPoints( - const Eigen::Vector2d & base_point, const Eigen::Vector2d & target_point, - const double length_threshold, double & sum_length, boost::optional & min_x, - boost::optional & min_y, boost::optional & max_x, boost::optional & max_y) -{ - const double norm_length = (base_point - target_point).norm(); - sum_length += norm_length; - if (length_threshold < sum_length) { - const double diff_length = norm_length - (sum_length - length_threshold); - const Eigen::Vector2d interpolated_point = - base_point + diff_length * (target_point - base_point).normalized(); - updateMinMaxPosition(interpolated_point, min_x, min_y, max_x, max_y); - - const bool is_end = true; - return is_end; - } - - updateMinMaxPosition(target_point, min_x, min_y, max_x, max_y); - const bool is_end = false; - return is_end; -} - -void fillYawFromXY(std::vector & points) -{ - if (points.size() < 2) { - return; - } - - for (size_t i = 0; i < points.size(); ++i) { - const size_t prev_idx = (i == points.size() - 1) ? i - 1 : i; - const size_t next_idx = (i == points.size() - 1) ? i : i + 1; - - const double yaw = tier4_autoware_utils::calcAzimuthAngle( - points.at(prev_idx).position, points.at(next_idx).position); - points.at(i).orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - } -} - -lanelet::ConstLanelets extractLanesFromPathWithLaneId( - const std::shared_ptr & route_handler, const PathWithLaneId & path) -{ - // extract "unique" lane ids from path_with_lane_id - std::vector path_lane_ids; - for (const auto & path_point : path.points) { - for (const size_t lane_id : path_point.lane_ids) { - if (std::find(path_lane_ids.begin(), path_lane_ids.end(), lane_id) == path_lane_ids.end()) { - path_lane_ids.push_back(lane_id); - } +template +size_t findNearestSegmentIndexFromLateralDistance( + const std::vector & points, const geometry_msgs::msg::Pose & pose) +{ + size_t closest_idx = motion_utils::findNearestSegmentIndex(points, pose.position); + double min_lateral_dist = + std::fabs(motion_utils::calcLateralOffset(points, pose.position, closest_idx)); + + for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { + const double lon_dist = + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); + const double segment_length = + tier4_autoware_utils::calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + if (lon_dist < 0.0 || segment_length < lon_dist) { + continue; } - } - - // get lanes according to lane ids - lanelet::ConstLanelets path_lanes; - path_lanes.reserve(path_lane_ids.size()); - for (const auto path_lane_id : path_lane_ids) { - const auto & lane = route_handler->getLaneletsFromId(static_cast(path_lane_id)); - path_lanes.push_back(lane); - } - - return path_lanes; -} -size_t getNearestLaneId(const lanelet::ConstLanelets & path_lanes, const Pose & current_pose) -{ - lanelet::ConstLanelet closest_lanelet; - if (lanelet::utils::query::getClosestLanelet(path_lanes, current_pose, &closest_lanelet)) { - for (size_t i = 0; i < path_lanes.size(); ++i) { - if (path_lanes.at(i).id() == closest_lanelet.id()) { - return i; - } + const double lat_dist = + std::fabs(motion_utils::calcLateralOffset(points, pose.position, seg_idx)); + if (lat_dist < min_lateral_dist) { + closest_idx = seg_idx; + min_lateral_dist = lat_dist; } } - return 0; -} - -void updateMinMaxPositionFromForwardLanelet( - const lanelet::ConstLanelets & path_lanes, const std::vector & points, - const Pose & current_pose, const double & forward_lane_length, const double & lane_margin, - const size_t & nearest_lane_idx, const size_t & nearest_segment_idx, - const std::function & - get_bound_func, - boost::optional & min_x, boost::optional & min_y, boost::optional & max_x, - boost::optional & max_y) -{ - const auto forward_offset_length = motion_utils::calcSignedArcLength( - points, current_pose.position, nearest_segment_idx, nearest_segment_idx); - double sum_length = std::min(forward_offset_length, 0.0); - size_t current_lane_idx = nearest_lane_idx; - auto current_lane = path_lanes.at(current_lane_idx); - size_t current_point_idx = nearest_segment_idx; - while (true) { - const auto & bound = get_bound_func(current_lane); - if (current_point_idx != bound.size() - 1) { - const Eigen::Vector2d & current_point = bound[current_point_idx].basicPoint(); - const Eigen::Vector2d & next_point = bound[current_point_idx + 1].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - current_point, next_point, forward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - - ++current_point_idx; - } else { - const auto previous_lane = current_lane; - const size_t previous_point_idx = get_bound_func(previous_lane).size() - 1; - const auto & previous_bound = get_bound_func(previous_lane); - drivable_area_utils::updateMinMaxPosition( - previous_bound[previous_point_idx].basicPoint(), min_x, min_y, max_x, max_y); - if (current_lane_idx == path_lanes.size() - 1) { - break; - } - - current_lane_idx += 1; - current_lane = path_lanes.at(current_lane_idx); - current_point_idx = 0; - const auto & current_bound = get_bound_func(current_lane); - - const Eigen::Vector2d & prev_point = previous_bound[previous_point_idx].basicPoint(); - const Eigen::Vector2d & current_point = current_bound[current_point_idx].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - prev_point, current_point, forward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - } - } + return closest_idx; } -void updateMinMaxPositionFromBackwardLanelet( - const lanelet::ConstLanelets & path_lanes, const std::vector & points, - const Pose & current_pose, const double & backward_lane_length, const double & lane_margin, - const size_t & nearest_lane_idx, const size_t & nearest_segment_idx, - const std::function & - get_bound_func, - boost::optional & min_x, boost::optional & min_y, boost::optional & max_x, - boost::optional & max_y) -{ - size_t current_point_idx = nearest_segment_idx + 1; - const auto backward_offset_length = motion_utils::calcSignedArcLength( - points, nearest_segment_idx + 1, current_pose.position, nearest_segment_idx); - double sum_length = std::min(backward_offset_length, 0.0); - size_t current_lane_idx = nearest_lane_idx; - lanelet::ConstLanelet current_lane = path_lanes.at(current_lane_idx); - while (true) { - const auto & bound = get_bound_func(current_lane); - if (current_point_idx != 0) { - const Eigen::Vector2d & current_point = bound[current_point_idx].basicPoint(); - const Eigen::Vector2d & prev_point = bound[current_point_idx - 1].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - current_point, prev_point, backward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - - --current_point_idx; - } else { - const auto next_lane = current_lane; - const size_t next_point_idx = 0; - const auto & next_bound = get_bound_func(next_lane); - drivable_area_utils::updateMinMaxPosition( - next_bound[next_point_idx].basicPoint(), min_x, min_y, max_x, max_y); - - if (current_lane_idx == 0) { - break; - } - - current_lane_idx -= 1; - current_lane = path_lanes.at(current_lane_idx); - const auto & current_bound = get_bound_func(current_lane); - current_point_idx = current_bound.size() - 1; - - const Eigen::Vector2d & next_point = next_bound[next_point_idx].basicPoint(); - const Eigen::Vector2d & current_point = current_bound[current_point_idx].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - next_point, current_point, backward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - } - } -} -std::array getPathScope( - const PathWithLaneId & path, const std::shared_ptr & route_handler, - const Pose & current_pose, const double forward_lane_length, const double backward_lane_length, - const double lane_margin, const double max_dist, const double max_yaw) +bool checkHasSameLane( + const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelet & target_lane) { - const lanelet::ConstLanelets path_lanes = extractLanesFromPathWithLaneId(route_handler, path); - - const size_t nearest_lane_idx = getNearestLaneId(path_lanes, current_pose); - - // define functions to get right/left bounds as a vector - const std::vector> - get_bound_funcs{ - [](const lanelet::ConstLanelet & lane) -> lanelet::ConstLineString2d { - return lane.rightBound2d(); - }, - [](const lanelet::ConstLanelet & lane) -> lanelet::ConstLineString2d { - return lane.leftBound2d(); - }}; + if (lanelets.empty()) return false; - // calculate min/max x and y - boost::optional min_x; - boost::optional min_y; - boost::optional max_x; - boost::optional max_y; - - for (const auto & get_bound_func : get_bound_funcs) { - // search nearest point index to current pose - const auto & nearest_bound = get_bound_func(path_lanes.at(nearest_lane_idx)); - if (nearest_bound.empty()) { - continue; - } - - const std::vector points = std::invoke([&nearest_bound]() { - std::vector points; - points.reserve(nearest_bound.size()); - for (const auto & point : nearest_bound) { // calculate x and y - Pose p; - p.position.x = point.x(); - p.position.y = point.y(); - points.push_back(p); - } - - fillYawFromXY(points); // calculate yaw - return points; - }); - - const size_t nearest_segment_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, current_pose, max_dist, max_yaw); - - // forward lanelet - updateMinMaxPositionFromForwardLanelet( - path_lanes, points, current_pose, forward_lane_length, lane_margin, nearest_lane_idx, - nearest_segment_idx, get_bound_func, min_x, min_y, max_x, max_y); - - // backward lanelet - updateMinMaxPositionFromBackwardLanelet( - path_lanes, points, current_pose, backward_lane_length, lane_margin, nearest_lane_idx, - nearest_segment_idx, get_bound_func, min_x, min_y, max_x, max_y); - } - - if (!min_x || !min_y || !max_x || !max_y) { - const double x = current_pose.position.x; - const double y = current_pose.position.y; - return {x, y, x, y}; - } - - return {min_x.get(), min_y.get(), max_x.get(), max_y.get()}; + const auto has_same = [&](const auto & ll) { return ll.id() == target_lane.id(); }; + return std::find_if(lanelets.begin(), lanelets.end(), has_same) != lanelets.end(); } -} // namespace drivable_area_utils +} // namespace namespace behavior_path_planner::util { @@ -961,7 +722,8 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) filtered_path.points.push_back(pt); } } - filtered_path.drivable_area = input_path.drivable_area; + filtered_path.left_bound = input_path.left_bound; + filtered_path.right_bound = input_path.right_bound; return filtered_path; } @@ -1031,7 +793,7 @@ bool setGoal( // NOTE: goal does not have valid z, that will be calculated by interpolation here PathPointWithLaneId refined_goal{}; const size_t closest_seg_idx_for_goal = - drivable_area_utils::findNearestSegmentIndex(input.points, goal, 3.0, M_PI_4); + findNearestSegmentIndex(input.points, goal, 3.0, M_PI_4); refined_goal.point.pose = goal; refined_goal.point.pose.position.z = calcInterpolatedZ(input, goal.position, closest_seg_idx_for_goal); @@ -1043,8 +805,8 @@ bool setGoal( constexpr double goal_to_pre_goal_distance = -1.0; pre_refined_goal.point.pose = tier4_autoware_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); - const size_t closest_seg_idx_for_pre_goal = drivable_area_utils::findNearestSegmentIndex( - input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); + const size_t closest_seg_idx_for_pre_goal = + findNearestSegmentIndex(input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); pre_refined_goal.point.pose.position.z = calcInterpolatedZ(input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal); pre_refined_goal.point.longitudinal_velocity_mps = @@ -1084,7 +846,8 @@ bool setGoal( output_ptr->points.back().lane_ids = input.points.back().lane_ids; } - output_ptr->drivable_area = input.drivable_area; + output_ptr->left_bound = input.left_bound; + output_ptr->right_bound = input.right_bound; return true; } catch (std::out_of_range & ex) { RCLCPP_ERROR_STREAM( @@ -1096,7 +859,14 @@ bool setGoal( const Pose refineGoal(const Pose & goal, const lanelet::ConstLanelet & goal_lanelet) { + // return goal; const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal.position); + const double distance = boost::geometry::distance( + goal_lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(lanelet_point).basicPoint()); + if (distance < std::numeric_limits::epsilon()) { + return goal; + } + const auto segment = lanelet::utils::getClosestSegment( lanelet::utils::to2D(lanelet_point), goal_lanelet.centerline()); if (segment.empty()) { @@ -1258,148 +1028,261 @@ std::vector generateDrivableLanesWithShoulderLanes( return drivable_lanes; } -// input lanes must be in sequence -// NOTE: lanes in the path argument is used to calculate the size of the drivable area to cover -// designated forward and backward length by getPathScope function. -// lanes argument is used to determine (= draw) the drivable area. -// This is because lanes argument has multiple parallel lanes which makes hard to calculate -// the size of the drivable area -OccupancyGrid generateDrivableArea( - const PathWithLaneId & path, const std::vector & lanes, const double resolution, - const double vehicle_length, const std::shared_ptr planner_data) +boost::optional getOverlappedLaneletId(const std::vector & lanes) { - const auto transformed_lanes = util::transformToLanelets(lanes); - const auto & params = planner_data->parameters; - const auto route_handler = planner_data->route_handler; - const auto current_pose = planner_data->self_pose; + auto overlaps = [](const DrivableLanes & lanes, const DrivableLanes & target_lanes) { + const auto lanelets = transformToLanelets(lanes); + const auto target_lanelets = transformToLanelets(target_lanes); - // calculate min/max x and y from lanes in path argument (not from lanes argument) - const auto path_scope = drivable_area_utils::getPathScope( - path, route_handler, current_pose->pose, params.drivable_lane_forward_length, - params.drivable_lane_backward_length, params.drivable_lane_margin, - params.ego_nearest_dist_threshold, params.ego_nearest_yaw_threshold); + for (const auto & lanelet : lanelets) { + for (const auto & target_lanelet : target_lanelets) { + if (boost::geometry::intersects( + lanelet.polygon2d().basicPolygon(), target_lanelet.polygon2d().basicPolygon())) { + return true; + } + } + } - const double min_x = - drivable_area_utils::quantize(path_scope.at(0) - params.drivable_area_margin, resolution); - const double min_y = - drivable_area_utils::quantize(path_scope.at(1) - params.drivable_area_margin, resolution); - const double max_x = - drivable_area_utils::quantize(path_scope.at(2) + params.drivable_area_margin, resolution); - const double max_y = - drivable_area_utils::quantize(path_scope.at(3) + params.drivable_area_margin, resolution); + // No overlapping + return false; + }; - const double width = max_x - min_x; - const double height = max_y - min_y; + if (lanes.size() <= 2) { + return {}; + } - lanelet::ConstLanelets drivable_lanes; - { // add lanes which covers initial and final footprints - // 1. add preceding lanes before current pose - const auto lanes_before_current_pose = route_handler->getLanesBeforePose( - current_pose->pose, params.drivable_lane_backward_length + params.drivable_lane_margin); - drivable_lanes.insert( - drivable_lanes.end(), lanes_before_current_pose.begin(), lanes_before_current_pose.end()); + size_t overlapped_idx = lanes.size(); + for (size_t i = 0; i < lanes.size() - 2; ++i) { + for (size_t j = i + 2; j < lanes.size(); ++j) { + if (overlaps(lanes.at(i), lanes.at(j))) { + overlapped_idx = std::min(overlapped_idx, j); + } + } + } - // 2. add lanes - drivable_lanes.insert(drivable_lanes.end(), transformed_lanes.begin(), transformed_lanes.end()); + return overlapped_idx; +} - // 3. add succeeding lanes after goal - if (containsGoal(transformed_lanes, route_handler->getGoalLaneId())) { - const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); - drivable_lanes.insert(drivable_lanes.end(), lanes_after_goal.begin(), lanes_after_goal.end()); - } +std::vector cutOverlappedLanes( + PathWithLaneId & path, const std::vector & lanes) +{ + const auto overlapped_lanelet_id = getOverlappedLaneletId(lanes); + if (!overlapped_lanelet_id) { + return lanes; } - OccupancyGrid occupancy_grid; - PoseStamped grid_origin; + const std::vector shorten_lanes{ + lanes.begin(), lanes.begin() + *overlapped_lanelet_id}; + const std::vector removed_lanes{ + lanes.begin() + *overlapped_lanelet_id, lanes.end()}; - // calculate grid origin - { - grid_origin.header = current_pose->header; + const auto transformed_lanes = util::transformToLanelets(removed_lanes); - grid_origin.pose.position.x = min_x; - grid_origin.pose.position.y = min_y; - grid_origin.pose.position.z = current_pose->pose.position.z; - } + auto isIncluded = [&transformed_lanes](const std::vector & lane_ids) { + if (transformed_lanes.empty() || lane_ids.empty()) return false; - // header - { - occupancy_grid.header.stamp = current_pose->header.stamp; - occupancy_grid.header.frame_id = "map"; + for (const auto & transformed_lane : transformed_lanes) { + for (const auto & lane_id : lane_ids) { + if (lane_id == transformed_lane.id()) { + return true; + } + } + } + + return false; + }; + + for (size_t i = 0; i < path.points.size(); ++i) { + const auto & lane_ids = path.points.at(i).lane_ids; + if (isIncluded(lane_ids)) { + path.points.erase(path.points.begin() + i, path.points.end()); + break; + } } - // info - { - const int width_cell = std::round(width / resolution); - const int height_cell = std::round(height / resolution); + return shorten_lanes; +} + +geometry_msgs::msg::Pose calcLongitudinalOffsetStartPose( + const std::vector & points, const geometry_msgs::msg::Pose & pose, + const size_t nearest_segment_idx, const double offset) +{ + const double offset_length = + motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); + const auto offset_pose = + motion_utils::calcLongitudinalOffsetPose(points, nearest_segment_idx, offset_length + offset); + + return offset_pose ? offset_pose.get() : points.at(nearest_segment_idx); +} - occupancy_grid.info.map_load_time = occupancy_grid.header.stamp; - occupancy_grid.info.resolution = resolution; - occupancy_grid.info.width = width_cell; - occupancy_grid.info.height = height_cell; - occupancy_grid.info.origin = grid_origin.pose; +geometry_msgs::msg::Pose calcLongitudinalOffsetGoalPose( + const std::vector & points, const geometry_msgs::msg::Pose & pose, + const size_t nearest_segment_idx, const double offset) +{ + const double offset_length = + motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); + const auto offset_pose = + motion_utils::calcLongitudinalOffsetPose(points, nearest_segment_idx, offset_length + offset); + + return offset_pose ? offset_pose.get() : points.at(nearest_segment_idx + 1); +} + +void generateDrivableArea( + PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + const std::shared_ptr planner_data) +{ + std::vector left_bound; + std::vector right_bound; + + // extract data + const auto transformed_lanes = util::transformToLanelets(lanes); + const auto current_pose = planner_data->self_pose; + const auto route_handler = planner_data->route_handler; + constexpr double overlap_threshold = 0.01; + + auto addLeftBoundPoints = [&left_bound](const lanelet::ConstLanelet & lane) { + for (const auto & lp : lane.leftBound()) { + geometry_msgs::msg::Pose current_pose; + current_pose.position = lanelet::utils::conversion::toGeomMsgPt(lp); + if (left_bound.empty()) { + left_bound.push_back(current_pose); + } else if ( + tier4_autoware_utils::calcDistance2d(current_pose, left_bound.back()) > overlap_threshold) { + left_bound.push_back(current_pose); + } + } + }; + + auto addRightBoundPoints = [&right_bound](const lanelet::ConstLanelet & lane) { + for (const auto & rp : lane.rightBound()) { + geometry_msgs::msg::Pose current_pose; + current_pose.position = lanelet::utils::conversion::toGeomMsgPt(rp); + if (right_bound.empty()) { + right_bound.push_back(current_pose); + } else if ( + tier4_autoware_utils::calcDistance2d(current_pose, right_bound.back()) > + overlap_threshold) { + right_bound.push_back(current_pose); + } + } + }; + + // Insert Position + for (const auto & lane : lanes) { + addLeftBoundPoints(lane.left_lane); + addRightBoundPoints(lane.right_lane); } - // occupancy_grid.data = image; - { - constexpr uint8_t free_space = 0; - constexpr uint8_t occupied_space = 100; - // get transform - tf2::Stamped tf_grid2map, tf_map2grid; - tf2::fromMsg(grid_origin, tf_grid2map); - tf_map2grid.setData(tf_grid2map.inverse()); - const auto geom_tf_map2grid = tf2::toMsg(tf_map2grid); - - // convert lane polygons into cv type - cv::Mat cv_image( - occupancy_grid.info.width, occupancy_grid.info.height, CV_8UC1, cv::Scalar(occupied_space)); - for (const auto & lane : drivable_lanes) { - lanelet::BasicPolygon2d lane_poly = lane.polygon2d().basicPolygon(); - - if (lane.hasAttribute("intersection_area")) { - const std::string area_id = lane.attributeOr("intersection_area", "none"); - const auto intersection_area = - route_handler->getIntersectionAreaById(atoi(area_id.c_str())); - const auto poly = lanelet::utils::to2D(intersection_area).basicPolygon(); - std::vector lane_polys{}; - if (boost::geometry::intersection(poly, lane_poly, lane_polys)) { - lane_poly = lane_polys.front(); + const auto has_overlap = + [&](const lanelet::ConstLanelet & lane, const lanelet::Id & ignore_lane_id = lanelet::InvalId) { + for (const auto & transformed_lane : transformed_lanes) { + if (transformed_lane.id() == ignore_lane_id) { + continue; + } + if (boost::geometry::intersects( + lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { + return true; } } + return false; + }; - // create drivable area using opencv - std::vector> cv_polygons; - std::vector cv_polygon; - cv_polygon.reserve(lane_poly.size()); - for (const auto & p : lane_poly) { - const double z = lane.polygon3d().basicPolygon().at(0).z(); - Point geom_pt = tier4_autoware_utils::createPoint(p.x(), p.y(), z); - Point transformed_geom_pt; - tf2::doTransform(geom_pt, transformed_geom_pt, geom_tf_map2grid); - cv_polygon.push_back(toCVPoint(transformed_geom_pt, width, height, resolution)); + // Insert points after goal + lanelet::ConstLanelet goal_lanelet; + if ( + route_handler->getGoalLanelet(&goal_lanelet) && + checkHasSameLane(transformed_lanes, goal_lanelet)) { + const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); + const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); + for (const auto & lane : lanes_after_goal) { + // If lane is already in the transformed lanes, ignore it + if (checkHasSameLane(transformed_lanes, lane)) { + continue; } - if (!cv_polygon.empty()) { - cv_polygons.push_back(cv_polygon); - // fill in drivable area and copy to occupancy grid - cv::fillPoly(cv_image, cv_polygons, cv::Scalar(free_space)); + // Check if overlapped + const bool is_overlapped = + (checkHasSameLane(next_lanes_after_goal, lane) + ? has_overlap(lane, route_handler->getGoalLaneId()) + : has_overlap(lane)); + if (is_overlapped) { + continue; } - } - - // Closing - // NOTE: Because of the discretization error, there may be some discontinuity between two - // successive lanelets in the drivable area. This issue is dealt with by the erode/dilate - // process. - constexpr int num_iter = 1; - cv::Mat cv_erode, cv_dilate; - cv::erode(cv_image, cv_erode, cv::Mat(), cv::Point(-1, -1), num_iter); - cv::dilate(cv_erode, cv_dilate, cv::Mat(), cv::Point(-1, -1), num_iter); - // const auto & cv_image_reshaped = cv_dilate.reshape(1, 1); - imageToOccupancyGrid(cv_dilate, &occupancy_grid); - occupancy_grid.data[0] = 0; - // cv_image_reshaped.copyTo(occupancy_grid.data); + addLeftBoundPoints(lane); + addRightBoundPoints(lane); + } + } + + // Give Orientation + motion_utils::insertOrientation(left_bound, true); + motion_utils::insertOrientation(right_bound, true); + + // Get Closest segment for the start point + constexpr double front_length = 0.5; + const auto front_pose = path.points.empty() ? current_pose->pose : path.points.front().point.pose; + const size_t front_left_start_idx = + findNearestSegmentIndexFromLateralDistance(left_bound, front_pose); + const size_t front_right_start_idx = + findNearestSegmentIndexFromLateralDistance(right_bound, front_pose); + const auto left_start_pose = + calcLongitudinalOffsetStartPose(left_bound, front_pose, front_left_start_idx, -front_length); + const auto right_start_pose = + calcLongitudinalOffsetStartPose(right_bound, front_pose, front_right_start_idx, -front_length); + const size_t left_start_idx = + findNearestSegmentIndexFromLateralDistance(left_bound, left_start_pose); + const size_t right_start_idx = + findNearestSegmentIndexFromLateralDistance(right_bound, right_start_pose); + + // Get Closest segment for the goal point + const auto goal_pose = path.points.empty() ? current_pose->pose : path.points.back().point.pose; + const size_t goal_left_start_idx = + findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose); + const size_t goal_right_start_idx = + findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose); + const auto left_goal_pose = + calcLongitudinalOffsetGoalPose(left_bound, goal_pose, goal_left_start_idx, vehicle_length); + const auto right_goal_pose = + calcLongitudinalOffsetGoalPose(right_bound, goal_pose, goal_right_start_idx, vehicle_length); + const size_t left_goal_idx = + findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_pose); + const size_t right_goal_idx = + findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_pose); + + // Store Data + path.left_bound.clear(); + path.right_bound.clear(); + + // Insert a start point + path.left_bound.push_back(left_start_pose.position); + path.right_bound.push_back(right_start_pose.position); + + // Insert middle points + for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { + const auto & next_point = left_bound.at(i).position; + const double dist = tier4_autoware_utils::calcDistance2d(path.left_bound.back(), next_point); + if (dist > overlap_threshold) { + path.left_bound.push_back(next_point); + } + } + for (size_t i = right_start_idx + 1; i <= right_goal_idx; ++i) { + const auto & next_point = right_bound.at(i).position; + const double dist = tier4_autoware_utils::calcDistance2d(path.right_bound.back(), next_point); + if (dist > overlap_threshold) { + path.right_bound.push_back(next_point); + } + } + + // Insert a goal point + if ( + tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_pose.position) > + overlap_threshold) { + path.left_bound.push_back(left_goal_pose.position); + } + if ( + tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_pose.position) > + overlap_threshold) { + path.right_bound.push_back(right_goal_pose.position); } - - return occupancy_grid; } double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets) @@ -1558,7 +1441,8 @@ Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id) { Path path; path.header = path_with_lane_id.header; - path.drivable_area = path_with_lane_id.drivable_area; + path.left_bound = path_with_lane_id.left_bound; + path.right_bound = path_with_lane_id.right_bound; path.points.reserve(path_with_lane_id.points.size()); for (const auto & pt_with_lane_id : path_with_lane_id.points) { path.points.push_back(pt_with_lane_id.point); @@ -1754,42 +1638,6 @@ std::vector getTargetLaneletPolygons( return polygons; } -void occupancyGridToImage(const OccupancyGrid & occupancy_grid, cv::Mat * cv_image) -{ - const int width = cv_image->cols; - const int height = cv_image->rows; - for (int x = width - 1; x >= 0; x--) { - for (int y = height - 1; y >= 0; y--) { - const int idx = (height - 1 - y) + (width - 1 - x) * height; - const unsigned char intensity = occupancy_grid.data.at(idx); - cv_image->at(y, x) = intensity; - } - } -} - -void imageToOccupancyGrid(const cv::Mat & cv_image, OccupancyGrid * occupancy_grid) -{ - const int width = cv_image.cols; - const int height = cv_image.rows; - occupancy_grid->data.clear(); - occupancy_grid->data.resize(width * height); - for (int x = width - 1; x >= 0; x--) { - for (int y = height - 1; y >= 0; y--) { - const int idx = (height - 1 - y) + (width - 1 - x) * height; - const unsigned char intensity = cv_image.at(y, x); - occupancy_grid->data.at(idx) = intensity; - } - } -} - -cv::Point toCVPoint( - const Point & geom_point, const double width_m, const double height_m, const double resolution) -{ - return { - static_cast((height_m - geom_point.y) / resolution), - static_cast((width_m - geom_point.x) / resolution)}; -} - // TODO(Horibe) There is a similar function in route_handler. std::shared_ptr generateCenterLinePath( const std::shared_ptr & planner_data) @@ -1824,8 +1672,7 @@ std::shared_ptr generateCenterLinePath( centerline_path->header = route_handler->getRouteHeader(); - centerline_path->drivable_area = util::generateDrivableArea( - *centerline_path, drivable_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data); + util::generateDrivableArea(*centerline_path, drivable_lanes, p.vehicle_length, planner_data); return centerline_path; } @@ -1938,13 +1785,11 @@ PathWithLaneId getCenterLinePath( const double s_backward = std::max(0., s - backward_path_length); double s_forward = s + forward_path_length; - const double buffer = - parameter.backward_length_buffer_for_end_of_lane; // buffer for min_lane_change_length const int num_lane_change = std::abs(route_handler.getNumLaneToPreferredLane(lanelet_sequence.back())); const double lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); const double lane_change_buffer = - std::fabs(num_lane_change * (parameter.minimum_lane_change_length + buffer) + optional_length); + calcLaneChangeBuffer(parameter, std::abs(num_lane_change), optional_length); if (route_handler.isDeadEndLanelet(lanelet_sequence.back())) { s_forward = std::min(s_forward, lane_length - lane_change_buffer); @@ -2084,7 +1929,7 @@ PathWithLaneId setDecelerationVelocity( const double lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); const auto arclength = lanelet::utils::getArcCoordinates(lanelet_sequence, point.point.pose); const double distance_to_end = - std::max(0.0, lane_length - std::fabs(lane_change_buffer) - arclength.length); + std::max(0.0, lane_length - std::abs(lane_change_buffer) - arclength.length); point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, static_cast(distance_to_end / lane_change_prepare_duration)); @@ -2692,4 +2537,19 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre return true; } +double calcTotalLaneChangeDistanceWithBuffer(const BehaviorPathPlannerParameters & common_param) +{ + const double minimum_lane_change_distance = + common_param.minimum_lane_change_prepare_distance + common_param.minimum_lane_change_length; + const double end_of_lane_buffer = common_param.backward_length_buffer_for_end_of_lane; + return minimum_lane_change_distance + end_of_lane_buffer; +} + +double calcLaneChangeBuffer( + const BehaviorPathPlannerParameters & common_param, const int num_lane_change, + const double length_to_intersection) +{ + return num_lane_change * calcTotalLaneChangeDistanceWithBuffer(common_param) + + length_to_intersection; +} } // namespace behavior_path_planner::util diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp index 0453d03cd0b4e..e3cacf3a362ba 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp @@ -48,13 +48,11 @@ class BlindSpotModule : public SceneModuleInterface struct DebugData { - autoware_auto_planning_msgs::msg::PathWithLaneId path_raw; geometry_msgs::msg::Pose virtual_wall_pose; geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; geometry_msgs::msg::Polygon conflict_area_for_blind_spot; geometry_msgs::msg::Polygon detection_area_for_blind_spot; - autoware_auto_planning_msgs::msg::PathWithLaneId spline_path; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; }; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index ba095a88b1fad..8decae844adb7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -46,7 +46,6 @@ class IntersectionModule : public SceneModuleInterface struct DebugData { bool stop_required; - autoware_auto_planning_msgs::msg::PathWithLaneId path_raw; geometry_msgs::msg::Pose slow_wall_pose; geometry_msgs::msg::Pose stop_wall_pose; diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 371ec93aa8253..bfe03cced0a77 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -190,8 +190,6 @@ struct DebugData std::vector parked_vehicle_point; std::vector possible_collisions; std::vector occlusion_points; - PathWithLaneId path_raw; - PathWithLaneId path_interpolated; void resetData() { close_partition.clear(); diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp index 14e8271a19b5b..77fafdf7ddeea 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -16,12 +16,15 @@ #define SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ #include "behavior_velocity_planner/planner_data.hpp" +#include "velocity_factor_interface.hpp" #include #include #include #include +#include +#include #include #include #include @@ -100,6 +103,9 @@ class SceneModuleInterface bool isSafe() const { return safe_; } double getDistance() const { return distance_; } + void resetVelocityFactor() { velocity_factor_.reset(); } + VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } + protected: const int64_t module_id_; bool activated_; @@ -110,6 +116,7 @@ class SceneModuleInterface std::shared_ptr planner_data_; boost::optional infrastructure_command_; boost::optional first_stop_path_point_index_; + VelocityFactorInterface velocity_factor_; void setSafe(const bool safe) { safe_ = safe; } void setDistance(const double distance) { distance_ = distance; } @@ -150,6 +157,8 @@ class SceneModuleManagerInterface } pub_virtual_wall_ = node.create_publisher( std::string("~/virtual_wall/") + module_name, 5); + pub_velocity_factor_ = node.create_publisher( + std::string("/planning/velocity_factors/") + module_name, 1); pub_stop_reason_ = node.create_publisher("~/output/stop_reasons", 1); pub_infrastructure_commands_ = @@ -188,8 +197,11 @@ class SceneModuleManagerInterface visualization_msgs::msg::MarkerArray debug_marker_array; visualization_msgs::msg::MarkerArray virtual_wall_marker_array; tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; stop_reason_array.header.frame_id = "map"; stop_reason_array.header.stamp = clock_->now(); + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = clock_->now(); tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; infrastructure_command_array.stamp = clock_->now(); @@ -197,9 +209,15 @@ class SceneModuleManagerInterface first_stop_path_point_index_ = static_cast(path->points.size()) - 1; for (const auto & scene_module : scene_modules_) { tier4_planning_msgs::msg::StopReason stop_reason; + scene_module->resetVelocityFactor(); scene_module->setPlannerData(planner_data_); scene_module->modifyPathVelocity(path, &stop_reason); + // The velocity factor must be called after modifyPathVelocity. + const auto velocity_factor = scene_module->getVelocityFactor(); + if (velocity_factor.type != VelocityFactor::UNKNOWN) { + velocity_factor_array.factors.emplace_back(velocity_factor); + } if (stop_reason.reason != "") { stop_reason_array.stop_reasons.emplace_back(stop_reason); } @@ -224,6 +242,7 @@ class SceneModuleManagerInterface if (!stop_reason_array.stop_reasons.empty()) { pub_stop_reason_->publish(stop_reason_array); } + pub_velocity_factor_->publish(velocity_factor_array); pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { @@ -311,6 +330,8 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_debug_; rclcpp::Publisher::SharedPtr pub_debug_path_; rclcpp::Publisher::SharedPtr pub_stop_reason_; + rclcpp::Publisher::SharedPtr + pub_velocity_factor_; rclcpp::Publisher::SharedPtr pub_infrastructure_commands_; diff --git a/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp new file mode 100644 index 0000000000000..cff6e54b4f134 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp @@ -0,0 +1,68 @@ + +// 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 SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ +#define SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ + +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; +using geometry_msgs::msg::Pose; +using VelocityFactorType = VelocityFactor::_type_type; +using VelocityFactorStatus = VelocityFactor::_status_type; + +class VelocityFactorInterface +{ +public: + VelocityFactorInterface() { type_ = VelocityFactor::UNKNOWN; } + + VelocityFactor get() const { return velocity_factor_; } + void init(const VelocityFactorType type) { type_ = type; } + void reset() { velocity_factor_.type = VelocityFactor::UNKNOWN; } + + template + void set( + const T & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string detail = "") + { + const auto & curr_point = curr_pose.position; + const auto & stop_point = stop_pose.position; + velocity_factor_.type = type_; + velocity_factor_.pose = stop_pose; + velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point); + velocity_factor_.status = status; + velocity_factor_.detail = detail; + } + +private: + VelocityFactorType type_; + VelocityFactor velocity_factor_; +}; + +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp index 1d49c74afbd83..130e4a1715fde 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp @@ -99,6 +99,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface void setStopReason(const Pose & stop_pose, StopReason * stop_reason); + void setVelocityFactor( + const geometry_msgs::msg::Pose & stop_pose, + autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor); + boost::optional getPathIndexOfFirstEndLine(); bool isBeforeStartLine(const size_t end_line_idx); diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index c648e25a055d2..3995866f760c6 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -47,6 +47,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 64a79e87bd762..5fea6f0a13e82 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -454,7 +454,8 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath output_path_msg = to_path(*input_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); - output_path_msg.drivable_area = input_path_msg->drivable_area; + output_path_msg.left_bound = input_path_msg->left_bound; + output_path_msg.right_bound = input_path_msg->right_bound; return output_path_msg; } @@ -475,7 +476,8 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath output_path_msg.header.stamp = this->now(); // TODO(someone): This must be updated in each scene module, but copy from input message for now. - output_path_msg.drivable_area = input_path_msg->drivable_area; + output_path_msg.left_bound = input_path_msg->left_bound; + output_path_msg.right_bound = input_path_msg->right_bound; return output_path_msg; } diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp index 7163a49aae6ea..fbd525294a50e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp @@ -92,11 +92,6 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() const auto state = state_machine_.getState(); const auto now = this->clock_->now(); - appendMarkerArray( - debug::createPathMarkerArray( - debug_data_.path_raw, "path_raw", lane_id_, now, 0.6, 0.3, 0.3, 0.0, 1.0, 1.0), - &debug_marker_array, now); - appendMarkerArray( createPoseMarkerArray( debug_data_.stop_point_pose, state, "stop_point_pose", module_id_, 1.0, 0.0, 0.0), @@ -124,11 +119,6 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), &debug_marker_array, now); - appendMarkerArray( - debug::createPathMarkerArray( - debug_data_.spline_path, "spline", lane_id_, now, 0.3, 0.1, 0.1, 0.5, 0.5, 0.5), - &debug_marker_array, now); - return debug_marker_array; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 4647cb9d3f883..1b64566611cfe 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -63,6 +63,7 @@ BlindSpotModule::BlindSpotModule( turn_direction_(TurnDirection::INVALID), is_over_pass_judge_line_(false) { + velocity_factor_.init(VelocityFactor::REAR_CHECK); planner_param_ = planner_param; const auto & assigned_lanelet = @@ -83,7 +84,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT); const auto input_path = *path; - debug_data_.path_raw = input_path; StateMachine::State current_state = state_machine_.getState(); RCLCPP_DEBUG( @@ -184,6 +184,8 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto stop_factor.stop_pose = debug_data_.stop_point_pose; stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); } else { *path = input_path; // reset path } @@ -235,7 +237,6 @@ bool BlindSpotModule::generateStopLine( if (!splineInterpolate(*path, interval, path_ip, logger_)) { return false; } - debug_data_.spline_path = path_ip; /* generate stop point */ int stop_idx_ip = 0; // stop point index for interpolated path. diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index eac948942f76c..9b40751449dd6 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -137,6 +137,7 @@ CrosswalkModule::CrosswalkModule( crosswalk_(std::move(crosswalk)), planner_param_(planner_param) { + velocity_factor_.init(VelocityFactor::CROSSWALK); passed_safety_slow_point_ = false; } @@ -218,9 +219,15 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto if (nearest_stop_point) { insertDecelPointWithDebugInfo(nearest_stop_point.get(), 0.0, *path); planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_factor.stop_pose, + VelocityFactor::UNKNOWN); } else if (rtc_stop_point) { insertDecelPointWithDebugInfo(rtc_stop_point.get(), 0.0, *path); planning_utils::appendStopReason(stop_factor_rtc, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_factor_rtc.stop_pose, + VelocityFactor::UNKNOWN); } RCLCPP_INFO_EXPRESSION( diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp index 30efdc27c4197..c28c1ff461800 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp @@ -39,6 +39,7 @@ WalkwayModule::WalkwayModule( state_(State::APPROACH), planner_param_(planner_param) { + velocity_factor_.init(VelocityFactor::SIDEWALK); } boost::optional> WalkwayModule::getStopLine( @@ -121,6 +122,8 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ stop_factor.stop_pose = stop_pose.get(); stop_factor.stop_factor_points.push_back(path_intersects_.front()); planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose.get(), VelocityFactor::UNKNOWN); // use arc length to identify if ego vehicle is in front of walkway stop or not. const double signed_arc_dist_to_stop_point = @@ -149,7 +152,6 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ if (planner_data_->isVehicleStopped()) { state_ = State::SURPASSED; } - } else if (state_ == State::SURPASSED) { } return true; diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index d89bb16b8b3ce..8f9bc0a6e92a3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -46,6 +46,7 @@ DetectionAreaModule::DetectionAreaModule( state_(State::GO), planner_param_(planner_param) { + velocity_factor_.init(VelocityFactor::USER_DEFINED_DETECTION_AREA); } LineString2d DetectionAreaModule::getStopLineGeometry2d() const @@ -182,6 +183,8 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_factor.stop_pose = stop_point->second; stop_factor.stop_factor_points = obstacle_points; planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_point->second, VelocityFactor::UNKNOWN); } // Create legacy StopReason diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index c7bbb10395af8..8697be34d0222 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -112,11 +112,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( const auto state = state_machine_.getState(); const auto now = this->clock_->now(); - appendMarkerArray( - debug::createPathMarkerArray( - debug_data_.path_raw, "path_raw", lane_id_, now, 0.6, 0.3, 0.3, 0.0, 1.0, 1.0), - &debug_marker_array, now); - appendMarkerArray( createLaneletPolygonsMarkerArray( debug_data_.detection_area, "detection_area", lane_id_, 0.0, 1.0, 0.0), diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 98bf2ec3fab82..d3c20427f118e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -57,6 +57,7 @@ IntersectionModule::IntersectionModule( const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), is_go_out_(false) { + velocity_factor_.init(VelocityFactor::INTERSECTION); planner_param_ = planner_param; const auto & assigned_lanelet = @@ -77,7 +78,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const StateMachine::State current_state = state_machine_.getState(); debug_data_ = DebugData(); - debug_data_.path_raw = *path; *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); @@ -128,7 +128,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * logger_.get_child("util"), clock_); if (!stuck_line_idx_opt.has_value()) { // returns here if path is not intersecting with conflicting areas - RCLCPP_WARN_SKIPFIRST_THROTTLE( + RCLCPP_DEBUG_SKIPFIRST_THROTTLE( logger_, *clock_, 1000 /* ms */, "setStopLineIdx for stuck line fail"); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); @@ -257,14 +257,21 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_.stop_point_pose = path->points.at(stop_line_idx_final).point.pose; debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx_final).point.pose; - /* get stop point and stop factor */ - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = debug_data_.stop_point_pose; - const auto stop_factor_conflict = planning_utils::toRosPoints(debug_data_.conflicting_targets); - const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets); - stop_factor.stop_factor_points = - planning_utils::concatVector(stop_factor_conflict, stop_factor_stuck); - planning_utils::appendStopReason(stop_factor, stop_reason); + // Get stop point and stop factor + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = debug_data_.stop_point_pose; + const auto stop_factor_conflict = + planning_utils::toRosPoints(debug_data_.conflicting_targets); + const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets); + stop_factor.stop_factor_points = + planning_utils::concatVector(stop_factor_conflict, stop_factor_stuck); + planning_utils::appendStopReason(stop_factor, stop_reason); + + const auto & stop_pose = path->points.at(stop_line_idx_final).point.pose; + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); + } RCLCPP_DEBUG(logger_, "not activated. stop at the line."); RCLCPP_DEBUG(logger_, "===== plan end ====="); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index c76f6c4844a99..d880a6c76dc1f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -39,6 +39,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id) { + velocity_factor_.init(VelocityFactor::MERGE); planner_param_ = planner_param; state_machine_.setState(StateMachine::State::STOP); } @@ -111,6 +112,9 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR stop_factor.stop_pose = debug_data_.stop_point_pose; stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point); planning_utils::appendStopReason(stop_factor, stop_reason); + const auto & stop_pose = path->points.at(stop_line_idx).point.pose; + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position); diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index 542652cdf604b..1789b6dc6ff3c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -49,6 +49,7 @@ NoStoppingAreaModule::NoStoppingAreaModule( no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param) { + velocity_factor_.init(VelocityFactor::NO_STOPPING_AREA); state_machine_.setState(StateMachine::State::GO); state_machine_.setMarginTime(planner_param_.state_clear_time); } @@ -193,6 +194,9 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason stop_factor.stop_pose = stop_point->second; stop_factor.stop_factor_points = debug_data_.stuck_points; planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_point->second, + VelocityFactor::UNKNOWN); } // Create legacy StopReason diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index a0d70279679b4..76e71b67a2d03 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -204,18 +204,6 @@ MarkerArray OcclusionSpotModule::createDebugMarkerArray() makePolygonMarker(debug_data_.close_partition, "close_partition", module_id_, debug_data_.z), &debug_marker_array, now); } - if (!debug_data_.path_interpolated.points.empty()) { - const int64_t virtual_lane_id = 0; - appendMarkerArray( - debug::createPathMarkerArray( - debug_data_.path_raw, "path_raw", virtual_lane_id, now, 0.6, 0.3, 0.3, 0.0, 1.0, 1.0), - &debug_marker_array, now); - appendMarkerArray( - debug::createPathMarkerArray( - debug_data_.path_interpolated, "path_interpolated", virtual_lane_id, now, 0.6, 0.3, 0.3, - 0.0, 1.0, 1.0), - &debug_marker_array, now); - } if (!debug_data_.occlusion_points.empty()) { appendMarkerArray( debug::createPointsMarkerArray( diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 510ea7fe88010..9bafd9e707741 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -63,6 +63,8 @@ OcclusionSpotModule::OcclusionSpotModule( const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), param_(planner_param) { + velocity_factor_.init(VelocityFactor::UNKNOWN); + if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; //! occupancy grid limitation( 100 * 100 ) @@ -185,10 +187,6 @@ bool OcclusionSpotModule::modifyPathVelocity( // these debug topics needs computation resource debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; - if (param_.is_show_occlusion) { - debug_data_.path_interpolated = path_interpolated; - debug_data_.path_raw.points = clipped_path.points; - } DEBUG_PRINT(show_time, "total [ms]: ", stop_watch_.toc("total_processing_time", true)); return true; } diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index 5bb27d1c0b24c..50334e356556b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -33,6 +33,8 @@ RunOutModule::RunOutModule( debug_ptr_(debug_ptr), state_machine_(std::make_unique(planner_param.state_param)) { + velocity_factor_.init(VelocityFactor::UNKNOWN); + if (planner_param.run_out.use_partition_lanelet) { const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); planning_utils::getAllPartitionLanelets(ll, partition_lanelets_); diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp index 0f6921691025f..9c00a98394154 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp @@ -399,7 +399,8 @@ PathWithLaneId trimPathFromSelfPose( PathWithLaneId output{}; output.header = input.header; - output.drivable_area = input.drivable_area; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; double dist_sum = 0; for (size_t i = nearest_idx; i < input.points.size(); ++i) { output.points.push_back(input.points.at(i)); diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp index 10c42ccd894a4..0210c716ac514 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp @@ -33,6 +33,7 @@ StopLineModule::StopLineModule( stop_line_(stop_line), state_(State::APPROACH) { + velocity_factor_.init(VelocityFactor::STOP_SIGN); planner_param_ = planner_param; } @@ -89,6 +90,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop stop_factor.stop_pose = stop_pose; stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::APPROACHING); } // Move to stopped state if stopped @@ -133,6 +136,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop stop_factor.stop_pose = ego_pos_on_path.pose; stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::STOPPED); } const auto elapsed_time = (clock_->now() - *stopped_time_).seconds(); diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp index 7ac0d3797e545..523750c833889 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp @@ -50,6 +50,10 @@ void TrafficLightModuleManager::modifyPathVelocity( tl_state.header.stamp = path->header.stamp; tl_state.is_module_running = false; + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = clock_->now(); + tier4_planning_msgs::msg::StopReasonArray stop_reason_array; stop_reason_array.header.frame_id = "map"; stop_reason_array.header.stamp = path->header.stamp; @@ -60,9 +64,15 @@ void TrafficLightModuleManager::modifyPathVelocity( tier4_planning_msgs::msg::StopReason stop_reason; std::shared_ptr traffic_light_scene_module( std::dynamic_pointer_cast(scene_module)); + traffic_light_scene_module->resetVelocityFactor(); traffic_light_scene_module->setPlannerData(planner_data_); traffic_light_scene_module->modifyPathVelocity(path, &stop_reason); + // The velocity factor must be called after modifyPathVelocity. + const auto velocity_factor = traffic_light_scene_module->getVelocityFactor(); + if (velocity_factor.type != VelocityFactor::UNKNOWN) { + velocity_factor_array.factors.emplace_back(velocity_factor); + } if (stop_reason.reason != "") { stop_reason_array.stop_reasons.emplace_back(stop_reason); } @@ -91,6 +101,7 @@ void TrafficLightModuleManager::modifyPathVelocity( if (!stop_reason_array.stop_reasons.empty()) { pub_stop_reason_->publish(stop_reason_array); } + pub_velocity_factor_->publish(velocity_factor_array); pub_debug_->publish(debug_marker_array); pub_virtual_wall_->publish(virtual_wall_marker_array); pub_tl_state_->publish(tl_state); diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index 8620ed5fe7d34..4f09f89a458d3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -199,6 +199,7 @@ TrafficLightModule::TrafficLightModule( input_(Input::PERCEPTION), is_prev_state_stop_(false) { + velocity_factor_.init(VelocityFactor::TRAFFIC_SIGNAL); planner_param_ = planner_param; } @@ -487,6 +488,9 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP stop_factor.stop_factor_points = std::vector{ debug_data_.highest_confidence_traffic_light_point.value()}; } + velocity_factor_.set( + modified_path.points, planner_data_->current_pose.pose, target_point_with_lane_id.point.pose, + VelocityFactor::UNKNOWN); planning_utils::appendStopReason(stop_factor, stop_reason); return modified_path; diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp index d365de58adcea..263daa9c169bc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -194,6 +194,8 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( lane_(lane), planner_param_(planner_param) { + velocity_factor_.init(VelocityFactor::V2I_GATE_CONTROL_ENTER); + // Get map data const auto instrument = reg_elem_.getVirtualTrafficLight(); const auto instrument_bottom_line = toAutowarePoints(instrument); @@ -249,6 +251,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe // Initialize setInfrastructureCommand({}); *stop_reason = planning_utils::initializeStopReason(StopReason::VIRTUAL_TRAFFIC_LIGHT); + module_data_ = {}; // Copy data @@ -575,6 +578,9 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( // Set StopReason setStopReason(stop_pose, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN, + command_.type); // Set data for visualization module_data_.stop_head_pose_at_stop_line = @@ -607,6 +613,8 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( // Set StopReason setStopReason(stop_pose, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); // Set data for visualization module_data_.stop_head_pose_at_end_line = diff --git a/planning/freespace_planning_algorithms/src/rrtstar.cpp b/planning/freespace_planning_algorithms/src/rrtstar.cpp index 967446a7a10a3..c800fd45a0539 100644 --- a/planning/freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/freespace_planning_algorithms/src/rrtstar.cpp @@ -150,14 +150,18 @@ void RRTStar::setRRTPath(const std::vector & waypoints) pose.pose = local2global(costmap_, pose_local); pose.header = header; PlannerWaypoint pw; - if (i == waypoints.size() - 1) { - pw.is_back = waypoints_.waypoints.at(i - 1).is_back; - } else { + if (0 == i) { const auto & pt_now = waypoints.at(i); const auto & pt_next = waypoints.at(i + 1); const double inner_product = cos(pt_now.yaw) * (pt_next.x - pt_now.x) + sin(pt_now.yaw) * (pt_next.y - pt_now.y); pw.is_back = (inner_product < 0.0); + } else { + const auto & pt_pre = waypoints.at(i - 1); + const auto & pt_now = waypoints.at(i); + const double inner_product = + cos(pt_pre.yaw) * (pt_now.x - pt_pre.x) + sin(pt_pre.yaw) * (pt_now.y - pt_pre.y); + pw.is_back = !(inner_product > 0.0); } pw.pose = pose; waypoints_.waypoints.push_back(pw); diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 03b042e3e3179..081fbf6920bc0 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -246,7 +246,7 @@ std::unordered_map rosbag_dir_prefix_table( {RRTSTAR_UPDATE, "fpalgos-rrtstar_update"}, {RRTSTAR_INFORMED_UPDATE, "fpalgos-rrtstar_informed_update"}}); -bool test_algorithm(enum AlgorithmType algo_type) +bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) { std::unique_ptr algo; if (algo_type == AlgorithmType::ASTAR_SINGLE) { @@ -269,8 +269,6 @@ bool test_algorithm(enum AlgorithmType algo_type) rclcpp::Clock clock{RCL_SYSTEM_TIME}; for (size_t i = 0; i < goal_poses.size(); ++i) { - const std::string dir_name = - "/tmp/" + rosbag_dir_prefix_table[algo_type] + "-case" + std::to_string(i); const auto goal_pose = goal_poses.at(i); bool success_local = true; @@ -321,32 +319,38 @@ bool test_algorithm(enum AlgorithmType algo_type) success_all = false; } - rcpputils::fs::remove_all(dir_name); - - rosbag2_storage::StorageOptions storage_options; - storage_options.uri = dir_name; - storage_options.storage_id = "sqlite3"; - - rosbag2_cpp::ConverterOptions converter_options; - converter_options.input_serialization_format = "cdr"; - converter_options.output_serialization_format = "cdr"; - - rosbag2_cpp::Writer writer(std::make_unique()); - writer.open(storage_options, converter_options); - - add_message_to_rosbag( - writer, create_float_msg(vehicle_shape.length), "vehicle_length", "std_msgs/msg/Float64"); - add_message_to_rosbag( - writer, create_float_msg(vehicle_shape.width), "vehicle_width", "std_msgs/msg/Float64"); - add_message_to_rosbag( - writer, create_float_msg(vehicle_shape.base2back), "vehicle_base2back", - "std_msgs/msg/Float64"); - - add_message_to_rosbag(writer, costmap_msg, "costmap", "nav_msgs/msg/OccupancyGrid"); - add_message_to_rosbag(writer, create_pose_msg(start_pose), "start", "geometry_msgs/msg/Pose"); - add_message_to_rosbag(writer, create_pose_msg(goal_pose), "goal", "geometry_msgs/msg/Pose"); - add_message_to_rosbag(writer, trajectory, "trajectory", "geometry_msgs/msg/PoseArray"); - add_message_to_rosbag(writer, create_float_msg(msec), "elapsed_time", "std_msgs/msg/Float64"); + if (dump_rosbag) { + // dump rosbag for visualization using python script + const std::string dir_name = + "/tmp/" + rosbag_dir_prefix_table[algo_type] + "-case" + std::to_string(i); + + rcpputils::fs::remove_all(dir_name); + + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = dir_name; + storage_options.storage_id = "sqlite3"; + + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + + rosbag2_cpp::Writer writer(std::make_unique()); + writer.open(storage_options, converter_options); + + add_message_to_rosbag( + writer, create_float_msg(vehicle_shape.length), "vehicle_length", "std_msgs/msg/Float64"); + add_message_to_rosbag( + writer, create_float_msg(vehicle_shape.width), "vehicle_width", "std_msgs/msg/Float64"); + add_message_to_rosbag( + writer, create_float_msg(vehicle_shape.base2back), "vehicle_base2back", + "std_msgs/msg/Float64"); + + add_message_to_rosbag(writer, costmap_msg, "costmap", "nav_msgs/msg/OccupancyGrid"); + add_message_to_rosbag(writer, create_pose_msg(start_pose), "start", "geometry_msgs/msg/Pose"); + add_message_to_rosbag(writer, create_pose_msg(goal_pose), "goal", "geometry_msgs/msg/Pose"); + add_message_to_rosbag(writer, trajectory, "trajectory", "geometry_msgs/msg/PoseArray"); + add_message_to_rosbag(writer, create_float_msg(msec), "elapsed_time", "std_msgs/msg/Float64"); + } } return success_all; } diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index 028352362a0ba..388f35e770359 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -14,12 +14,13 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Parameters -| Name | Type | Description | -| ------------------------- | ------ | --------------------------------- | -| `map_frame` | string | The frame name for map | -| `arrival_check_angle_deg` | double | Angle threshold for goal check | -| `arrival_check_distance` | double | Distance threshold for goal check | -| `arrival_check_duration` | double | Duration threshold for goal check | +| Name | Type | Description | +| ------------------------- | ------ | ------------------------------------ | +| `map_frame` | string | The frame name for map | +| `arrival_check_angle_deg` | double | Angle threshold for goal check | +| `arrival_check_distance` | double | Distance threshold for goal check | +| `arrival_check_duration` | double | Duration threshold for goal check | +| `goal_angle_threshold` | double | Max goal pose angle for goal approve | ### Services @@ -38,11 +39,12 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Publications -| Name | Type | Description | -| ------------------------------- | --------------------------------------- | ---------------------- | -| `/planning/routing/route_state` | autoware_adapi_v1_msgs::msg::RouteState | route state | -| `/planning/routing/route` | autoware_planning_msgs/LaneletRoute | route | -| `debug/route_marker` | visualization_msgs::msg::MarkerArray | route marker for debug | +| Name | Type | Description | +| ------------------------------- | --------------------------------------- | ------------------------ | +| `/planning/routing/route_state` | autoware_adapi_v1_msgs::msg::RouteState | route state | +| `/planning/routing/route` | autoware_planning_msgs/LaneletRoute | route | +| `debug/route_marker` | visualization_msgs::msg::MarkerArray | route marker for debug | +| `debug/goal_footprint` | visualization_msgs::msg::MarkerArray | goal footprint for debug | ## Route section @@ -57,6 +59,15 @@ The ROS message of route section contains following three elements for each rout - `preferred_primitive`: Preferred lane to follow towards the goal. - `primitives`: All neighbor lanes in the same direction including the preferred lane. +## Goal Validation + +The mission planner has control mechanism to validate the given goal pose and create a route. If goal pose angle between goal pose lanelet and goal pose' yaw is greater than `goal_angle_threshold` parameter, the goal is rejected. +Another control mechanism is the creation of a footprint of the goal pose according to the dimensions of the vehicle and checking whether this footprint is within the lanelets. If goal footprint exceeds lanelets, then the goal is rejected. + +At the image below, there are sample goal pose validation cases. + +![goal_footprints](./media/goal_footprints.svg) + ## Implementation ### Mission Planner diff --git a/planning/mission_planner/launch/mission_planner.launch.xml b/planning/mission_planner/launch/mission_planner.launch.xml index 20c2cfbd2a0b7..d3a36f1e5cf6d 100644 --- a/planning/mission_planner/launch/mission_planner.launch.xml +++ b/planning/mission_planner/launch/mission_planner.launch.xml @@ -8,6 +8,7 @@ + diff --git a/planning/mission_planner/media/goal_footprints.svg b/planning/mission_planner/media/goal_footprints.svg new file mode 100644 index 0000000000000..c3347c0b52410 --- /dev/null +++ b/planning/mission_planner/media/goal_footprints.svg @@ -0,0 +1,195 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Goal Pose is Valid +
+
+
+ + Goal Pose... + + + + + + +
+
+
+ Goal Pose is not Valid, goal footprint exceeds lane boundaries +
+
+
+ + Goal Pose is not Valid, go... + + + + + + +
+
+
+ Goal Pose is not Valid, goal footprint exceeds opposite lane +
+
+
+ + Goal Pose is not Valid, go... + + + + + + +
+
+
+ Goal Pose is Valid +
+
+
+ + Goal Pose... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index 9e5764dfca638..f972c18b66d0d 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -31,6 +31,7 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils + vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index e02d055e20577..b741bd4479763 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -113,6 +114,13 @@ void DefaultPlanner::initialize(rclcpp::Node * node) map_subscriber_ = node_->create_subscription( "input/vector_map", rclcpp::QoS{10}.transient_local(), std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1)); + + const auto durable_qos = rclcpp::QoS(1).transient_local(); + pub_goal_footprint_marker_ = + node_->create_publisher("debug/goal_footprint", durable_qos); + + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); + param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg", 45.0); } void DefaultPlanner::initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) @@ -186,12 +194,110 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) return route_marker_array; } -bool DefaultPlanner::is_goal_valid(const geometry_msgs::msg::Pose & goal) const +visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( + tier4_autoware_utils::LinearRing2d goal_footprint) const +{ + visualization_msgs::msg::MarkerArray msg; + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock().now(), "goal_footprint", 0, visualization_msgs::msg::Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0)); + marker.lifetime = rclcpp::Duration::from_seconds(2.5); + + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0)); + marker.points.push_back(marker.points.front()); + + msg.markers.push_back(marker); + + return msg; +} + +bool DefaultPlanner::check_goal_footprint( + const lanelet::ConstLanelet & current_lanelet, + const lanelet::ConstLanelet & combined_prev_lanelet, + const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length, + const double search_margin) +{ + std::vector points_intersection; + + // check if goal footprint is in current lane + boost::geometry::intersection( + goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon(), points_intersection); + if (points_intersection.empty()) { + return true; + } + points_intersection.clear(); + + // check if goal footprint is in between many lanelets + for (const auto & next_lane : routing_graph_ptr_->following(current_lanelet)) { + next_lane_length += lanelet::utils::getLaneletLength2d(next_lane); + lanelet::ConstLanelets lanelets; + lanelets.push_back(combined_prev_lanelet); + lanelets.push_back(next_lane); + lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets); + + // if next lanelet length longer than vehicle longitudinal offset + if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { + next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane); + boost::geometry::intersection( + goal_footprint, combined_lanelets.polygon2d().basicPolygon(), points_intersection); + if (points_intersection.empty()) { + return true; + } + points_intersection.clear(); + } else { // if next lanelet length shorter than vehicle longitudinal offset -> recursive call + if (!check_goal_footprint(next_lane, combined_lanelets, goal_footprint, next_lane_length)) { + next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane); + continue; + } else { + return true; + } + } + } + return false; +} + +bool DefaultPlanner::is_goal_valid( + const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets) { + const auto logger = node_->get_logger(); lanelet::Lanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) { return false; } + + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + tier4_autoware_utils::LinearRing2d goal_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(goal)); + pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint)); + const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint); + + double next_lane_length = 0.0; + // combine calculated route lanelets + lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets); + + // check if goal footprint exceeds lane when the goal isn't in parking_lot + if ( + !check_goal_footprint( + closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) && + !is_in_parking_lot( + lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_), + lanelet::utils::conversion::toLaneletPoint(goal.position))) { + RCLCPP_WARN(logger, "Goal's footprint exceeds lane!"); + return false; + } + const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { @@ -199,8 +305,7 @@ bool DefaultPlanner::is_goal_valid(const geometry_msgs::msg::Pose & goal) const const auto goal_yaw = tf2::getYaw(goal.orientation); const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - constexpr double th_angle = M_PI / 4; - + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } @@ -230,12 +335,11 @@ bool DefaultPlanner::is_goal_valid(const geometry_msgs::msg::Pose & goal) const const auto goal_yaw = tf2::getYaw(goal.orientation); const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - constexpr double th_angle = M_PI / 4; + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } } - return false; } @@ -255,11 +359,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) LaneletRoute route_msg; RouteSections route_sections; - if (!is_goal_valid(points.back())) { - RCLCPP_WARN(logger, "Goal is not valid! Please check position and angle of goal_pose"); - return route_msg; - } - + lanelet::ConstLanelets all_route_lanelets; for (std::size_t i = 1; i < points.size(); i++) { const auto start_check_point = points.at(i - 1); const auto goal_check_point = points.at(i); @@ -268,12 +368,20 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) start_check_point, goal_check_point, &path_lanelets)) { return route_msg; } + for (const auto & lane : path_lanelets) { + all_route_lanelets.push_back(lane); + } // create local route sections route_handler_.setRouteLanelets(path_lanelets); const auto local_route_sections = route_handler_.createMapSegments(path_lanelets); route_sections = combine_consecutive_route_sections(route_sections, local_route_sections); } + if (!is_goal_valid(points.back(), all_route_lanelets)) { + RCLCPP_WARN(logger, "Goal is not valid! Please check position and angle of goal_pose"); + return route_msg; + } + if (route_handler_.isRouteLooped(route_sections)) { RCLCPP_WARN(logger, "Loop detected within route!"); return route_msg; diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index e532261e73e1f..20f731953667a 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -32,6 +33,11 @@ namespace mission_planner::lanelet2 { +struct DefaultPlannerParameters +{ + double goal_angle_threshold_deg; +}; + class DefaultPlanner : public mission_planner::PlannerPlugin { public: @@ -40,6 +46,8 @@ class DefaultPlanner : public mission_planner::PlannerPlugin bool ready() const override; LaneletRoute plan(const RoutePoints & points) override; MarkerArray visualize(const LaneletRoute & route) const override; + MarkerArray visualize_debug_footprint(tier4_autoware_utils::LinearRing2d goal_footprint_) const; + vehicle_info_util::VehicleInfo vehicle_info_; private: using RouteSections = std::vector; @@ -52,11 +60,19 @@ class DefaultPlanner : public mission_planner::PlannerPlugin lanelet::ConstLanelets shoulder_lanelets_; route_handler::RouteHandler route_handler_; + DefaultPlannerParameters param_; + rclcpp::Node * node_; rclcpp::Subscription::SharedPtr map_subscriber_; + rclcpp::Publisher::SharedPtr pub_goal_footprint_marker_; void map_callback(const HADMapBin::ConstSharedPtr msg); - bool is_goal_valid(const geometry_msgs::msg::Pose & goal) const; + bool check_goal_footprint( + const lanelet::ConstLanelet & current_lanelet, + const lanelet::ConstLanelet & combined_prev_lanelet, + const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length, + const double search_margin = 2.0); + bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets); Pose refine_goal_height(const Pose & goal, const RouteSections & route_sections); }; diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index d31d9ec3c0623..eaadd019b462c 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -14,15 +14,32 @@ #include "utility_functions.hpp" +#include + #include #include +#include bool exists(const std::unordered_set & set, const lanelet::Id & id) { return set.find(id) != set.end(); } +tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon( + tier4_autoware_utils::LinearRing2d footprint) +{ + tier4_autoware_utils::Polygon2d footprint_polygon; + boost::geometry::append(footprint_polygon.outer(), footprint[0]); + boost::geometry::append(footprint_polygon.outer(), footprint[1]); + boost::geometry::append(footprint_polygon.outer(), footprint[2]); + boost::geometry::append(footprint_polygon.outer(), footprint[3]); + boost::geometry::append(footprint_polygon.outer(), footprint[4]); + boost::geometry::append(footprint_polygon.outer(), footprint[5]); + boost::geometry::correct(footprint_polygon); + return footprint_polygon; +} + void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) { cl->r = r; @@ -36,3 +53,35 @@ void insert_marker_array( { a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } + +lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + lanelet::Points3d centers; + std::vector bound_ids; + + for (const auto & llt : lanelets) { + if (llt.id() != 0) { + bound_ids.push_back(llt.leftBound().id()); + bound_ids.push_back(llt.rightBound().id()); + } + } + + for (const auto & llt : lanelets) { + if (std::count(bound_ids.begin(), bound_ids.end(), llt.leftBound().id()) < 2) { + for (const auto & pt : llt.leftBound()) { + lefts.push_back(lanelet::Point3d(pt)); + } + } + if (std::count(bound_ids.begin(), bound_ids.end(), llt.rightBound().id()) < 2) { + for (const auto & pt : llt.rightBound()) { + rights.push_back(lanelet::Point3d(pt)); + } + } + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); + auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + return std::move(combined_lanelet); +} diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 8217502225439..2d986ac8e56ed 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -15,7 +15,11 @@ #ifndef LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + #include +#include +#include #include #include @@ -41,8 +45,11 @@ bool exists(const std::vector & vectors, const T & item) return false; } +tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon( + tier4_autoware_utils::LinearRing2d footprint); void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a); void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); +lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets); #endif // LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt index a215e158fab04..b1b29a1a5f389 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/obstacle_avoidance_planner/CMakeLists.txt @@ -10,12 +10,10 @@ find_package(OpenCV REQUIRED) ament_auto_add_library(obstacle_avoidance_planner SHARED src/vehicle_model/vehicle_model_interface.cpp src/vehicle_model/vehicle_model_bicycle_kinematics.cpp - src/costmap_generator.cpp src/eb_path_optimizer.cpp src/mpt_optimizer.cpp src/utils/utils.cpp src/utils/debug_utils.cpp - src/utils/cv_utils.cpp src/node.cpp ) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index 4fb800e4a8ead..2e1ce6502441b 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -36,16 +36,6 @@ using VehicleBounds = std::vector; using SequentialBounds = std::vector; using BoundsCandidates = std::vector; -using SequentialBoundsCandidates = std::vector; - -struct CVMaps -{ - cv::Mat drivable_area; - cv::Mat clearance_map; - cv::Mat only_objects_clearance_map; - cv::Mat area_with_objects_map; - nav_msgs::msg::MapMetaData map_info; -}; struct QPParam { @@ -171,7 +161,7 @@ struct DebugData std::vector extended_fixed_traj; std::vector extended_non_fixed_traj; - SequentialBoundsCandidates sequential_bounds_candidates; + BoundsCandidates bounds_candidates; }; struct Trajectories diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp deleted file mode 100644 index cdff21bd71132..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2021 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 OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ - -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" - -#include "boost/optional.hpp" - -#include -#include -#include -#include - -class CostmapGenerator -{ -public: - CVMaps getMaps( - const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & objects, - const TrajectoryParam & traj_param, DebugData & debug_data); - -private: - mutable tier4_autoware_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; - - cv::Mat getAreaWithObjects( - const cv::Mat & drivable_area, const cv::Mat & objects_image, DebugData & debug_data) const; - - cv::Mat getClearanceMap(const cv::Mat & drivable_area, DebugData & debug_data) const; - - cv::Mat drawObstaclesOnImage( - const bool enable_avoidance, - const std::vector & objects, - const std::vector & path_points, - const nav_msgs::msg::MapMetaData & map_info, [[maybe_unused]] const cv::Mat & drivable_area, - const cv::Mat & clearance_map, const TrajectoryParam & traj_param, - std::vector * debug_avoiding_objects, - DebugData & debug_data) const; - - cv::Mat getDrivableAreaInCV( - const nav_msgs::msg::OccupancyGrid & occupancy_grid, DebugData & debug_data) const; -}; -#endif // OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index 81453424d3a0b..d48f1f384ae52 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -201,8 +201,9 @@ class MPTOptimizer std::vector getReferencePoints( const std::vector & points, - const std::unique_ptr & prev_mpt_points, const bool enable_avoidance, - const CVMaps & maps, DebugData & debug_data) const; + const std::vector & left_bound, + const std::vector & right_bound, + const std::unique_ptr & prev_mpt_points, DebugData & debug_data) const; void calcPlanningFromEgo(std::vector & ref_points) const; @@ -218,12 +219,11 @@ class MPTOptimizer const std::unique_ptr & prev_trajs) const; void calcBounds( - std::vector & ref_points, const bool enable_avoidance, const CVMaps & maps, - const std::unique_ptr & prev_trajs, DebugData & debug_data) const; + std::vector & ref_points, + const std::vector & left_bound, + const std::vector & right_bound, DebugData & debug_data) const; - void calcVehicleBounds( - std::vector & ref_points, const CVMaps & maps, DebugData & debug_data, - const bool enable_avoidance) const; + void calcVehicleBounds(std::vector & ref_points, DebugData & debug_data) const; // void calcFixState( // std::vector & ref_points, @@ -269,19 +269,6 @@ class MPTOptimizer std::vector getMPTFixedPoints( const std::vector & ref_points) const; - BoundsCandidates getBoundsCandidates( - const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, - const CVMaps & maps, DebugData & debug_data) const; - - CollisionType getCollisionType( - const CVMaps & maps, const bool enable_avoidance, - const geometry_msgs::msg::Pose & avoiding_point, const double traversed_dist, - const double bound_angle) const; - - boost::optional getClearance( - const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & map_info) const; - ObjectiveMatrix getObjectiveMatrix( const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, [[maybe_unused]] const std::vector & ref_points, DebugData & debug_data) const; @@ -299,7 +286,9 @@ class MPTOptimizer const bool enable_avoidance, const std::vector & smoothed_points, const std::vector & path_points, - const std::unique_ptr & prev_trajs, const CVMaps & maps, + const std::vector & left_bound, + const std::vector & right_bound, + const std::unique_ptr & prev_trajs, const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, DebugData & debug_data); }; diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 4e57bac6a0848..6b9683fb24400 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -16,7 +16,6 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/costmap_generator.hpp" #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "opencv2/core.hpp" @@ -191,7 +190,6 @@ class ObstacleAvoidancePlanner : public rclcpp::Node double max_delta_time_sec_for_replan_; // core algorithm - std::unique_ptr costmap_generator_ptr_; std::unique_ptr eb_path_optimizer_ptr_; std::unique_ptr mpt_optimizer_ptr_; @@ -266,7 +264,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node Trajectory generateTrajectory(const PlannerData & planner_data); - Trajectories optimizeTrajectory(const PlannerData & planner_data, const CVMaps & cv_maps); + Trajectories optimizeTrajectory(const PlannerData & planner_data); Trajectories getPrevTrajs(const std::vector & path_points) const; @@ -274,8 +272,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node const std::vector & path_points, std::vector & traj_points) const; void insertZeroVelocityOutsideDrivableArea( - const PlannerData & planner_data, std::vector & traj_points, - const CVMaps & cv_maps); + const PlannerData & planner_data, std::vector & traj_points); void publishDebugDataInOptimization( const PlannerData & planner_data, const std::vector & traj_points); diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp deleted file mode 100644 index da5edace43536..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp +++ /dev/null @@ -1,121 +0,0 @@ -// Copyright 2020 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 OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_ - -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "opencv2/core.hpp" -#include "opencv2/imgproc/imgproc_c.h" -#include "opencv2/opencv.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "geometry_msgs/msg/point32.hpp" - -#include "boost/optional/optional_fwd.hpp" - -#include -#include - -namespace util -{ -struct Footprint; -} - -struct Edges -{ - int front_idx; - int back_idx; - geometry_msgs::msg::Point extended_front; - geometry_msgs::msg::Point extended_back; - geometry_msgs::msg::Point origin; -}; - -struct PolygonPoints -{ - std::vector points_in_image; - std::vector points_in_map; -}; - -namespace cv_utils -{ -void getOccupancyGridValue( - const nav_msgs::msg::OccupancyGrid & og, const int i, const int j, unsigned char & value); - -void putOccupancyGridValue( - nav_msgs::msg::OccupancyGrid & og, const int i, const int j, const unsigned char value); -} // namespace cv_utils - -namespace cv_polygon_utils -{ -PolygonPoints getPolygonPoints( - const std::vector & points, - const nav_msgs::msg::MapMetaData & map_info); - -PolygonPoints getPolygonPoints( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); - -PolygonPoints getPolygonPointsFromBB( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); - -PolygonPoints getPolygonPointsFromCircle( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); - -PolygonPoints getPolygonPointsFromPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); - -std::vector getCVPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const PolygonPoints & polygon_points, - const std::vector & path_points, - const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info); - -std::vector getDefaultCVPolygon( - const std::vector & points_in_image); - -std::vector getExtendedCVPolygon( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info); - -boost::optional getEdges( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info); -} // namespace cv_polygon_utils - -namespace cv_drivable_area_utils -{ -bool isOutsideDrivableAreaFromRectangleFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - const VehicleParam & vehicle_param); - -bool isOutsideDrivableAreaFromCirclesFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - const std::vector vehicle_circle_longitudinal_offsets, - const double vehicle_circle_radius); -} // namespace cv_drivable_area_utils - -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp index 083b5dd00efec..506587f63f5e6 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp @@ -42,9 +42,6 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( DebugData & debug_data, const VehicleParam & vehicle_param); - -nav_msgs::msg::OccupancyGrid getDebugCostmap( - const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid); } // namespace debug_utils #endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp index 03129dd1e4bd7..1f1b3dbdf9249 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp @@ -354,4 +354,12 @@ namespace utils void logOSQPSolutionStatus(const int solution_status, const std::string & msg); } // namespace utils +namespace drivable_area_utils +{ +bool isOutsideDrivableAreaFromRectangleFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const std::vector left_bound, + const std::vector right_bound, const VehicleParam & vehicle_param); +} + #endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp deleted file mode 100644 index 0936ccc9854c3..0000000000000 --- a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp +++ /dev/null @@ -1,337 +0,0 @@ -// Copyright 2021 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 "obstacle_avoidance_planner/costmap_generator.hpp" - -#include "obstacle_avoidance_planner/utils/cv_utils.hpp" -#include "obstacle_avoidance_planner/utils/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - -namespace -{ -cv::Point toCVPoint(const geometry_msgs::msg::Point & p) -{ - cv::Point cv_point; - cv_point.x = p.x; - cv_point.y = p.y; - return cv_point; -} - -bool isAvoidingObjectType( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const TrajectoryParam & traj_param) -{ - if ( - (object.classification.at(0).label == object.classification.at(0).UNKNOWN && - traj_param.is_avoiding_unknown) || - (object.classification.at(0).label == object.classification.at(0).CAR && - traj_param.is_avoiding_car) || - (object.classification.at(0).label == object.classification.at(0).TRUCK && - traj_param.is_avoiding_truck) || - (object.classification.at(0).label == object.classification.at(0).BUS && - traj_param.is_avoiding_bus) || - (object.classification.at(0).label == object.classification.at(0).BICYCLE && - traj_param.is_avoiding_bicycle) || - (object.classification.at(0).label == object.classification.at(0).MOTORCYCLE && - traj_param.is_avoiding_motorbike) || - (object.classification.at(0).label == object.classification.at(0).PEDESTRIAN && - traj_param.is_avoiding_pedestrian)) { - return true; - } - return false; -} - -bool arePointsInsideDriveableArea( - const std::vector & image_points, const cv::Mat & clearance_map) -{ - bool points_inside_area = false; - for (const auto & image_point : image_points) { - const float clearance = - clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)]; - if (clearance > 0) { - points_inside_area = true; - } - } - return points_inside_area; -} - -bool isAvoidingObject( - const PolygonPoints & polygon_points, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info, - const std::vector & path_points, - const TrajectoryParam & traj_param) -{ - if (path_points.empty()) { - return false; - } - if (!isAvoidingObjectType(object, traj_param)) { - return false; - } - const auto image_point = geometry_utils::transformMapToOptionalImage( - object.kinematics.initial_pose_with_covariance.pose.position, map_info); - if (!image_point) { - return false; - } - - // TODO(murooka) remove findNearestIndex without any constraints - const int nearest_idx = motion_utils::findNearestIndex( - path_points, object.kinematics.initial_pose_with_covariance.pose.position); - const auto nearest_path_point = path_points[nearest_idx]; - const auto rel_p = geometry_utils::transformToRelativeCoordinate2D( - object.kinematics.initial_pose_with_covariance.pose.position, nearest_path_point.pose); - // skip object located back the beginning of path points - if (nearest_idx == 0 && rel_p.x < 0) { - return false; - } - - /* - const float object_clearance_from_road = - clearance_map.ptr( - static_cast(image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - */ - const geometry_msgs::msg::Vector3 twist = - object.kinematics.initial_twist_with_covariance.twist.linear; - const double vel = std::sqrt(twist.x * twist.x + twist.y * twist.y + twist.z * twist.z); - /* - const auto nearest_path_point_image = - geometry_utils::transformMapToOptionalImage(nearest_path_point.pose.position, map_info); - if (!nearest_path_point_image) { - return false; - } - const float nearest_path_point_clearance = - clearance_map.ptr(static_cast( - nearest_path_point_image.get().y))[static_cast(nearest_path_point_image.get().x)] * - map_info.resolution; - */ - const double lateral_offset_to_path = motion_utils::calcLateralOffset( - path_points, object.kinematics.initial_pose_with_covariance.pose.position); - if ( - // nearest_path_point_clearance - traj_param.center_line_width * 0.5 < - // object_clearance_from_road || - std::abs(lateral_offset_to_path) < traj_param.center_line_width * 0.5 || - vel > traj_param.max_avoiding_objects_velocity_ms || - !arePointsInsideDriveableArea(polygon_points.points_in_image, clearance_map)) { - return false; - } - return true; -} -} // namespace - -namespace tier4_autoware_utils -{ -template <> -geometry_msgs::msg::Point getPoint(const cv::Point & p) -{ - geometry_msgs::msg::Point geom_p; - geom_p.x = p.x; - geom_p.y = p.y; - - return geom_p; -} -} // namespace tier4_autoware_utils - -CVMaps CostmapGenerator::getMaps( - const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & objects, - const TrajectoryParam & traj_param, DebugData & debug_data) -{ - stop_watch_.tic(__func__); - - // make cv_maps - CVMaps cv_maps; - - cv_maps.drivable_area = getDrivableAreaInCV(path.drivable_area, debug_data); - cv_maps.clearance_map = getClearanceMap(cv_maps.drivable_area, debug_data); - - std::vector debug_avoiding_objects; - cv::Mat objects_image = drawObstaclesOnImage( - enable_avoidance, objects, path.points, path.drivable_area.info, cv_maps.drivable_area, - cv_maps.clearance_map, traj_param, &debug_avoiding_objects, debug_data); - - cv_maps.area_with_objects_map = - getAreaWithObjects(cv_maps.drivable_area, objects_image, debug_data); - cv_maps.only_objects_clearance_map = getClearanceMap(objects_image, debug_data); - cv_maps.map_info = path.drivable_area.info; - - // debug data - debug_data.clearance_map = cv_maps.clearance_map; - debug_data.only_object_clearance_map = cv_maps.only_objects_clearance_map; - debug_data.area_with_objects_map = cv_maps.area_with_objects_map; - debug_data.avoiding_objects = debug_avoiding_objects; - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return cv_maps; -} - -cv::Mat CostmapGenerator::getDrivableAreaInCV( - const nav_msgs::msg::OccupancyGrid & occupancy_grid, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); - - cv::Mat drivable_area = cv::Mat(occupancy_grid.info.width, occupancy_grid.info.height, CV_8UC1); - - drivable_area.forEach([&](unsigned char & value, const int * position) -> void { - cv_utils::getOccupancyGridValue(occupancy_grid, position[0], position[1], value); - }); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return drivable_area; -} - -cv::Mat CostmapGenerator::getClearanceMap( - const cv::Mat & drivable_area, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); - - cv::Mat clearance_map; - cv::distanceTransform(drivable_area, clearance_map, cv::DIST_L2, 5); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return clearance_map; -} - -cv::Mat CostmapGenerator::drawObstaclesOnImage( - const bool enable_avoidance, - const std::vector & objects, - const std::vector & path_points, - const nav_msgs::msg::MapMetaData & map_info, [[maybe_unused]] const cv::Mat & drivable_area, - const cv::Mat & clearance_map, const TrajectoryParam & traj_param, - std::vector * debug_avoiding_objects, - DebugData & debug_data) const -{ - stop_watch_.tic(__func__); - - std::vector path_points_inside_area; - for (const auto & point : path_points) { - std::vector points; - geometry_msgs::msg::Point image_point; - if (!geometry_utils::transformMapToImage(point.pose.position, map_info, image_point)) { - continue; - } - const float clearance = - clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)]; - if (clearance < 1e-5) { - continue; - } - path_points_inside_area.push_back(point); - } - - // NOTE: objects image is too sparse so that creating cost map is heavy. - // Then, objects image is created by filling dilated drivable area, - // instead of "cv::Mat objects_image = cv::Mat::ones(clearance_map.rows, clearance_map.cols, - // CV_8UC1) * 255;". - constexpr double dilate_margin = 1.0; - cv::Mat objects_image; - const int dilate_size = static_cast( - (1.8 + dilate_margin) / - (std::sqrt(2) * map_info.resolution)); // TODO(murooka) use clearance_from_object - cv::dilate(drivable_area, objects_image, cv::Mat(), cv::Point(-1, -1), dilate_size); - - if (!enable_avoidance) { - return objects_image; - } - - // fill object - std::vector> cv_polygons; - std::vector> obj_cog_info; - std::vector obj_positions; - for (const auto & object : objects) { - const PolygonPoints polygon_points = cv_polygon_utils::getPolygonPoints(object, map_info); - if (isAvoidingObject( - polygon_points, object, clearance_map, map_info, path_points_inside_area, traj_param)) { - const double lon_dist_to_path = motion_utils::calcSignedArcLength( - path_points, 0, object.kinematics.initial_pose_with_covariance.pose.position); - const double lat_dist_to_path = motion_utils::calcLateralOffset( - path_points, object.kinematics.initial_pose_with_covariance.pose.position); - obj_cog_info.push_back({lon_dist_to_path, lat_dist_to_path}); - obj_positions.push_back(object.kinematics.initial_pose_with_covariance.pose.position); - - cv_polygons.push_back(cv_polygon_utils::getCVPolygon( - object, polygon_points, path_points_inside_area, clearance_map, map_info)); - debug_avoiding_objects->push_back(object); - } - } - for (const auto & polygon : cv_polygons) { - cv::fillConvexPoly(objects_image, polygon, cv::Scalar(0)); - } - - // fill between objects in the same side - const auto get_closest_obj_point = [&](size_t idx) { - // TODO(murooka) remove findNearestIndex without any constraints - const auto & path_point = - path_points.at(motion_utils::findNearestIndex(path_points, obj_positions.at(idx))); - double min_dist = std::numeric_limits::min(); - size_t min_idx = 0; - for (size_t p_idx = 0; p_idx < cv_polygons.at(idx).size(); ++p_idx) { - const double dist = - tier4_autoware_utils::calcDistance2d(cv_polygons.at(idx).at(p_idx), path_point); - if (dist < min_dist) { - min_dist = dist; - min_idx = p_idx; - } - } - - geometry_msgs::msg::Point geom_point; - geom_point.x = cv_polygons.at(idx).at(min_idx).x; - geom_point.y = cv_polygons.at(idx).at(min_idx).y; - return geom_point; - }; - - std::vector> cv_between_polygons; - for (size_t i = 0; i < obj_positions.size(); ++i) { - for (size_t j = i + 1; j < obj_positions.size(); ++j) { - const auto & obj_info1 = obj_cog_info.at(i); - const auto & obj_info2 = obj_cog_info.at(j); - - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("lat"), obj_info1.at(1) << " " << obj_info2.at(1)); - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("lon"), obj_info1.at(0) << " " << obj_info2.at(0)); - - constexpr double max_lon_dist_to_convex_obstacles = 30.0; - if ( - obj_info1.at(1) * obj_info2.at(1) < 0 || - std::abs(obj_info1.at(0) - obj_info2.at(0)) > max_lon_dist_to_convex_obstacles) { - continue; - } - - std::vector cv_points; - cv_points.push_back(toCVPoint(obj_positions.at(i))); - cv_points.push_back(toCVPoint(get_closest_obj_point(i))); - cv_points.push_back(toCVPoint(get_closest_obj_point(j))); - cv_points.push_back(toCVPoint(obj_positions.at(j))); - - cv_between_polygons.push_back(cv_points); - } - } - /* - for (const auto & polygon : cv_between_polygons) { - cv::fillConvexPoly(objects_image, polygon, cv::Scalar(0)); - } - */ - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - - return objects_image; -} - -cv::Mat CostmapGenerator::getAreaWithObjects( - const cv::Mat & drivable_area, const cv::Mat & objects_image, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); - - cv::Mat area_with_objects = cv::min(objects_image, drivable_area); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return area_with_objects; -} diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 56d74c9eab4ef..acc5ab823ca59 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -18,9 +18,11 @@ #include "obstacle_avoidance_planner/utils/utils.hpp" #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "tf2/utils.h" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "nav_msgs/msg/map_meta_data.hpp" +#include "boost/assign/list_of.hpp" #include "boost/optional.hpp" #include @@ -73,44 +75,6 @@ Bounds findWidestBounds(const BoundsCandidates & front_bounds_candidates) } */ -Bounds findNearestBounds( - const geometry_msgs::msg::Pose & bounds_pose, const BoundsCandidates & front_bounds_candidates, - const geometry_msgs::msg::Point & target_pos) -{ - double min_dist = std::numeric_limits::max(); - size_t min_dist_index = 0; - if (front_bounds_candidates.size() != 1) { - for (size_t candidate_idx = 0; candidate_idx < front_bounds_candidates.size(); - ++candidate_idx) { - const auto & front_bounds_candidate = front_bounds_candidates.at(candidate_idx); - - const double bound_center = - (front_bounds_candidate.upper_bound + front_bounds_candidate.lower_bound) / 2.0; - const auto center_pos = - tier4_autoware_utils::calcOffsetPose(bounds_pose, 0.0, bound_center, 0.0); - const double dist = tier4_autoware_utils::calcDistance2d(center_pos, target_pos); - - if (dist < min_dist) { - min_dist_index = candidate_idx; - min_dist = dist; - } - } - } - return front_bounds_candidates.at(min_dist_index); -} - -double calcOverlappedBoundsSignedLength( - const Bounds & prev_front_continuous_bounds, const Bounds & front_bounds_candidate) -{ - const double min_ub = - std::min(front_bounds_candidate.upper_bound, prev_front_continuous_bounds.upper_bound); - const double max_lb = - std::max(front_bounds_candidate.lower_bound, prev_front_continuous_bounds.lower_bound); - - const double overlapped_signed_length = min_ub - max_lb; - return overlapped_signed_length; -} - geometry_msgs::msg::Pose calcVehiclePose( const ReferencePoint & ref_point, const double lat_error, const double yaw_error, const double offset) @@ -202,6 +166,47 @@ size_t findNearestIndexWithSoftYawConstraints( return nearest_idx_optional ? *nearest_idx_optional : motion_utils::findNearestIndex(points_with_yaw, pose.position); } + +boost::optional intersection( + const Eigen::Vector2d & start_point1, const Eigen::Vector2d & end_point1, + const Eigen::Vector2d & start_point2, const Eigen::Vector2d & end_point2) +{ + using Point = boost::geometry::model::d2::point_xy; + using Line = boost::geometry::model::linestring; + + const Line line1 = + boost::assign::list_of(start_point1(0), start_point1(1))(end_point1(0), end_point1(1)); + const Line line2 = + boost::assign::list_of(start_point2(0), start_point2(1))(end_point2(0), end_point2(1)); + + std::vector output; + boost::geometry::intersection(line1, line2, output); + if (output.empty()) { + return {}; + } + + const Eigen::Vector2d output_point{output.front().x(), output.front().y()}; + return output_point; +} + +double calcLateralDistToBound( + const Eigen::Vector2d & current_point, const Eigen::Vector2d & edge_point, + const std::vector & bound, const bool is_right_bound = false) +{ + for (size_t i = 0; i < bound.size() - 1; ++i) { + const Eigen::Vector2d current_bound_point = {bound.at(i).x, bound.at(i).y}; + const Eigen::Vector2d next_bound_point = {bound.at(i + 1).x, bound.at(i + 1).y}; + + const auto intersects_point = + intersection(current_point, edge_point, current_bound_point, next_bound_point); + if (intersects_point) { + const double dist = (*intersects_point - current_point).norm(); + return is_right_bound ? -dist : dist; + } + } + + return is_right_bound ? -5.0 : 5.0; +} } // namespace MPTOptimizer::MPTOptimizer( @@ -221,7 +226,9 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto const bool enable_avoidance, const std::vector & smoothed_points, const std::vector & path_points, - const std::unique_ptr & prev_trajs, const CVMaps & maps, + const std::vector & left_bound, + const std::vector & right_bound, + const std::unique_ptr & prev_trajs, const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, DebugData & debug_data) { @@ -238,7 +245,7 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto } std::vector full_ref_points = - getReferencePoints(smoothed_points, prev_trajs, enable_avoidance, maps, debug_data); + getReferencePoints(smoothed_points, left_bound, right_bound, prev_trajs, debug_data); if (full_ref_points.empty()) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, @@ -322,8 +329,9 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto std::vector MPTOptimizer::getReferencePoints( const std::vector & smoothed_points, - const std::unique_ptr & prev_trajs, const bool enable_avoidance, - const CVMaps & maps, DebugData & debug_data) const + const std::vector & left_bound, + const std::vector & right_bound, + const std::unique_ptr & prev_trajs, DebugData & debug_data) const { stop_watch_.tic(__func__); @@ -406,8 +414,8 @@ std::vector MPTOptimizer::getReferencePoints( } // set bounds information - calcBounds(ref_points, enable_avoidance, maps, prev_trajs, debug_data); - calcVehicleBounds(ref_points, maps, debug_data, enable_avoidance); + calcBounds(ref_points, left_bound, right_bound, debug_data); + calcVehicleBounds(ref_points, debug_data); // set extra information (alpha and has_object_collision) // NOTE: This must be after bounds calculation. @@ -1376,109 +1384,73 @@ void MPTOptimizer::addSteerWeightR( } void MPTOptimizer::calcBounds( - std::vector & ref_points, const bool enable_avoidance, const CVMaps & maps, - const std::unique_ptr & prev_trajs, DebugData & debug_data) const + std::vector & ref_points, + const std::vector & left_bound, + const std::vector & right_bound, DebugData & debug_data) const { stop_watch_.tic(__func__); - // search bounds candidate for each ref points - SequentialBoundsCandidates sequential_bounds_candidates; - for (const auto & ref_point : ref_points) { - const auto bounds_candidates = - getBoundsCandidates(enable_avoidance, convertRefPointsToPose(ref_point), maps, debug_data); - sequential_bounds_candidates.push_back(bounds_candidates); + if (left_bound.empty() || right_bound.empty()) { + std::cerr << "[ObstacleAvoidancePlanner]: Boundary is empty when calculating bounds" + << std::endl; + return; } - debug_data.sequential_bounds_candidates = sequential_bounds_candidates; - - // search continuous and widest bounds only for front point - for (size_t i = 0; i < sequential_bounds_candidates.size(); ++i) { - // NOTE: back() is the front avoiding circle - const auto & bounds_candidates = sequential_bounds_candidates.at(i); - - // extract only continuous bounds; - if (i == 0) { // TODO(murooka) use previous bounds, not widest bounds - const auto target_pos = [&]() { - if (prev_trajs && !prev_trajs->mpt_ref_points.empty()) { - return prev_trajs->mpt_ref_points.front().p; - } - return ref_points.at(i).p; - }(); - - geometry_msgs::msg::Pose ref_pose; - ref_pose.position = ref_points.at(i).p; - ref_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(i).yaw); - - const auto & nearest_bounds = findNearestBounds(ref_pose, bounds_candidates, target_pos); - ref_points.at(i).bounds = nearest_bounds; - } else { - const auto & prev_ref_point = ref_points.at(i - 1); - const auto & prev_continuous_bounds = prev_ref_point.bounds; - - BoundsCandidates filtered_bounds_candidates; - for (const auto & bounds_candidate : bounds_candidates) { - // Step 1. Bounds is continuous to the previous one, - // and the overlapped signed length is longer than vehicle width - // overlapped_signed_length already considers vehicle width. - const double overlapped_signed_length = - calcOverlappedBoundsSignedLength(prev_continuous_bounds, bounds_candidate); - if (overlapped_signed_length < 0) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "non-overlapped length bounds is ignored."); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "In detail, prev: lower=%f, upper=%f, candidate: lower=%f, upper=%f", - prev_continuous_bounds.lower_bound, prev_continuous_bounds.upper_bound, - bounds_candidate.lower_bound, bounds_candidate.upper_bound); - continue; - } - // Step 2. Bounds is longer than vehicle width. - // bounds_length already considers vehicle width. - const double bounds_length = bounds_candidate.upper_bound - bounds_candidate.lower_bound; - if (bounds_length < 0) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "negative length bounds is ignored."); - continue; - } - - filtered_bounds_candidates.push_back(bounds_candidate); - } - - // Step 3. Nearest bounds to trajectory - if (!filtered_bounds_candidates.empty()) { - const auto nearest_bounds = std::min_element( - filtered_bounds_candidates.begin(), filtered_bounds_candidates.end(), - [](const auto & a, const auto & b) { - return std::min(std::abs(a.lower_bound), std::abs(a.upper_bound)) < - std::min(std::abs(b.lower_bound), std::abs(b.upper_bound)); - }); - if ( - filtered_bounds_candidates.begin() <= nearest_bounds && - nearest_bounds < filtered_bounds_candidates.end()) { - ref_points.at(i).bounds = *nearest_bounds; - continue; - } - } + /* + const double min_soft_road_clearance = vehicle_param_.width / 2.0 + + mpt_param_.soft_clearance_from_road + + mpt_param_.extra_desired_clearance_from_road; + */ + const double min_soft_road_clearance = vehicle_param_.width / 2.0; - // invalid bounds - RCLCPP_WARN_EXPRESSION( - rclcpp::get_logger("getBounds: not front"), is_showing_debug_info_, "invalid bounds"); - const auto invalid_bounds = - Bounds{-5.0, 5.0, CollisionType::OUT_OF_ROAD, CollisionType::OUT_OF_ROAD}; - ref_points.at(i).bounds = invalid_bounds; - } + // search bounds candidate for each ref points + debug_data.bounds_candidates.clear(); + for (size_t i = 0; i < ref_points.size() - 1; ++i) { + const auto curr_ref_position = convertRefPointsToPose(ref_points.at(i)).position; + const auto next_ref_position = convertRefPointsToPose(ref_points.at(i + 1)).position; + const Eigen::Vector2d current_ref_point = {curr_ref_position.x, curr_ref_position.y}; + const Eigen::Vector2d next_ref_point = {next_ref_position.x, next_ref_position.y}; + const Eigen::Vector2d current_to_next_vec = next_ref_point - current_ref_point; + const Eigen::Vector2d left_vertical_vec = {-current_to_next_vec(1), current_to_next_vec(0)}; + const Eigen::Vector2d right_vertical_vec = {current_to_next_vec(1), -current_to_next_vec(0)}; + + const Eigen::Vector2d left_point = current_ref_point + left_vertical_vec.normalized() * 5.0; + const Eigen::Vector2d right_point = current_ref_point + right_vertical_vec.normalized() * 5.0; + const double lat_dist_to_left_bound = std::min( + calcLateralDistToBound(current_ref_point, left_point, left_bound) - min_soft_road_clearance, + 5.0); + const double lat_dist_to_right_bound = std::max( + calcLateralDistToBound(current_ref_point, right_point, right_bound, true) + + min_soft_road_clearance, + -5.0); + + ref_points.at(i).bounds = Bounds{ + lat_dist_to_right_bound, lat_dist_to_left_bound, CollisionType::NO_COLLISION, + CollisionType::NO_COLLISION}; + debug_data.bounds_candidates.push_back(ref_points.at(i).bounds); } + // Terminal Boundary + const double lat_dist_to_left_bound = + -motion_utils::calcLateralOffset( + left_bound, convertRefPointsToPose(ref_points.back()).position) - + min_soft_road_clearance; + const double lat_dist_to_right_bound = + -motion_utils::calcLateralOffset( + right_bound, convertRefPointsToPose(ref_points.back()).position) + + min_soft_road_clearance; + ref_points.back().bounds = Bounds{ + lat_dist_to_right_bound, lat_dist_to_left_bound, CollisionType::NO_COLLISION, + CollisionType::NO_COLLISION}; + debug_data.bounds_candidates.push_back(ref_points.back().bounds); + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return; } void MPTOptimizer::calcVehicleBounds( - std::vector & ref_points, [[maybe_unused]] const CVMaps & maps, - DebugData & debug_data, [[maybe_unused]] const bool enable_avoidance) const + std::vector & ref_points, DebugData & debug_data) const { stop_watch_.tic(__func__); @@ -1558,168 +1530,3 @@ void MPTOptimizer::calcVehicleBounds( debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; } - -BoundsCandidates MPTOptimizer::getBoundsCandidates( - const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, const CVMaps & maps, - [[maybe_unused]] DebugData & debug_data) const -{ - BoundsCandidates bounds_candidate; - - constexpr double max_search_lane_width = 5.0; - const auto search_widths = mpt_param_.bounds_search_widths; - - // search right to left - const double bound_angle = - tier4_autoware_utils::normalizeRadian(tf2::getYaw(avoiding_point.orientation) + M_PI_2); - - double traversed_dist = -max_search_lane_width; - double current_right_bound = -max_search_lane_width; - - // calculate the initial position is empty or not - // 0.drivable, 1.out of map, 2.out of road, 3. object - CollisionType previous_collision_type = - getCollisionType(maps, enable_avoidance, avoiding_point, traversed_dist, bound_angle); - - const auto has_collision = [&](const CollisionType & collision_type) -> bool { - return collision_type == CollisionType::OUT_OF_ROAD || collision_type == CollisionType::OBJECT; - }; - CollisionType latest_right_bound_collision_type = previous_collision_type; - - while (traversed_dist < max_search_lane_width) { - for (size_t search_idx = 0; search_idx < search_widths.size(); ++search_idx) { - const double ds = search_widths.at(search_idx); - while (true) { - const CollisionType current_collision_type = - getCollisionType(maps, enable_avoidance, avoiding_point, traversed_dist, bound_angle); - - if (has_collision(current_collision_type)) { // currently collision - if (!has_collision(previous_collision_type)) { - // if target_position becomes collision from no collision or out_of_sight - if (search_idx == search_widths.size() - 1) { - const double left_bound = traversed_dist - ds / 2.0; - bounds_candidate.push_back(Bounds{ - current_right_bound, left_bound, latest_right_bound_collision_type, - current_collision_type}); - previous_collision_type = current_collision_type; - } - break; - } - } else if (current_collision_type == CollisionType::OUT_OF_SIGHT) { // currently - // out_of_sight - if (previous_collision_type == CollisionType::NO_COLLISION) { - // if target_position becomes out_of_sight from no collision - if (search_idx == search_widths.size() - 1) { - const double left_bound = max_search_lane_width; - bounds_candidate.push_back(Bounds{ - current_right_bound, left_bound, latest_right_bound_collision_type, - current_collision_type}); - previous_collision_type = current_collision_type; - } - break; - } - } else { // currently no collision - if (has_collision(previous_collision_type)) { - // if target_position becomes no collision from collision - if (search_idx == search_widths.size() - 1) { - current_right_bound = traversed_dist - ds / 2.0; - latest_right_bound_collision_type = previous_collision_type; - previous_collision_type = current_collision_type; - } - break; - } - } - - // if target_position is longer than max_search_lane_width - if (traversed_dist >= max_search_lane_width) { - if (!has_collision(previous_collision_type)) { - if (search_idx == search_widths.size() - 1) { - const double left_bound = traversed_dist - ds / 2.0; - bounds_candidate.push_back(Bounds{ - current_right_bound, left_bound, latest_right_bound_collision_type, - CollisionType::OUT_OF_ROAD}); - } - } - break; - } - - // go forward with ds - traversed_dist += ds; - previous_collision_type = current_collision_type; - } - - if (search_idx != search_widths.size() - 1) { - // go back with ds since target_position became empty or road/object - // NOTE: if ds is the last of search_widths, don't have to go back - traversed_dist -= ds; - } - } - } - - // if empty - // TODO(murooka) sometimes this condition realizes - if (bounds_candidate.empty()) { - RCLCPP_WARN_EXPRESSION( - rclcpp::get_logger("getBoundsCandidate"), is_showing_debug_info_, "empty bounds candidate"); - // NOTE: set invalid bounds so that MPT won't be solved - const auto invalid_bounds = - Bounds{-5.0, 5.0, CollisionType::OUT_OF_ROAD, CollisionType::OUT_OF_ROAD}; - bounds_candidate.push_back(invalid_bounds); - } - - return bounds_candidate; -} - -// 0.NO_COLLISION, 1.OUT_OF_SIGHT, 2.OUT_OF_ROAD, 3.OBJECT -CollisionType MPTOptimizer::getCollisionType( - const CVMaps & maps, const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, - const double traversed_dist, const double bound_angle) const -{ - // calculate clearance - const double min_soft_road_clearance = vehicle_param_.width / 2.0 + - mpt_param_.soft_clearance_from_road + - mpt_param_.extra_desired_clearance_from_road; - const double min_obj_clearance = vehicle_param_.width / 2.0 + mpt_param_.clearance_from_object + - mpt_param_.soft_clearance_from_road; - - // calculate target position - const geometry_msgs::msg::Point target_pos = tier4_autoware_utils::createPoint( - avoiding_point.position.x + traversed_dist * std::cos(bound_angle), - avoiding_point.position.y + traversed_dist * std::sin(bound_angle), 0.0); - - const auto opt_road_clearance = getClearance(maps.clearance_map, target_pos, maps.map_info); - const auto opt_obj_clearance = - getClearance(maps.only_objects_clearance_map, target_pos, maps.map_info); - - // object has more priority than road, so its condition exists first - if (enable_avoidance && opt_obj_clearance) { - const bool is_obj = opt_obj_clearance.get() < min_obj_clearance; - if (is_obj) { - return CollisionType::OBJECT; - } - } - - if (opt_road_clearance) { - const bool out_of_road = opt_road_clearance.get() < min_soft_road_clearance; - if (out_of_road) { - return CollisionType::OUT_OF_ROAD; - } else { - return CollisionType::NO_COLLISION; - } - } - - return CollisionType::OUT_OF_SIGHT; -} - -boost::optional MPTOptimizer::getClearance( - const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & map_info) const -{ - const auto image_point = geometry_utils::transformMapToOptionalImage(map_point, map_info); - if (!image_point) { - return boost::none; - } - const float clearance = clearance_map.ptr(static_cast( - image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - return clearance; -} diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 4120034db3744..2775089335128 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -16,7 +16,6 @@ #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/motion_utils.hpp" -#include "obstacle_avoidance_planner/utils/cv_utils.hpp" #include "obstacle_avoidance_planner/utils/debug_utils.hpp" #include "obstacle_avoidance_planner/utils/utils.hpp" #include "rclcpp/time.hpp" @@ -847,8 +846,6 @@ void ObstacleAvoidancePlanner::resetPlanning() { RCLCPP_WARN(get_logger(), "[ObstacleAvoidancePlanner] Reset planning"); - costmap_generator_ptr_ = std::make_unique(); - eb_path_optimizer_ptr_ = std::make_unique( is_showing_debug_info_, traj_param_, eb_param_, vehicle_param_); @@ -869,12 +866,14 @@ void ObstacleAvoidancePlanner::onPath(const Path::SharedPtr path_ptr) { stop_watch_.tic(__func__); - if ( - path_ptr->points.empty() || path_ptr->drivable_area.data.empty() || !current_twist_ptr_ || - !objects_ptr_) { + if (path_ptr->points.empty() || !current_twist_ptr_ || !objects_ptr_) { return; } + if (path_ptr->left_bound.empty() || path_ptr->right_bound.empty()) { + std::cerr << "Right or left bound is empty" << std::endl; + } + // create planner data PlannerData planner_data; planner_data.path = *path_ptr; @@ -956,12 +955,8 @@ std::vector ObstacleAvoidancePlanner::generateOptimizedTrajecto } prev_replanned_time_ptr_ = std::make_unique(this->now()); - // create clearance maps - const CVMaps cv_maps = costmap_generator_ptr_->getMaps( - enable_avoidance_, path, planner_data.objects, traj_param_, debug_data_); - // calculate trajectory with EB and MPT - auto optimal_trajs = optimizeTrajectory(planner_data, cv_maps); + auto optimal_trajs = optimizeTrajectory(planner_data); // calculate velocity // NOTE: Velocity is not considered in optimization. @@ -969,8 +964,7 @@ std::vector ObstacleAvoidancePlanner::generateOptimizedTrajecto // insert 0 velocity when trajectory is over drivable area if (is_stopping_if_outside_drivable_area_) { - insertZeroVelocityOutsideDrivableArea( - planner_data, optimal_trajs.model_predictive_trajectory, cv_maps); + insertZeroVelocityOutsideDrivableArea(planner_data, optimal_trajs.model_predictive_trajectory); } publishDebugDataInOptimization(planner_data, optimal_trajs.model_predictive_trajectory); @@ -1122,8 +1116,7 @@ bool ObstacleAvoidancePlanner::isEgoNearToPrevTrajectory(const geometry_msgs::ms return true; } -Trajectories ObstacleAvoidancePlanner::optimizeTrajectory( - const PlannerData & planner_data, const CVMaps & cv_maps) +Trajectories ObstacleAvoidancePlanner::optimizeTrajectory(const PlannerData & planner_data) { stop_watch_.tic(__func__); @@ -1176,8 +1169,8 @@ Trajectories ObstacleAvoidancePlanner::optimizeTrajectory( // MPT: optimize trajectory to be kinematically feasible and collision free const auto mpt_trajs = mpt_optimizer_ptr_->getModelPredictiveTrajectory( - enable_avoidance_, eb_traj.get(), p.path.points, prev_optimal_trajs_ptr_, cv_maps, p.ego_pose, - p.ego_vel, debug_data_); + enable_avoidance_, eb_traj.get(), p.path.points, p.path.left_bound, p.path.right_bound, + prev_optimal_trajs_ptr_, p.ego_pose, p.ego_vel, debug_data_); if (!mpt_trajs) { return getPrevTrajs(p.path.points); } @@ -1231,8 +1224,7 @@ void ObstacleAvoidancePlanner::calcVelocity( } void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( - const PlannerData & planner_data, std::vector & traj_points, - const CVMaps & cv_maps) + const PlannerData & planner_data, std::vector & traj_points) { if (traj_points.empty()) { return; @@ -1240,9 +1232,6 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( stop_watch_.tic(__func__); - const auto & map_info = cv_maps.map_info; - const auto & road_clearance_map = cv_maps.clearance_map; - const size_t nearest_idx = findEgoNearestIndex(traj_points, planner_data.ego_pose); // NOTE: Some end trajectory points will be ignored to check if outside the drivable area @@ -1258,12 +1247,18 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( return traj_points.size(); }(); + const auto left_bound = planner_data.path.left_bound; + const auto right_bound = planner_data.path.right_bound; + if (left_bound.empty() || right_bound.empty()) { + return; + } + for (size_t i = nearest_idx; i < end_idx; ++i) { const auto & traj_point = traj_points.at(i); // calculate the first point being outside drivable area - const bool is_outside = cv_drivable_area_utils::isOutsideDrivableAreaFromRectangleFootprint( - traj_point, road_clearance_map, map_info, vehicle_param_); + const bool is_outside = drivable_area_utils::isOutsideDrivableAreaFromRectangleFootprint( + traj_point, left_bound, right_bound, vehicle_param_); // only insert zero velocity to the first point outside drivable area if (is_outside) { @@ -1616,25 +1611,6 @@ void ObstacleAvoidancePlanner::publishDebugDataInMain(const Path & path) const debug_extended_non_fixed_traj_pub_->publish(debug_extended_non_fixed_traj); } - { // publish clearance map - stop_watch_.tic("publishClearanceMap"); - - if (is_publishing_area_with_objects_) { // false - debug_area_with_objects_pub_->publish( - debug_utils::getDebugCostmap(debug_data_.area_with_objects_map, path.drivable_area)); - } - if (is_publishing_object_clearance_map_) { // false - debug_object_clearance_map_pub_->publish( - debug_utils::getDebugCostmap(debug_data_.only_object_clearance_map, path.drivable_area)); - } - if (is_publishing_clearance_map_) { // true - debug_clearance_map_pub_->publish( - debug_utils::getDebugCostmap(debug_data_.clearance_map, path.drivable_area)); - } - debug_data_.msg_stream << " getDebugCostMap * 3:= " << stop_watch_.toc("publishClearanceMap") - << " [ms]\n"; - } - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; } diff --git a/planning/obstacle_avoidance_planner/src/utils/cv_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/cv_utils.cpp deleted file mode 100644 index a543df9d54a9d..0000000000000 --- a/planning/obstacle_avoidance_planner/src/utils/cv_utils.cpp +++ /dev/null @@ -1,470 +0,0 @@ -// Copyright 2020 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 "obstacle_avoidance_planner/utils/cv_utils.hpp" - -#include "obstacle_avoidance_planner/utils/utils.hpp" -#include "tf2/utils.h" -#include "tier4_autoware_utils/system/stop_watch.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" - -#include "boost/optional.hpp" - -#include -#include -#include - -namespace -{ -boost::optional getDistance( - const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & map_info) -{ - const auto image_point = geometry_utils::transformMapToOptionalImage(map_point, map_info); - if (!image_point) { - return boost::none; - } - const float clearance = clearance_map.ptr(static_cast( - image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - return clearance; -} - -bool isOutsideDrivableArea( - const geometry_msgs::msg::Point & pos, const cv::Mat & road_clearance_map, - const nav_msgs::msg::MapMetaData & map_info, const double max_dist) -{ - const auto dist = getDistance(road_clearance_map, pos, map_info); - if (dist) { - return dist.get() < max_dist; - } - - return false; -} -} // namespace - -namespace cv_utils -{ -void getOccupancyGridValue( - const nav_msgs::msg::OccupancyGrid & og, const int i, const int j, unsigned char & value) -{ - int i_flip = og.info.width - i - 1; - int j_flip = og.info.height - j - 1; - if (og.data[i_flip + j_flip * og.info.width] > 0) { - value = 0; - } else { - value = 255; - } -} - -void putOccupancyGridValue( - nav_msgs::msg::OccupancyGrid & og, const int i, const int j, const unsigned char value) -{ - int i_flip = og.info.width - i - 1; - int j_flip = og.info.height - j - 1; - og.data[i_flip + j_flip * og.info.width] = value; -} -} // namespace cv_utils - -namespace cv_polygon_utils -{ -PolygonPoints getPolygonPoints( - const std::vector & points, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - for (const auto & point : points) { - const auto image_point = geometry_utils::transformMapToOptionalImage(point, map_info); - if (image_point) { - points_in_image.push_back(image_point.get()); - points_in_map.push_back(point); - } - } - PolygonPoints polygon_points; - polygon_points.points_in_image = points_in_image; - polygon_points.points_in_map = points_in_map; - return polygon_points; -} - -PolygonPoints getPolygonPoints( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - PolygonPoints polygon_points; - if (object.shape.type == object.shape.BOUNDING_BOX) { - polygon_points = getPolygonPointsFromBB(object, map_info); - } else if (object.shape.type == object.shape.CYLINDER) { - polygon_points = getPolygonPointsFromCircle(object, map_info); - } else if (object.shape.type == object.shape.POLYGON) { - polygon_points = getPolygonPointsFromPolygon(object, map_info); - } - return polygon_points; -} - -PolygonPoints getPolygonPointsFromBB( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - const double dim_x = object.shape.dimensions.x; - const double dim_y = object.shape.dimensions.y; - const std::vector rel_x = {0.5 * dim_x, 0.5 * dim_x, -0.5 * dim_x, -0.5 * dim_x}; - const std::vector rel_y = {0.5 * dim_y, -0.5 * dim_y, -0.5 * dim_y, 0.5 * dim_y}; - const geometry_msgs::msg::Pose object_pose = object.kinematics.initial_pose_with_covariance.pose; - for (size_t i = 0; i < rel_x.size(); i++) { - geometry_msgs::msg::Point rel_point; - rel_point.x = rel_x[i]; - rel_point.y = rel_y[i]; - auto abs_point = geometry_utils::transformToAbsoluteCoordinate2D(rel_point, object_pose); - geometry_msgs::msg::Point image_point; - if (geometry_utils::transformMapToImage(abs_point, map_info, image_point)) { - points_in_image.push_back(image_point); - points_in_map.push_back(abs_point); - } - } - PolygonPoints polygon_points; - polygon_points.points_in_image = points_in_image; - polygon_points.points_in_map = points_in_map; - return polygon_points; -} - -PolygonPoints getPolygonPointsFromCircle( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - const double radius = object.shape.dimensions.x; - const geometry_msgs::msg::Point center = - object.kinematics.initial_pose_with_covariance.pose.position; - constexpr int num_sampling_points = 5; - for (int i = 0; i < num_sampling_points; ++i) { - std::vector deltas = {0, 1.0}; - for (const auto & delta : deltas) { - geometry_msgs::msg::Point point; - point.x = std::cos( - ((i + delta) / static_cast(num_sampling_points)) * 2.0 * M_PI + - M_PI / static_cast(num_sampling_points)) * - (radius / 2.0) + - center.x; - point.y = std::sin( - ((i + delta) / static_cast(num_sampling_points)) * 2.0 * M_PI + - M_PI / static_cast(num_sampling_points)) * - (radius / 2.0) + - center.y; - point.z = center.z; - geometry_msgs::msg::Point image_point; - if (geometry_utils::transformMapToImage(point, map_info, image_point)) { - points_in_image.push_back(image_point); - points_in_map.push_back(point); - } - } - } - PolygonPoints polygon_points; - polygon_points.points_in_image = points_in_image; - polygon_points.points_in_map = points_in_map; - return polygon_points; -} - -PolygonPoints getPolygonPointsFromPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - for (const auto & polygon_p : object.shape.footprint.points) { - geometry_msgs::msg::Point rel_point; - rel_point.x = polygon_p.x; - rel_point.y = polygon_p.y; - geometry_msgs::msg::Point point = geometry_utils::transformToAbsoluteCoordinate2D( - rel_point, object.kinematics.initial_pose_with_covariance.pose); - const auto image_point = geometry_utils::transformMapToOptionalImage(point, map_info); - if (image_point) { - points_in_image.push_back(image_point.get()); - points_in_map.push_back(point); - } - } - PolygonPoints polygon_points; - polygon_points.points_in_image = points_in_image; - polygon_points.points_in_map = points_in_map; - return polygon_points; -} - -std::vector getCVPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const PolygonPoints & polygon_points, - const std::vector & path_points, - const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info) -{ - // TODO(murooka) remove findNearestIndex without any constraints - const int nearest_idx = motion_utils::findNearestIndex( - path_points, object.kinematics.initial_pose_with_covariance.pose.position); - const auto nearest_path_point = path_points[nearest_idx]; - if (path_points.empty()) { - return getDefaultCVPolygon(polygon_points.points_in_image); - } - return getExtendedCVPolygon( - polygon_points.points_in_image, polygon_points.points_in_map, nearest_path_point.pose, object, - clearance_map, map_info); -} - -std::vector getDefaultCVPolygon( - const std::vector & points_in_image) -{ - std::vector cv_polygon; - for (const auto & point : points_in_image) { - cv::Point image_point = cv::Point(point.x, point.y); - cv_polygon.push_back(image_point); - } - return cv_polygon; -} - -std::vector getExtendedCVPolygon( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) -{ - const boost::optional optional_edges = getEdges( - points_in_image, points_in_map, nearest_path_point_pose, object, clearance_map, map_info); - if (!optional_edges) { - return getDefaultCVPolygon(points_in_image); - } - const Edges edges = optional_edges.get(); - - // TODO(murooka) remove findNearestIndex without any constraints - const int nearest_polygon_idx = motion_utils::findNearestIndex(points_in_image, edges.origin); - std::vector cv_polygon; - if (edges.back_idx == nearest_polygon_idx || edges.front_idx == nearest_polygon_idx) { - // make polygon only with edges and extended edges - } else if (edges.back_idx < nearest_polygon_idx) { - // back_idx -> nearest_idx -> frond_idx - if (edges.back_idx < edges.front_idx && nearest_polygon_idx < edges.front_idx) { - for (int i = edges.back_idx + 1; i < edges.front_idx; i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - // back_idx -> vector_front -> vector_back -> nearest_idx -> frond_idx - } else if (edges.back_idx < edges.front_idx && nearest_polygon_idx > edges.front_idx) { - for (int i = edges.back_idx - 1; i >= 0; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = points_in_image.size() - 1; i > edges.front_idx; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - // back_idx -> vector_back -> vector_front -> nearest_idx -> front_idx - } else { - for (size_t i = edges.back_idx + 1; i < points_in_image.size(); i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = 0; i < edges.front_idx; i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - } - } else { - // back_idx -> nearest_idx -> front_idx - if (edges.back_idx >= edges.front_idx && nearest_polygon_idx > edges.front_idx) { - for (int i = edges.back_idx - 1; i > edges.front_idx; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - // back_idx -> vector_back -> vector_front -> nearest_idx -> front_idx - } else { - if (edges.back_idx >= edges.front_idx && nearest_polygon_idx < edges.front_idx) { - for (size_t i = edges.back_idx + 1; i < points_in_image.size(); i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = 0; i < edges.front_idx; i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - } else { // back_idx -> vector_front -> vector_back -> nearest_idx -> front_idx - for (int i = edges.back_idx - 1; i >= 0; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = points_in_image.size() - 1; i > edges.front_idx; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - } - } - } - cv_polygon.push_back( - cv::Point(points_in_image[edges.front_idx].x, points_in_image[edges.front_idx].y)); - cv_polygon.push_back(cv::Point(edges.extended_front.x, edges.extended_front.y)); - cv_polygon.push_back(cv::Point(edges.extended_back.x, edges.extended_back.y)); - cv_polygon.push_back( - cv::Point(points_in_image[edges.back_idx].x, points_in_image[edges.back_idx].y)); - return cv_polygon; -} - -boost::optional getEdges( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) -{ - // calculate perpendicular point to object along with path point orientation - const double yaw = tf2::getYaw(nearest_path_point_pose.orientation); - const Eigen::Vector2d rel_path_vec(std::cos(yaw), std::sin(yaw)); - const Eigen::Vector2d obj_vec( - object.kinematics.initial_pose_with_covariance.pose.position.x, - object.kinematics.initial_pose_with_covariance.pose.position.y); - const double inner_product = rel_path_vec[0] * (obj_vec[0] - nearest_path_point_pose.position.x) + - rel_path_vec[1] * (obj_vec[1] - nearest_path_point_pose.position.y); - geometry_msgs::msg::Point origin; - origin.x = nearest_path_point_pose.position.x + rel_path_vec[0] * inner_product; - origin.y = nearest_path_point_pose.position.y + rel_path_vec[1] * inner_product; - const Eigen::Vector2d obj2origin(origin.x - obj_vec[0], origin.y - obj_vec[1]); - - // calculate origin for casting ray to edges - const auto path_point_image = - geometry_utils::transformMapToImage(nearest_path_point_pose.position, map_info); - constexpr double ray_origin_dist_scale = 1.0; - const float clearance = clearance_map.ptr(static_cast( - path_point_image.y))[static_cast(path_point_image.x)] * - map_info.resolution * ray_origin_dist_scale; - const Eigen::Vector2d obj2ray_origin = obj2origin.normalized() * (obj2origin.norm() + clearance); - geometry_msgs::msg::Point ray_origin; - ray_origin.x = obj_vec[0] + obj2ray_origin[0]; - ray_origin.y = obj_vec[1] + obj2ray_origin[1]; - geometry_msgs::msg::Point ray_origin_image; - ray_origin_image = geometry_utils::transformMapToImage(ray_origin, map_info); - - double min_cos = std::numeric_limits::max(); - double max_cos = std::numeric_limits::lowest(); - const double path_yaw = tf2::getYaw(nearest_path_point_pose.orientation); - const double dx1 = std::cos(path_yaw); - const double dy1 = std::sin(path_yaw); - const Eigen::Vector2d path_point_vec(dx1, dy1); - const double path_point_vec_norm = path_point_vec.norm(); - Edges edges; - for (size_t i = 0; i < points_in_image.size(); i++) { - const double dx2 = points_in_map[i].x - ray_origin.x; - const double dy2 = points_in_map[i].y - ray_origin.y; - const Eigen::Vector2d path_point2point(dx2, dy2); - const double inner_product = path_point_vec.dot(path_point2point); - const double cos = inner_product / (path_point_vec_norm * path_point2point.norm()); - if (cos > max_cos) { - max_cos = cos; - edges.front_idx = i; - } - if (cos < min_cos) { - min_cos = cos; - edges.back_idx = i; - } - } - - const double max_sin = std::sqrt(1 - max_cos * max_cos); - const double min_sin = std::sqrt(1 - min_cos * min_cos); - const Eigen::Vector2d point2front_edge( - points_in_image[edges.front_idx].x - ray_origin_image.x, - points_in_image[edges.front_idx].y - ray_origin_image.y); - const Eigen::Vector2d point2back_edge( - points_in_image[edges.back_idx].x - ray_origin_image.x, - points_in_image[edges.back_idx].y - ray_origin_image.y); - const Eigen::Vector2d point2extended_front_edge = - point2front_edge.normalized() * (clearance * 2 / max_sin) * (1 / map_info.resolution); - const Eigen::Vector2d point2extended_back_edge = - point2back_edge.normalized() * (clearance * 2 / min_sin) * (1 / map_info.resolution); - - const double dist2extended_front_edge = point2extended_front_edge.norm() * map_info.resolution; - const double dist2front_edge = point2front_edge.norm() * map_info.resolution; - const double dist2extended_back_edge = point2extended_back_edge.norm() * map_info.resolution; - const double dist2back_edge = point2back_edge.norm() * map_info.resolution; - if ( - dist2extended_front_edge < clearance * 2 || dist2extended_back_edge < clearance * 2 || - dist2front_edge > dist2extended_front_edge || dist2back_edge > dist2extended_back_edge) { - return boost::none; - } - geometry_msgs::msg::Point extended_front; - geometry_msgs::msg::Point extended_back; - extended_front.x = point2extended_front_edge(0) + ray_origin_image.x; - extended_front.y = point2extended_front_edge(1) + ray_origin_image.y; - extended_back.x = point2extended_back_edge(0) + ray_origin_image.x; - extended_back.y = point2extended_back_edge(1) + ray_origin_image.y; - edges.extended_front = extended_front; - edges.extended_back = extended_back; - edges.origin = ray_origin_image; - return edges; -} -} // namespace cv_polygon_utils - -namespace cv_drivable_area_utils -{ -bool isOutsideDrivableAreaFromRectangleFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - const VehicleParam & vehicle_param) -{ - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - - const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; - const double base_to_rear = vehicle_param.rear_overhang; - - const auto top_left_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_left, 0.0) - .position; - const auto top_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_right, 0.0) - .position; - const auto bottom_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_right, 0.0) - .position; - const auto bottom_left_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_left, 0.0) - .position; - - constexpr double epsilon = 1e-8; - const bool out_top_left = - isOutsideDrivableArea(top_left_pos, road_clearance_map, map_info, epsilon); - const bool out_top_right = - isOutsideDrivableArea(top_right_pos, road_clearance_map, map_info, epsilon); - const bool out_bottom_left = - isOutsideDrivableArea(bottom_left_pos, road_clearance_map, map_info, epsilon); - const bool out_bottom_right = - isOutsideDrivableArea(bottom_right_pos, road_clearance_map, map_info, epsilon); - - if (out_top_left || out_top_right || out_bottom_left || out_bottom_right) { - return true; - } - - return false; -} - -[[maybe_unused]] bool isOutsideDrivableAreaFromCirclesFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - const std::vector vehicle_circle_longitudinal_offsets, const double vehicle_circle_radius) -{ - for (const double offset : vehicle_circle_longitudinal_offsets) { - const auto avoiding_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, offset, 0.0, 0.0).position; - - const bool outside_drivable_area = - isOutsideDrivableArea(avoiding_pos, road_clearance_map, map_info, vehicle_circle_radius); - if (outside_drivable_area) { - return true; - } - } - - return false; -} - -} // namespace cv_drivable_area_utils diff --git a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp index f8f494684d7cc..7cab7cb64d202 100644 --- a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp @@ -15,7 +15,6 @@ #include "obstacle_avoidance_planner/utils/debug_utils.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/utils/cv_utils.hpp" #include "obstacle_avoidance_planner/utils/utils.hpp" #include "tf2/utils.h" @@ -438,9 +437,9 @@ visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray( } visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray( - const std::vector & ref_points, - std::vector> & bounds_candidates, const double r, const double g, - const double b, [[maybe_unused]] const double vehicle_width, const size_t sampling_num) + const std::vector & ref_points, std::vector & bounds_candidates, + const double r, const double g, const double b, [[maybe_unused]] const double vehicle_width, + const size_t sampling_num) { const auto current_time = rclcpp::Clock().now(); visualization_msgs::msg::MarkerArray msg; @@ -459,23 +458,19 @@ visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray( } const auto & bound_candidates = bounds_candidates.at(i); - for (size_t j = 0; j < bound_candidates.size(); ++j) { - geometry_msgs::msg::Pose pose; - pose.position = ref_points.at(i).p; - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(i).yaw); - - // lower bound - const double lb_y = bound_candidates.at(j).lower_bound; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; - marker.points.push_back(lb); - - // upper bound - const double ub_y = bound_candidates.at(j).upper_bound; - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; - marker.points.push_back(ub); - - msg.markers.push_back(marker); - } + geometry_msgs::msg::Pose pose; + pose.position = ref_points.at(i).p; + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(i).yaw); + + // lower bound + const double lb_y = bound_candidates.lower_bound; + const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + marker.points.push_back(lb); + + // upper bound + const double ub_y = bound_candidates.upper_bound; + const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + marker.points.push_back(ub); } return msg; @@ -766,8 +761,8 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( // bounds candidates appendMarkerArray( getBoundsCandidatesLineMarkerArray( - debug_data.ref_points, debug_data.sequential_bounds_candidates, 0.2, 0.99, 0.99, - vehicle_param.width, debug_data.mpt_visualize_sampling_num), + debug_data.ref_points, debug_data.bounds_candidates, 0.2, 0.99, 0.99, vehicle_param.width, + debug_data.mpt_visualize_sampling_num), &vis_marker_array); // vehicle circle line @@ -817,19 +812,4 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( } return vis_marker_array; } - -nav_msgs::msg::OccupancyGrid getDebugCostmap( - const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid) -{ - if (clearance_map.empty()) return nav_msgs::msg::OccupancyGrid(); - - cv::Mat tmp; - clearance_map.copyTo(tmp); - cv::normalize(tmp, tmp, 0, 255, cv::NORM_MINMAX, CV_8UC1); - nav_msgs::msg::OccupancyGrid clearance_map_in_og = occupancy_grid; - tmp.forEach([&](const unsigned char & value, const int * position) -> void { - cv_utils::putOccupancyGridValue(clearance_map_in_og, position[0], position[1], value); - }); - return clearance_map_in_og; -} } // namespace debug_utils diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp index cc3edd1c42223..66bd14c7c7fba 100644 --- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/utils.cpp @@ -601,3 +601,108 @@ void logOSQPSolutionStatus(const int solution_status, const std::string & msg) } } } // namespace utils + +namespace +{ +geometry_msgs::msg::Point getStartPoint( + const std::vector & bound, const geometry_msgs::msg::Point & point) +{ + const size_t segment_idx = motion_utils::findNearestSegmentIndex(bound, point); + const auto & curr_seg_point = bound.at(segment_idx); + const auto & next_seg_point = bound.at(segment_idx); + const Eigen::Vector2d first_to_target{point.x - curr_seg_point.x, point.y - curr_seg_point.y}; + const Eigen::Vector2d first_to_second{ + next_seg_point.x - curr_seg_point.x, next_seg_point.y - curr_seg_point.y}; + const double length = first_to_target.dot(first_to_second.normalized()); + + if (length < 0.0) { + return bound.front(); + } + + const auto first_point = motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); + + if (first_point) { + return *first_point; + } + + return bound.front(); +} + +bool isOutsideDrivableArea( + const geometry_msgs::msg::Point & point, + const std::vector & left_bound, + const std::vector & right_bound) +{ + if (left_bound.empty() || right_bound.empty()) { + return false; + } + + constexpr double min_dist = 0.1; + const auto left_start_point = getStartPoint(left_bound, right_bound.front()); + const auto right_start_point = getStartPoint(right_bound, left_bound.front()); + + // ignore point in front of the front line + const std::vector front_bound = {left_start_point, right_start_point}; + const double lat_dist_to_front_bound = motion_utils::calcLateralOffset(front_bound, point); + if (lat_dist_to_front_bound > min_dist) { + return false; + } + + // left bound check + const double lat_dist_to_left_bound = motion_utils::calcLateralOffset(left_bound, point); + if (lat_dist_to_left_bound > min_dist) { + return true; + } + + // right bound check + const double lat_dist_to_right_bound = motion_utils::calcLateralOffset(right_bound, point); + if (lat_dist_to_right_bound < -min_dist) { + return true; + } + + return false; +} +} // namespace + +namespace drivable_area_utils +{ +bool isOutsideDrivableAreaFromRectangleFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const std::vector left_bound, + const std::vector right_bound, const VehicleParam & vehicle_param) +{ + if (left_bound.empty() || right_bound.empty()) { + return false; + } + + const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; + const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; + + const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; + const double base_to_rear = vehicle_param.rear_overhang; + + const auto top_left_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_left, 0.0) + .position; + const auto top_right_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_right, 0.0) + .position; + const auto bottom_right_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_right, 0.0) + .position; + const auto bottom_left_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_left, 0.0) + .position; + + const bool out_top_left = isOutsideDrivableArea(top_left_pos, left_bound, right_bound); + const bool out_top_right = isOutsideDrivableArea(top_right_pos, left_bound, right_bound); + const bool out_bottom_left = isOutsideDrivableArea(bottom_left_pos, left_bound, right_bound); + const bool out_bottom_right = isOutsideDrivableArea(bottom_right_pos, left_bound, right_bound); + + if (out_top_left || out_top_right || out_bottom_left || out_bottom_right) { + return true; + } + + return false; +} +} // namespace drivable_area_utils diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index a6ca4a7a91563..7fe64181ff420 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -21,6 +21,7 @@ #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "tier4_planning_msgs/msg/stop_reason_array.hpp" #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" @@ -32,6 +33,8 @@ #include #include +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -50,6 +53,8 @@ class PlannerInterface { stop_reasons_pub_ = node.create_publisher("~/output/stop_reasons", 1); + velocity_factors_pub_ = + node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); } @@ -110,6 +115,7 @@ class PlannerInterface // Publishers rclcpp::Publisher::SharedPtr stop_reasons_pub_; + rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; // Vehicle Parameters diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index 399fe1a39608b..dd294674bb160 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -16,6 +16,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs geometry_msgs diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index db2c9c4407ca6..45ed1d3f92d0e 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -72,6 +72,26 @@ tier4_planning_msgs::msg::StopReasonArray makeEmptyStopReasonArray( return stop_reason_array; } +VelocityFactorArray makeVelocityFactorArray( + const rclcpp::Time & time, const std::optional pose = std::nullopt) +{ + VelocityFactorArray velocity_factor_array; + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = time; + + if (pose) { + using distance_type = VelocityFactor::_distance_type; + VelocityFactor velocity_factor; + velocity_factor.type = VelocityFactor::ROUTE_OBSTACLE; + velocity_factor.pose = pose.value(); + velocity_factor.distance = std::numeric_limits::quiet_NaN(); + velocity_factor.status = VelocityFactor::UNKNOWN; + velocity_factor.detail = std::string(); + velocity_factor_array.factors.push_back(velocity_factor); + } + return velocity_factor_array; +} + double calcMinimumDistanceToStop( const double initial_vel, const double max_acc, const double min_acc) { @@ -92,6 +112,7 @@ Trajectory PlannerInterface::generateStopTrajectory( if (planner_data.target_obstacles.empty()) { stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time)); + velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time)); // delete marker const auto markers = @@ -207,6 +228,7 @@ Trajectory PlannerInterface::generateStopTrajectory( const auto stop_reasons_msg = makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle); stop_reasons_pub_->publish(stop_reasons_msg); + velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); // Publish if ego vehicle collides with the obstacle with a limit acceleration const auto stop_speed_exceeded_msg = diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp index 2b34a92c4c747..2d3dc40914895 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -43,6 +44,8 @@ using std_msgs::msg::Header; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -113,6 +116,7 @@ class ObstacleStopPlannerDebugNode MarkerArray makeVirtualWallMarker(); MarkerArray makeVisualizationMarker(); StopReasonArray makeStopReasonArray(); + VelocityFactorArray makeVelocityFactorArray(); void setDebugValues(const DebugValues::TYPE type, const double val) { @@ -124,6 +128,7 @@ class ObstacleStopPlannerDebugNode rclcpp::Publisher::SharedPtr virtual_wall_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr stop_reason_pub_; + rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr pub_debug_values_; rclcpp::Node * node_; double base_link2front_; diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index eb03e76b520ce..5b48e16a848d3 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -19,6 +19,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs diagnostic_msgs diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index 813c4c3b9c7dc..683bbdf9e6113 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -46,6 +46,8 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( virtual_wall_pub_ = node_->create_publisher("~/virtual_wall", 1); debug_viz_pub_ = node_->create_publisher("~/debug/marker", 1); stop_reason_pub_ = node_->create_publisher("~/output/stop_reasons", 1); + velocity_factor_pub_ = + node_->create_publisher("/planning/velocity_factors/obstacle_stop", 1); pub_debug_values_ = node_->create_publisher("~/obstacle_stop/debug_values", 1); } @@ -149,6 +151,8 @@ void ObstacleStopPlannerDebugNode::publish() /* publish stop reason for autoware api */ const auto stop_reason_msg = makeStopReasonArray(); stop_reason_pub_->publish(stop_reason_msg); + const auto velocity_factor_msg = makeVelocityFactorArray(); + velocity_factor_pub_->publish(velocity_factor_msg); // publish debug values Float32MultiArrayStamped debug_msg{}; @@ -389,4 +393,23 @@ StopReasonArray ObstacleStopPlannerDebugNode::makeStopReasonArray() return stop_reason_array; } +VelocityFactorArray ObstacleStopPlannerDebugNode::makeVelocityFactorArray() +{ + VelocityFactorArray velocity_factor_array; + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = node_->now(); + + if (stop_pose_ptr_) { + using distance_type = VelocityFactor::_distance_type; + VelocityFactor velocity_factor; + velocity_factor.type = VelocityFactor::ROUTE_OBSTACLE; + velocity_factor.pose = *stop_pose_ptr_; + velocity_factor.distance = std::numeric_limits::quiet_NaN(); + velocity_factor.status = VelocityFactor::UNKNOWN; + velocity_factor.detail = std::string(); + velocity_factor_array.factors.push_back(velocity_factor); + } + return velocity_factor_array; +} + } // namespace motion_planning diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index dfd9a9dec757a..8845bbb52c056 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -28,9 +28,6 @@ import numpy as np import rclpy from rclpy.node import Node -import tf2_ros -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener from tier4_planning_msgs.msg import VelocityLimit parser = argparse.ArgumentParser() @@ -62,9 +59,6 @@ PLOT_TYPE = "VEL" print("plot type = " + PLOT_TYPE) -PATH_ORIGIN_FRAME = "map" -SELF_POSE_FRAME = "base_link" - class TrajectoryVisualizer(Node): def __init__(self): @@ -83,6 +77,7 @@ def __init__(self): # update flag self.update_ex_vel_lim = False self.update_lat_acc_fil = False + self.update_steer_rate_fil = False self.update_traj_raw = False self.update_traj_resample = False self.update_traj_final = False @@ -91,9 +86,6 @@ def __init__(self): self.update_traj_ob_avoid = False self.update_traj_ob_stop = False - self.tf_buffer = Buffer(node=self) - self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=True) - self.self_pose = Pose() self.self_pose_received = False self.localization_vx = 0.0 @@ -102,6 +94,7 @@ def __init__(self): self.trajectory_external_velocity_limited = Trajectory() self.trajectory_lateral_acc_filtered = Trajectory() + self.trajectory_steer_rate_filtered = Trajectory() self.trajectory_raw = Trajectory() self.trajectory_time_resampled = Trajectory() self.trajectory_final = Trajectory() @@ -112,7 +105,7 @@ def __init__(self): self.obstacle_stop_traj = Trajectory() self.plotted = [False] * 9 - self.sub_localization_twist = self.create_subscription( + self.sub_localization_kinematics = self.create_subscription( Odometry, "/localization/kinematic_state", self.CallbackLocalizationTwist, 1 ) self.sub_vehicle_twist = self.create_subscription( @@ -135,6 +128,9 @@ def __init__(self): self.sub4 = message_filters.Subscriber( self, Trajectory, optimizer_debug + "trajectory_time_resampled" ) + self.sub41 = message_filters.Subscriber( + self, Trajectory, optimizer_debug + "trajectory_steering_rate_limited" + ) self.sub5 = message_filters.Subscriber( self, Trajectory, "/planning/scenario_planning/trajectory" ) @@ -152,7 +148,7 @@ def __init__(self): self.sub9 = message_filters.Subscriber(self, Trajectory, lane_driving + "/trajectory") self.ts1 = message_filters.ApproximateTimeSynchronizer( - [self.sub1, self.sub2, self.sub3, self.sub4], 30, 0.5 + [self.sub1, self.sub2, self.sub3, self.sub4, self.sub41], 30, 0.5 ) self.ts1.registerCallback(self.CallbackMotionVelOptTraj) @@ -177,11 +173,10 @@ def __init__(self): return - def test(self): - self.updatePose("map", "base_link") - def CallbackLocalizationTwist(self, cmd): self.localization_vx = cmd.twist.twist.linear.x + self.self_pose = cmd.pose.pose + self.self_pose_received = True def CallbackVehicleTwist(self, cmd): self.vehicle_vx = cmd.longitudinal_velocity @@ -189,12 +184,13 @@ def CallbackVehicleTwist(self, cmd): def CallbackVelocityLimit(self, cmd): self.velocity_limit = cmd.max_velocity - def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4): + def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5): print("CallbackMotionVelOptTraj called") self.CallBackTrajExVelLim(cmd1) self.CallBackTrajLatAccFiltered(cmd2) self.CallBackTrajRaw(cmd3) self.CallBackTrajTimeResampled(cmd4) + self.CallBackTrajSteerRateFiltered(cmd5) def CallBackTrajExVelLim(self, cmd): self.trajectory_external_velocity_limited = cmd @@ -204,6 +200,10 @@ def CallBackTrajLatAccFiltered(self, cmd): self.trajectory_lateral_acc_filtered = cmd self.update_lat_acc_fil = True + def CallBackTrajSteerRateFiltered(self, cmd): + self.trajectory_steer_rate_filtered = cmd + self.update_steer_rate_fil = True + def CallBackTrajRaw(self, cmd): self.trajectory_raw = cmd self.update_traj_raw = True @@ -252,9 +252,12 @@ def setPlotTrajectoryVelocity(self): (self.im6,) = self.ax1.plot( [], [], label="4-2: opt external_velocity_limited", marker="", ls="--" ) - (self.im7,) = self.ax1.plot([], [], label="4-3: opt lat_acc_filtered", marker="", ls="--") - (self.im8,) = self.ax1.plot([], [], label="4-4: opt time_resampled", marker="", ls="--") - (self.im9,) = self.ax1.plot([], [], label="4-5: opt final", marker="", ls="-") + (self.im7,) = self.ax1.plot([], [], label="4-3: opt lat_acc_filtered", marker=".", ls="--") + (self.im71,) = self.ax1.plot( + [], [], label="4-4: opt steer_rate_filtered", marker="", ls="-." + ) + (self.im8,) = self.ax1.plot([], [], label="4-5: opt time_resampled", marker="*", ls="--") + (self.im9,) = self.ax1.plot([], [], label="4-6: opt final", marker="", ls="-") (self.im10,) = self.ax1.plot( [], [], label="localization twist vx", color="r", marker="*", ls=":", markersize=10 ) @@ -277,6 +280,7 @@ def setPlotTrajectoryVelocity(self): self.im5, self.im6, self.im7, + self.im71, self.im8, self.im9, self.im10, @@ -285,7 +289,7 @@ def setPlotTrajectoryVelocity(self): ) def plotTrajectoryVelocity(self, data): - self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) + # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) if self.self_pose_received is False: print("plot start but self pose is not received") return ( @@ -296,6 +300,7 @@ def plotTrajectoryVelocity(self, data): self.im5, self.im6, self.im7, + self.im71, self.im8, self.im9, self.im10, @@ -312,6 +317,7 @@ def plotTrajectoryVelocity(self, data): trajectory_raw = self.trajectory_raw trajectory_external_velocity_limited = self.trajectory_external_velocity_limited trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered + trajectory_steer_rate_filtered = self.trajectory_steer_rate_filtered trajectory_time_resampled = self.trajectory_time_resampled trajectory_final = self.trajectory_final @@ -360,6 +366,12 @@ def plotTrajectoryVelocity(self, data): self.im7.set_data(x, y) self.update_lat_acc_fil = False + if self.update_steer_rate_fil: + x = self.CalcArcLength(trajectory_steer_rate_filtered) + y = self.ToVelList(trajectory_steer_rate_filtered) + self.im71.set_data(x, y) + self.update_steer_rate_fil = False + if self.update_traj_resample: x = self.CalcArcLength(trajectory_time_resampled) y = self.ToVelList(trajectory_time_resampled) @@ -391,6 +403,7 @@ def plotTrajectoryVelocity(self, data): self.im5, self.im6, self.im7, + self.im71, self.im8, self.im9, self.im10, @@ -577,7 +590,7 @@ def setPlotTrajectory(self): def plotTrajectory(self, data): print("plot called") - self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) + # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) # copy trajectory_raw = self.trajectory_raw @@ -668,25 +681,6 @@ def calcSquaredDist2d(self, p1, p2): dy = p1.position.y - p2.position.y return dx * dx + dy * dy - def updatePose(self, from_link, to_link): - try: - tf = self.tf_buffer.lookup_transform(from_link, to_link, rclpy.time.Time()) - self.self_pose.position.x = tf.transform.translation.x - self.self_pose.position.y = tf.transform.translation.y - self.self_pose.position.z = tf.transform.translation.z - self.self_pose.orientation.x = tf.transform.rotation.x - self.self_pose.orientation.y = tf.transform.rotation.y - self.self_pose.orientation.z = tf.transform.rotation.z - self.self_pose.orientation.w = tf.transform.rotation.w - print("updatePose succeeded") - self.self_pose_received = True - return - except tf2_ros.TransformException: - self.get_logger().warn( - "lookup transform failed: from {} to {}".format(from_link, to_link) - ) - return - def closeFigure(self): plt.close(self.fig) diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index f3bce09fa8615..8fe5e350a5599 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -107,7 +107,8 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) filtered_path.points.push_back(pt); } } - filtered_path.drivable_area = input_path.drivable_area; + filtered_path.left_bound = input_path.left_bound; + filtered_path.right_bound = input_path.right_bound; return filtered_path; } diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/collision_free_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/collision_free_optimizer_node.hpp index e76cd4b69365f..55f91bddbdab2 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/collision_free_optimizer_node.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/collision_free_optimizer_node.hpp @@ -20,7 +20,6 @@ #include "motion_utils/motion_utils.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/costmap_generator.hpp" #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "rclcpp/clock.hpp" @@ -67,7 +66,6 @@ class CollisionFreeOptimizerNode : public rclcpp::Node double max_delta_time_sec_for_replan_; // logic - std::unique_ptr costmap_generator_ptr_; std::unique_ptr eb_path_optimizer_ptr_; std::unique_ptr mpt_optimizer_ptr_; diff --git a/planning/static_centerline_optimizer/src/collision_free_optimizer_node.cpp b/planning/static_centerline_optimizer/src/collision_free_optimizer_node.cpp index f9d8439286473..5dd83d6d4b310 100644 --- a/planning/static_centerline_optimizer/src/collision_free_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/collision_free_optimizer_node.cpp @@ -371,8 +371,6 @@ void CollisionFreeOptimizerNode::resetPlanning() { RCLCPP_WARN(get_logger(), "[ObstacleAvoidancePlanner] Reset planning"); - costmap_generator_ptr_ = std::make_unique(); - eb_path_optimizer_ptr_ = std::make_unique( is_showing_debug_info_, traj_param_, eb_param_, vehicle_param_); @@ -391,7 +389,7 @@ void CollisionFreeOptimizerNode::resetPrevOptimization() Trajectory CollisionFreeOptimizerNode::pathCallback(const Path::ConstSharedPtr path_ptr) { - if (path_ptr->points.empty() || path_ptr->drivable_area.data.empty()) { + if (path_ptr->points.empty()) { return Trajectory{}; } @@ -411,8 +409,6 @@ Trajectory CollisionFreeOptimizerNode::pathCallback(const Path::ConstSharedPtr p // cv_maps const auto predicted_objects = PredictedObjects{}.objects; - const CVMaps cv_maps = - costmap_generator_ptr_->getMaps(false, *path_ptr, predicted_objects, traj_param_, debug_data_); const size_t initial_target_index = 3; auto target_pose = resampled_path.points.at(initial_target_index).pose; // TODO(murooka) @@ -424,8 +420,8 @@ Trajectory CollisionFreeOptimizerNode::pathCallback(const Path::ConstSharedPtr p .pose; const auto mpt_trajs = mpt_optimizer_ptr_->getModelPredictiveTrajectory( - false, resampled_traj_points, resampled_path.points, prev_optimal_trajs_ptr_, cv_maps, - target_pose, 0.0, debug_data_); + false, resampled_traj_points, resampled_path.points, resampled_path.left_bound, + resampled_path.right_bound, prev_optimal_trajs_ptr_, target_pose, 0.0, debug_data_); if (!mpt_trajs) { break; } diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index a756309aefd60..df2e01cbcd31c 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -45,7 +45,8 @@ Path convert_to_path(const PathWithLaneId & path_with_lane_id) { Path path; path.header = path_with_lane_id.header; - path.drivable_area = path_with_lane_id.drivable_area; + 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); } diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index bd01977734019..f97d08b113e2a 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -92,11 +92,10 @@ PathWithLaneId get_path_with_lane_id( planner_data->parameters.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold; // generate drivable area and store it in path with lane id - constexpr double drivable_area_resolution = 0.1; constexpr double vehicle_length = 0.0; const auto drivable_lanes = behavior_path_planner::util::generateDrivableLanes(lanelets); - path_with_lane_id.drivable_area = behavior_path_planner::util::generateDrivableArea( - path_with_lane_id, drivable_lanes, drivable_area_resolution, vehicle_length, planner_data); + behavior_path_planner::util::generateDrivableArea( + path_with_lane_id, drivable_lanes, vehicle_length, planner_data); return path_with_lane_id; } diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp index 36e1e22f4aa23..bd0fe48bbda28 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -31,6 +32,8 @@ namespace surround_obstacle_checker { +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using geometry_msgs::msg::PolygonStamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -63,6 +66,7 @@ class SurroundObstacleCheckerDebugNode rclcpp::Publisher::SharedPtr debug_virtual_wall_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr stop_reason_pub_; + rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; @@ -77,6 +81,7 @@ class SurroundObstacleCheckerDebugNode MarkerArray makeVirtualWallMarker(); MarkerArray makeVisualizationMarker(); StopReasonArray makeStopReasonArray(); + VelocityFactorArray makeVelocityFactorArray(); Polygon2d createSelfPolygonWithOffset(const Polygon2d & base_polygon, const double & offset); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 243cf2cf3dfd1..e1d31acbaba1c 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -14,6 +14,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp index 1fe52d4273571..c30f778584fd7 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -51,6 +51,8 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( node.create_publisher("~/virtual_wall", 1); debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); stop_reason_pub_ = node.create_publisher("~/output/stop_reasons", 1); + velocity_factor_pub_ = + node.create_publisher("/planning/velocity_factors/surround_obstacle", 1); vehicle_footprint_pub_ = node.create_publisher("~/debug/footprint", 1); vehicle_footprint_offset_pub_ = node.create_publisher("~/debug/footprint_offset", 1); @@ -116,6 +118,8 @@ void SurroundObstacleCheckerDebugNode::publish() /* publish stop reason for autoware api */ const auto stop_reason_msg = makeStopReasonArray(); stop_reason_pub_->publish(stop_reason_msg); + const auto velocity_factor_msg = makeVelocityFactorArray(); + velocity_factor_pub_->publish(velocity_factor_msg); /* reset variables */ stop_pose_ptr_ = nullptr; @@ -183,6 +187,25 @@ StopReasonArray SurroundObstacleCheckerDebugNode::makeStopReasonArray() return stop_reason_array; } +VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() +{ + VelocityFactorArray velocity_factor_array; + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = clock_->now(); + + if (stop_pose_ptr_) { + using distance_type = VelocityFactor::_distance_type; + VelocityFactor velocity_factor; + velocity_factor.type = VelocityFactor::SURROUNDING_OBSTACLE; + velocity_factor.pose = *stop_pose_ptr_; + velocity_factor.distance = std::numeric_limits::quiet_NaN(); + velocity_factor.status = VelocityFactor::UNKNOWN; + velocity_factor.detail = std::string(); + velocity_factor_array.factors.push_back(velocity_factor); + } + return velocity_factor_array; +} + Polygon2d SurroundObstacleCheckerDebugNode::createSelfPolygonWithOffset( const Polygon2d & base_polygon, const double & offset) { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 503f7964a6413..e15de4152290d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -109,6 +109,14 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( void SimModelDelaySteerAccGeared::updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) { + const auto setStopState = [&]() { + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + }; + using autoware_auto_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || @@ -120,31 +128,15 @@ void SimModelDelaySteerAccGeared::updateStateWithGear( gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) { if (state(IDX::VX) < 0.0) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { if (state(IDX::VX) > 0.0) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } } else if (gear == GearCommand::PARK) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } else { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index 345fec56233a6..f75ceed95f294 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -64,6 +64,14 @@ Eigen::VectorXd SimModelIdealSteerAccGeared::calcModel( void SimModelIdealSteerAccGeared::updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) { + const auto setStopState = [&]() { + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + }; + using autoware_auto_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || @@ -75,31 +83,15 @@ void SimModelIdealSteerAccGeared::updateStateWithGear( gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) { if (state(IDX::VX) < 0.0) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { if (state(IDX::VX) > 0.0) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } } else if (gear == GearCommand::PARK) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } else { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } } diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 12b6eef559fe4..e0656a15b82f3 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -34,4 +34,4 @@ install( DESTINATION lib/${PROJECT_NAME} ) -ament_auto_package(INSTALL_TO_SHARE launch test) +ament_auto_package(INSTALL_TO_SHARE config launch test) diff --git a/system/default_ad_api/README.md b/system/default_ad_api/README.md new file mode 100644 index 0000000000000..0b6cdf30778a2 --- /dev/null +++ b/system/default_ad_api/README.md @@ -0,0 +1,11 @@ +# default_ad_api + +This package is a default implementation AD API. + +- [autoware state (backward compatibility)](document/autoware-state.md) +- [fail-safe](document/fail-safe.md) +- [interface](document/interface.md) +- [localization](document/localization.md) +- [motion](document/motion.md) +- [operation mode](document/operation-mode.md) +- [routing](document/routing.md) diff --git a/system/default_ad_api/config/default_ad_api.param.yaml b/system/default_ad_api/config/default_ad_api.param.yaml new file mode 100644 index 0000000000000..a15abe091764c --- /dev/null +++ b/system/default_ad_api/config/default_ad_api.param.yaml @@ -0,0 +1,8 @@ +/default_ad_api/node/autoware_state: + ros__parameters: + update_rate: 10.0 + +/default_ad_api/node/motion: + ros__parameters: + require_accept_start: false + stop_check_duration: 1.0 diff --git a/system/default_ad_api/document/autoware-state.md b/system/default_ad_api/document/autoware-state.md new file mode 100644 index 0000000000000..e26756de1f4ba --- /dev/null +++ b/system/default_ad_api/document/autoware-state.md @@ -0,0 +1,16 @@ +# Autoware state compatibility + +## Overview + +Since `/autoware/state` was so widely used, default_ad_api creates it from the states of AD API for backwards compatibility. +The diagnostic checks that ad_service_state_monitor used to perform have been replaced by component_state_monitor. +The service `/autoware/shutdown` to change autoware state to finalizing is also supported for compatibility. + +![autoware-state-architecture](images/autoware-state-architecture.drawio.svg) + +## Conversion + +This is the correspondence between AD API states and autoware states. +The launch state is the data that default_ad_api node holds internally. + +![autoware-state-table](images/autoware-state-table.drawio.svg) diff --git a/system/default_ad_api/document/fail-safe.md b/system/default_ad_api/document/fail-safe.md new file mode 100644 index 0000000000000..b9967089c5e3a --- /dev/null +++ b/system/default_ad_api/document/fail-safe.md @@ -0,0 +1,5 @@ +# Fail-safe API + +## Overview + +The fail-safe API simply relays the MRM state. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/fail_safe/) for AD API specifications. diff --git a/system/default_ad_api/document/images/autoware-state-architecture.drawio.svg b/system/default_ad_api/document/images/autoware-state-architecture.drawio.svg new file mode 100644 index 0000000000000..40aebd20ae43f --- /dev/null +++ b/system/default_ad_api/document/images/autoware-state-architecture.drawio.svg @@ -0,0 +1,332 @@ + + + + + + + + + + + +
+
+
+ component state monitor +
+
+
+ + component state monitor + + + + + + + + +
+
+
+ localization +
+ initialization state +
+
+
+ + localization... + + + + + + + + +
+
+
+ routing state +
+
+
+ + routing state + + + + + + + + +
+
+
+ operation mode +
+
+
+ + operation mode + + + + + + + + +
+
+
+ topic check status +
+ for startup +
+
+
+ + topic check status... + + + + + + + + +
+
+
+ topic check status +
+ for autonomous +
+
+
+ + topic check status... + + + + + + + + + + + + +
+
+
+ default_ad_api +
+ (localization, routing, operation mode) +
+
+
+ + default_ad_api... + + + + + + + + +
+
+
+ diagnostics +
+
+
+ + diagnostics + + + + + + + + +
+
+
+ topic state monitor +
+
+
+ + topic state monitor + + + + + + + + +
+
+
+ default_ad_api +
+ (autoware state) +
+
+
+ + default_ad_api... + + + + + + +
+
+
+ autoware +
+ state +
+
+
+ + autoware... + + + + + + + + +
+
+
+ autoware +
+ shutdown +
+
+
+ + autoware... + + + + + + + + +
+
+
+ topics that ad_service_state_monitor was checking before +
+
+
+ + topics that ad_service_state_monitor was checking before + + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/autoware-state-table.drawio.svg b/system/default_ad_api/document/images/autoware-state-table.drawio.svg new file mode 100644 index 0000000000000..ab21c1b865407 --- /dev/null +++ b/system/default_ad_api/document/images/autoware-state-table.drawio.svg @@ -0,0 +1,457 @@ + + + + + + + + +
+
+
+ WaitingForRoute +
+
+
+ + WaitingForRoute + + + + + + +
+
+
+ localization state +
+
+
+ + localization state + + + + + + +
+
+
+ routing state +
+
+
+ + routing state + + + + + + +
+
+
+ operation mode +
+
+
+ + operation mode + + + + + + +
+
+
+ auto mode available +
+
+
+ + auto mode available + + + + + + +
+
+
+ initializing +
+
+
+ + initializing + + + + + + +
+
+
+ running +
+
+
+ + running + + + + + + +
+
+
+ uninitialized +
+
+
+ + uninitialized + + + + + + +
+
+
+ initializing +
+
+
+ + initializing + + + + + + +
+
+
+ initialized +
+
+
+ + initialized + + + + + + +
+
+
+ unset, arrived +
+
+
+ + unset, arrived + + + + + + +
+
+
+ set +
+
+
+ + set + + + + + + +
+
+
+ arrived +
+
+
+ + arrived + + + + + + +
+
+
+ finalizing +
+
+
+ + finalizing + + + + + + +
+
+
+ stop +
+
+
+ + stop + + + + + + +
+
+
+ not stop +
+
+
+ + not stop + + + + + + +
+
+
+ false +
+
+
+ + false + + + + + + +
+
+
+ true +
+
+
+ + true + + + + + + +
+
+
+ Initializing +
+
+
+ + Initializing + + + + + + +
+
+
+ launch state +
+
+
+ + launch state + + + + + + +
+
+
+ Planning +
+
+
+ + Planning + + + + + + +
+
+
+ WaitingForEngage +
+
+
+ + WaitingForEngage + + + + + + +
+
+
+ Driving +
+
+
+ + Driving + + + + + + +
+
+
+ ArrivedGoal +
+
+
+ + ArrivedGoal + + + + + + +
+
+
+ Finalizing +
+
+
+ + Finalizing + + + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/localization.drawio.svg b/system/default_ad_api/document/images/localization.drawio.svg new file mode 100644 index 0000000000000..3c1347e8b4a3c --- /dev/null +++ b/system/default_ad_api/document/images/localization.drawio.svg @@ -0,0 +1,553 @@ + + + + + + + + + + + + + +
+
+
+ Applications (FMS, etc.) +
+
+
+ + Applications (FMS, etc.) + + + + + + + + +
+
+
+ /api/localization/initialize +
+
+
+ + /api/localization/initialize + + + + + + + + +
+
+
+ AD API +
+ (default implementation) +
+
+
+ + AD API... + + + + + + + + + + +
+
+
+ /localization/initialize +
+
+
+ + /localization/initialize + + + + + + +
+
+
+ component_state_monitor +
+
+
+ + component_state_monitor + + + + + + + + +
+
+
+ initialpose3d +
+
+
+ + initialpose3d + + + + + + + + + + + +
+
+
+ simple_planning_simulator +
+
+
+ + simple_planning_simulator + + + + + + +
+
+
+ NDT +
+
+
+ + NDT + + + + + + + + + +
+
+
+ ndt_align_srv +
+
+
+ + ndt_align_srv + + + + + + +
+
+
+ common +
+
+
+ + common + + + + + + +
+
+
+ psim +
+
+
+ + psim + + + + + + +
+
+
+ lsim/real +
+
+
+ + lsim/real + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + + + + + + + +
+
+
+ + initial_pose_adaptor +
+ (Fix z position to fit map) + +
+
+
+ + initial_pose_adaptor... + + + + + + + + +
+
+
+ EKF +
+
+
+ + EKF + + + + + + + + +
+
+
+ gnss pose +
+
+
+ + gnss pose + + + + + + +
+
+
+ Module when using GNSS +
+ (Fix z position to fit map) +
+
+
+ + Module when using GNSS... + + + + + + + + +
+
+
+ automatic_pose_initializer +
+ (Call the API when state is uninitialized) +
+
+
+ + automatic_pose_initializer... + + + + + + + + +
+
+
+ initialpose (from rviz) +
+
+
+ + initialpose (from rviz) + + + + + + + + + + + + +
+
+
+ fit_map_height +
+
+
+ + fit_map_height + + + + + + +
+
+
+ map_height_fitter +
+
+
+ + map_height_fitter + + + + + + +
+
+
+ Module when using NDT +
+
+
+ + Module when using NDT + + + + + + + + +
+
+
+ /localization/initialization_state +
+
+
+ + /localization/initialization_state + + + + + + + + +
+
+
+ sensing twist +
+
+
+ + sensing twist + + + + + + +
+
+
+ pose_initializer +
+
+
+ + pose_initializer + + + + + + +
+
+
+ Module to check vehicle stops +
+
+
+ + Module to check vehicle stops + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/motion-architecture.drawio.svg b/system/default_ad_api/document/images/motion-architecture.drawio.svg new file mode 100644 index 0000000000000..24271b1c65643 --- /dev/null +++ b/system/default_ad_api/document/images/motion-architecture.drawio.svg @@ -0,0 +1,255 @@ + + + + + + + + + + + +
+
+
+ AD API +
+
+
+ + AD API + + + + + + + + +
+
+
+ Applications +
+
+
+ + Applications + + + + + + + + + + +
+
+
+ vehicle_cmd_gate +
+
+
+ + vehicle_cmd_gate + + + + + + + + +
+
+
+ /localization/kinematic_state +
+
+
+ + /localization/kinematic_state + + + + + + + + +
+
+
+ is_paused +
+
+
+ + is_paused + + + + + + + + +
+
+
+ is_start_requested +
+
+
+ + is_start_requested + + + + + + + + +
+
+
+ set_pause +
+
+
+ + set_pause + + + + + + + + +
+
+
+ localization +
+
+
+ + localization + + + + + + + + +
+
+
+ accept_start +
+
+
+ + accept_start + + + + + + + + +
+
+
+ motion_state +
+
+
+ + motion_state + + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/motion.drawio.svg b/system/default_ad_api/document/images/motion-state.drawio.svg similarity index 74% rename from system/default_ad_api/document/motion.drawio.svg rename to system/default_ad_api/document/images/motion-state.drawio.svg index c6e2593ed0eb7..3387b8ed29bb7 100644 --- a/system/default_ad_api/document/motion.drawio.svg +++ b/system/default_ad_api/document/images/motion-state.drawio.svg @@ -3,13 +3,31 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="342px" + width="521px" height="282px" - viewBox="-0.5 -0.5 342 282" - content="<mxfile><diagram id="Cz8Q8T6vesO4MC4sh1-d" name="Page-1">5VpNc5swEP01vmZAAhkf2zRpL5nJ1IeeZZCBqUCMkL/66ytAfIo4tgE7iXPIoGUl0Nu3T9LiGXyM9j85ToIX5hE6A4a3n8EfMwDmAMr/meFQGCwTFAafh15hMmvDMvxHlNFQ1k3okbTlKBijIkzaRpfFMXFFy4Y5Z7u225rR9lMT7BPNsHQx1a1/Qk8EymqiRX3jFwn9QD3aAfPiRoRLZzWTNMAe2zVM8GkGHzljoriK9o+EZtiVuBT9nt+4W70YJ7E4pQNUPVJxKCdHPDlX1WRcBMxnMaZPtfU7Z5vYI9kIhmwFIqLy0pSXqcBcfMsAlgaX4jQN3dL8HNLSrXhk9pw337p8LbbhrvJSFJFj+UR5wQoxyTTCIiL4QbpwQrEIt+3RsYq5X/nVsMgLhcwbKNmfGiVrIEx5V/nC+NBwSFgYi7Qx8mtmkA4qyYGhKK5yfNEh4lnu8qJ4fh2uaiInRVDBssV0o+b7ijepjE43rnXUshjsglCQZYJzdHdSy9qRXMtoPTLKeN4Xrh2XuEUoOftLGndWjm3ZRhXULeGC7I+HVY9X2QG1kTJLLdnVUuQoU9AQIcsYngjAOZoIMYvJEa6r202iGxcTHepEH8rzk+VAI9Nvkm6iMPZHpZNnE8ez+ujkgBVEaBw6QXBDOi0u0NUz2dWW4cu4ZulcQ7fQVNhJfYCOi+o7/oNVFcLJ1sVRtaInfvaVtMLq14qRV57bSEW1Zl9DKtCHWXlsnU3gSmyyNTZl25ixF57b7GOuySZoaIB9yIUHjS9co2zmwfy83XzHf/DCg7Q8eGHbz7n/0tLA0dOg4vzYeTDXcNTzIvY6RPdwGlSQNuA7leI6FI2p2j0zLW3ncVwjZRdoe/GA7EX9N28PWOShGuMI160u11EnMEX2agNdQHtHC1cqWCItecMQXJq78ZPMFF3xatJZBbXJfWXCNPRj2aRknY2QsTx0Mf2mzFHoebku9mVVn/gNSpNu8KzTFgswQpIsjqO+xjS9G9jR9WAvi713KU7mXIoTGkeQTGcyQTJNLUZJVle7R0kyr5kb4B3c70mUzJ4d01TAw+PH0A9T8YA9JY+hX0tGKVmZxnklq47/8JKVXpBZZoiPfoZer0H/GdpDK2RPdHjo2RVNdnaA+r4Iuy5JxASy40o8CL+58MD+j1RNuNFEwmPpHx10Jfqqu6HF4sGeDz+pVatCR1sm2BhZusy4kZfzj1aLtF6E+qIL9ImVvVHyRC+R5sAjHGVTjVdpksOTR6G0+iKf990E5IobJuuuSkxdhdFqQbeXKtmsf15UuNe/0YJP/wE=</diagram></mxfile>" + viewBox="-0.5 -0.5 521 282" + content="<mxfile><diagram id="Cz8Q8T6vesO4MC4sh1-d" name="Page-1">5Vpdc6IwFP01vnaAQMRHa223D906a2f3OUIEZoE4IWq7v36DhM+gxRq0rT44cEkgOffck5sLAzCJXh8oWvlPxMXhwNDc1wG4GxjGSDf4f2p4ywwW1DKDRwM3M+mlYR78w8KYN1sHLk5qDRkhIQtWdaND4hg7rGZDlJJtvdmShPWnrpCHJcPcQaFs/RO4zBdWHY7KCz9w4Pni0bYxzC5EKG8sZpL4yCXbiglMB2BCCWHZUfQ6wWGKXY5L1u9+z9ViYBTHrEsHU4x4g8K1mNycIYbTJjH/G9+lf7NHMVr2lkNAyTp2cXoXbQBut37A8HyFnPTqlvuc23wWhfxM54dLEjPhRe56cLvBlAUcznEYeDE3MpJ2ECPh1/Dr3unoBUicXJhEmNE33kR0ME2BqyCWLU63FS/l0PsVBxmaMCLBDK+4dQkePxD4tWMJdAkl7HLeiFNCmU88EqNwWlpv6zhWMEsYomyckpUbnBAlSeDk5vsgzJvtQYiPgqypI4Yhwo139bBoBTJTOsCDwFIcIhZs6sQ/CSXrK6FkqoZp15UPGL1VGqxIELOkcudZaihprdt1Wo8aQX1Uc36QPb90VzGRTh40JMmYoXXCvbNfI/QOGsG9NSEhobu+YGk72MlcSclfXLmysC3T0g45VZKQvXqhN/SiEIeKYNgtemEqkAvDPhgIMYnxAa6Ly1WiH8SkSnQgE105zzvLgUSmXzhZR0HsKaWTa2HbNdvoZBsLAKEaOgHtgnQafUBXj2RXXYY7cc2UuQY/haaCRugb8LCovtP+ZFUFoLd18RStaPGfdSmtMNu1QvHKcxmpKDLQc0gFvNTKY8lsMi7FJktiU5rGqF54LpPHnJNNQJMA+wwLDzyDcClJ5o3hcdl8o/3JCw+U4uCJbL5m/iWFQcv+v+C86jgYSjjKcRG7DaK7KPELSCvw7QXjXfJWpmq1zDS3HcdxiZRNoK3RDbRG5W9Yv2EWmOIeh5KsJtdhwzFZOEs3+gDtbcldSVqG4hl7eqIxys1N/3EasqZ4VeksnFrlvjAhUekK8ZK1FMCiwHV3utgWVR3Fr3uYNJ1ndlssDAVBIpcba6gvUZhcDezwfLDnhfOrFCd9yMUJqhGkYjFWL0i6LvloldbVrlGS9HPGhvEO7tckSnpLxtQX8ODwNvRSFQ/QUvJQ/rZESclK144rWTXan16ykgsy8xRx5Xvo5dJo30O7cAGtnjYPLVlRb3sHIOdFyHHwivUgOw6HCNOLC4/R/pKqCjfsSXhM+aWDrETfNRsajW6s4ek7tcI5DW3pITEyZZlxIndHtrBYpOUi1DddoDtW9pTEiVwi3QEPUZRONV4kqx08Oy/kVo/t5n01DjljwmReVYmpqTBSLegzSpVcVZq/jH+9PP58kDx1lozotK+prAZww5bPqbSeUqL82dW69PNv1UB2rksrBbJ4Z3IWIOW6wvzleTab3qmlZNcXXWopqfeHJD8tP8TMxKD8mhVM/wM=</diagram></mxfile>" > + + + + +
+
+
+ State in AD API +
+
+
+ + State in AD API + + @@ -265,6 +283,57 @@ + + + + +
+
+
+ STARTING +
+
+
+ + STARTING + + + + + + +
+
+
+ MOVING +
+
+
+ + MOVING + + + + + + +
+
+
+ STOPPED +
+
+
+ + STOPPED + + diff --git a/system/default_ad_api/document/images/operation-mode-architecture.drawio.svg b/system/default_ad_api/document/images/operation-mode-architecture.drawio.svg new file mode 100644 index 0000000000000..4a879e98ed09d --- /dev/null +++ b/system/default_ad_api/document/images/operation-mode-architecture.drawio.svg @@ -0,0 +1,323 @@ + + + + + + + + + +
+
+
+ external_cmd_selector +
+
+
+ + external_cmd_selector + + + + + + + + +
+
+
+ vehicle_cmd_gate +
+
+
+ + vehicle_cmd_gate + + + + + + + + + + + + +
+
+
+ vehicle_interface +
+
+
+ + vehicle_interface + + + + + + + + + + + + + + +
+
+
+ + operation mode +
+ transition manager + +
+
+
+ + operation mode... + + + + + + +
+
+
+ engage, gate_mode +
+
+
+ + engage, gate_mode + + + + + + +
+
+
+ control_mode +
+
+
+ + control_mode + + + + + + +
+
+
+ selector_mode +
+
+
+ + selector_mode + + + + + + +
+
+
+ AD API +
+
+
+ + AD API + + + + + + +
+
+
+ control command +
+ (joystick etc.) +
+
+
+ + control command... + + + + + + +
+
+
+ control command +
+ (remote) +
+
+
+ + control command... + + + + + + +
+
+
+ control command +
+ (autonomous) +
+
+
+ + control command... + + + + + + + + +
+
+
+ + component_state_monitor +
+ +
+
+
+ + component_state_monitor + + + + + + + + +
+
+
+ diagnostics +
+
+
+ + diagnostics + + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ service +
+
+
+ + service + + + + + + +
+
+
+ topic +
+
+
+ + topic + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/operation-mode-state.drawio.svg b/system/default_ad_api/document/images/operation-mode-state.drawio.svg new file mode 100644 index 0000000000000..3a008fd8a64f3 --- /dev/null +++ b/system/default_ad_api/document/images/operation-mode-state.drawio.svg @@ -0,0 +1,418 @@ + + + + + + + + + + + + + +
+
+
+ stop +
+ (autoware control is false) +
+
+
+ + stop... + + + + + + + + + + +
+
+
+ auto +
+ (autoware control is false) +
+
+
+ + auto... + + + + + + + + + + +
+
+
+ auto +
+ (autoware control is true) +
+
+
+ + auto... + + + + + + + + + + +
+
+
+ stop +
+ (autoware control is true) +
+
+
+ + stop... + + + + + + + + + + + + +
+
+
+ auto +
+ (in transition) +
+
+
+ + auto... + + + + + + + + + + + + +
+
+
+ stop +
+ (in transition) +
+
+
+ + stop... + + + + + + + + +
+
+
+ disable autoware control +
+
+
+ + disable autoware control + + + + + + + + +
+
+
+ enable autoware control +
+
+
+ + enable autoware control + + + + + + + + +
+
+
+ change operation mode +
+
+
+ + change operation mode + + + + + + + + +
+
+
+ mode change completed +
+
+
+ + mode change completed + + + + + + + + + + +
+
+
+ remote +
+ (autoware control is false) +
+
+
+ + remote... + + + + + + + + +
+
+
+ local +
+ (autoware control is false) +
+
+
+ + local... + + + + + + + + + + + + +
+
+
+ remote +
+ (in transition) +
+
+
+ + remote... + + + + + + + + + + + + +
+
+
+ local +
+ (in transition) +
+
+
+ + local... + + + + + + + + + + +
+
+
+ remote +
+ (autoware control is true) +
+
+
+ + remote... + + + + + + + + + + +
+
+
+ local +
+ (autoware control is true) +
+
+
+ + local... + + + + + + + +
+
+
+ mode change failed +
+
+
+ + mode change failed + + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/operation-mode-table.drawio.svg b/system/default_ad_api/document/images/operation-mode-table.drawio.svg new file mode 100644 index 0000000000000..19a3ea5bd1133 --- /dev/null +++ b/system/default_ad_api/document/images/operation-mode-table.drawio.svg @@ -0,0 +1,317 @@ + + + + + + + +
+
+
+ driver +
+
+
+ + driver + + + + + + +
+
+
+ autonomous +
+
+
+ + autonomous + + + + + + +
+
+
+ stop +
+
+
+ + stop + + + + + + +
+
+
+ remote +
+
+
+ + remote + + + + + + +
+
+
+ local +
+
+
+ + local + + + + + + +
+
+
+ control_mode +
+
+
+ + control_mode + + + + + + +
+
+
+ manual +
+
+
+ + manual + + + + + + +
+
+
+ autonomous +
+
+
+ + autonomous + + + + + + +
+
+
+ gate_mode +
+
+
+ + gate_mode + + + + + + + +
+
+
+ auto +
+
+
+ + auto + + + + + + +
+
+
+ external +
+
+
+ + external + + + + + + +
+
+
+ engage +
+
+
+ + engage + + + + + + + +
+
+
+ false +
+
+
+ + false + + + + + + +
+
+
+ true +
+
+
+ + true + + + + + + +
+
+
+ selector_mode +
+
+
+ + selector_mode + + + + + + + +
+
+
+ local +
+
+
+ + local + + + + + + +
+
+
+ remote +
+
+
+ + remote + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/images/routing.drawio.svg b/system/default_ad_api/document/images/routing.drawio.svg new file mode 100644 index 0000000000000..6742bfe4c12b0 --- /dev/null +++ b/system/default_ad_api/document/images/routing.drawio.svg @@ -0,0 +1,398 @@ + + + + + + + +
+
+
+ mission_planner +
+
+
+ + mission_planner + + + + + + + + +
+
+
+ planner plugin +
+
+
+ + planner plugin + + + + + + + + + + +
+
+
+ routing_adaptor +
+
+
+ + routing_adaptor + + + + + + +
+
+
+ set route +
+
+
+ + set route + + + + + + +
+
+
+ set route points +
+
+
+ + set route points + + + + + + +
+
+
+ pose topic +
+
+
+ + pose topic + + + + + + +
+
+
+ planning modules +
+
+
+ + planning modules + + + + + + +
+
+
+ lanelet route +
+
+
+ + lanelet route + + + + + + +
+
+
+ set route points +
+
+
+ + set route points + + + + + + + + + + + + +
+
+
+ App +
+
+
+ + App + + + + + + +
+
+
+ RViz +
+
+
+ + RViz + + + + + + + + + + + + +
+
+
+ AD +
+ API +
+
+
+ + AD... + + + + + + + + + + + + + + +
+
+
+ main module +
+
+
+ + main module + + + + + + +
+
+
+ set lanelet route +
+
+
+ + set lanelet route + + + + + + +
+
+
+ + route, route state + +
+
+
+ + route, route state + + + + + + +
+
+
+ set route points +
+
+
+ + set route points + + + + + + +
+
+
+ route state +
+
+
+ + route state + + + + + + +
+
+
+ + clear route + +
+
+
+ + clear route + + + + + + +
+
+
+ pose array +
+
+
+ + pose array + + + + + + +
+
+
+ lanelet route +
+
+
+ + lanelet route + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/document/interface.md b/system/default_ad_api/document/interface.md new file mode 100644 index 0000000000000..f3fc6a389c294 --- /dev/null +++ b/system/default_ad_api/document/interface.md @@ -0,0 +1,5 @@ +# Interface API + +## Overview + +The interface API simply returns a version number. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/interface/) for AD API specifications. diff --git a/system/default_ad_api/document/localization.md b/system/default_ad_api/document/localization.md new file mode 100644 index 0000000000000..866322ed807cf --- /dev/null +++ b/system/default_ad_api/document/localization.md @@ -0,0 +1,7 @@ +# Localization API + +## Overview + +Unify the location initialization method to the service. The topic `/initialpose` from rviz is now only subscribed to by adapter node and converted to API call. This API call is forwarded to the pose initializer node so it can centralize the state of pose initialization. For other nodes that require initialpose, pose initializer node publishes as `/initialpose3d`. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/localization/) for AD API specifications. + +![localization-architecture](images/localization.drawio.svg) diff --git a/system/default_ad_api/document/motion.md b/system/default_ad_api/document/motion.md new file mode 100644 index 0000000000000..fd01a8f56ed7f --- /dev/null +++ b/system/default_ad_api/document/motion.md @@ -0,0 +1,13 @@ +# Motion API + +## Overview + +Provides a hook for when the vehicle starts. It is typically used for announcements that call attention to the surroundings. Add a pause function to the vehicle_cmd_gate, and API will control it based on vehicle stopped and start requested. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/motion/) for AD API specifications. + +![motion-architecture](images/motion-architecture.drawio.svg) + +## States + +The implementation has more detailed state transitions to manage pause state synchronization. The correspondence with the AD API state is as follows. + +![motion-state](images/motion-state.drawio.svg) diff --git a/system/default_ad_api/document/operation-mode.md b/system/default_ad_api/document/operation-mode.md new file mode 100644 index 0000000000000..703f6aa47b50d --- /dev/null +++ b/system/default_ad_api/document/operation-mode.md @@ -0,0 +1,24 @@ +# Operation mode API + +## Overview + +Introduce operation mode. It handles autoware engage, gate_mode, external_cmd_selector and control_mode abstractly. When the mode is changed, it will be in-transition state, and if the transition completion condition to that mode is not satisfied, it will be returned to the previous mode. Also, currently, the condition for mode change is only `WaitingForEngage` in `/autoware/state`, and the engage state is shared between modes. After introducing the operation mode, each mode will have a transition available flag. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/operation_mode/) for AD API specifications. + +![operation-mode-architecture](images/operation-mode-architecture.drawio.svg) + +## States + +The operation mode has the following state transitions. Disabling autoware control and changing operation mode when autoware control is disabled can be done immediately. +Otherwise, enabling autoware control and changing operation mode when autoware control is enabled causes the state will be transition state. +If the mode change completion condition is not satisfied within the timeout in the transition state, it will return to the previous mode. + +![operation-mode-state](images/operation-mode-state.drawio.svg) + +## Compatibility + +Ideally, vehicle_cmd_gate and external_cmd_selector should be merged so that the operation mode can be handled directly. +However, currently the operation mode transition manager performs the following conversions to match the implementation. +The transition manager monitors each topic in the previous interface and synchronizes the operation mode when it changes. +When the operation mode is changed with the new interface, the transition manager disables synchronization and changes the operation mode using the previous interface. + +![operation-mode-table](images/operation-mode-table.drawio.svg) diff --git a/system/default_ad_api/document/routing.md b/system/default_ad_api/document/routing.md new file mode 100644 index 0000000000000..899136f2d9e50 --- /dev/null +++ b/system/default_ad_api/document/routing.md @@ -0,0 +1,7 @@ +# Routing API + +## Overview + +Unify the route setting method to the service. This API supports two waypoint formats, poses and lanelet segments. The goal and checkpoint topics from rviz is only subscribed to by adapter node and converted to API call. This API call is forwarded to the mission planner node so it can centralize the state of routing. For other nodes that require route, mission planner node publishes as `/planning/mission_planning/route`. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/) for AD API specifications. + +![routing-architecture](images/routing.drawio.svg) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index eab13c67ef42c..d3182a9070c2a 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -13,9 +13,14 @@ # limitations under the License. import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode +from launch_ros.parameter_descriptions import ParameterFile +from launch_ros.substitutions import FindPackageShare def create_api_node(node_name, class_name, **kwargs): @@ -24,17 +29,23 @@ def create_api_node(node_name, class_name, **kwargs): name=node_name, package="default_ad_api", plugin="default_ad_api::" + class_name, - **kwargs, + parameters=[ParameterFile(LaunchConfiguration("config"))], ) +def get_default_config(): + path = FindPackageShare("default_ad_api") + path = PathJoinSubstitution([path, "config/default_ad_api.param.yaml"]) + return path + + def generate_launch_description(): components = [ create_api_node("autoware_state", "AutowareStateNode"), create_api_node("fail_safe", "FailSafeNode"), create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), - create_api_node("motion", "MotionNode", parameters=[{"require_accept_start": False}]), + create_api_node("motion", "MotionNode"), create_api_node("operation_mode", "OperationModeNode"), create_api_node("routing", "RoutingNode"), ] @@ -51,4 +62,5 @@ def generate_launch_description(): name="web_server", executable="web_server.py", ) - return launch.LaunchDescription([container, web_server]) + argument = DeclareLaunchArgument("config", default_value=get_default_config()) + return launch.LaunchDescription([argument, container, web_server]) diff --git a/system/default_ad_api/src/compatibility/autoware_state.cpp b/system/default_ad_api/src/compatibility/autoware_state.cpp index 5778edf66559f..1aa49383aa48f 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.cpp +++ b/system/default_ad_api/src/compatibility/autoware_state.cpp @@ -46,8 +46,7 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) adaptor.init_sub(sub_routing_, this, &AutowareStateNode::on_routing); adaptor.init_sub(sub_operation_mode_, this, &AutowareStateNode::on_operation_mode); - // TODO(Takagi, Isamu): remove default value - const auto rate = rclcpp::Rate(declare_parameter("update_rate", 10.0)); + const auto rate = rclcpp::Rate(declare_parameter("update_rate")); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); component_states_.resize(module_names.size()); diff --git a/system/default_ad_api/src/motion.cpp b/system/default_ad_api/src/motion.cpp index d99f72f437685..b02fd43e9be44 100644 --- a/system/default_ad_api/src/motion.cpp +++ b/system/default_ad_api/src/motion.cpp @@ -23,8 +23,8 @@ namespace default_ad_api MotionNode::MotionNode(const rclcpp::NodeOptions & options) : Node("motion", options), vehicle_stop_checker_(this) { - stop_check_duration_ = declare_parameter("stop_check_duration", 1.0); - require_accept_start_ = declare_parameter("require_accept_start", false); + stop_check_duration_ = declare_parameter("stop_check_duration"); + require_accept_start_ = declare_parameter("require_accept_start"); is_calling_set_pause_ = false; const auto adaptor = component_interface_utils::NodeAdaptor(this); diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index de8ca1cca88a2..405359e2020ff 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -224,6 +224,10 @@ void EmergencyHandler::callMrmBehavior( auto request = std::make_shared(); request->operate = true; + if (mrm_behavior == MrmState::NONE) { + RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); + return; + } if (mrm_behavior == MrmState::COMFORTABLE_STOP) { auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); if (result->response.success == true) { @@ -253,6 +257,10 @@ void EmergencyHandler::cancelMrmBehavior( auto request = std::make_shared(); request->operate = false; + if (mrm_behavior == MrmState::NONE) { + // Do nothing + return; + } if (mrm_behavior == MrmState::COMFORTABLE_STOP) { auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); if (result->response.success == true) { diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index 01e973b8ff26a..87cf767accc06 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -20,6 +20,12 @@ contains: [": emergency_stop_operation"] timeout: 1.0 + service_log_checker: + type: diagnostic_aggregator/GenericAnalyzer + path: service_log_checker + contains: ["service_log_checker"] + timeout: 5.0 + resource_monitoring: type: diagnostic_aggregator/AnalyzerGroup path: resource_monitoring diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index b9de5cc4f2e13..71dc2ac600685 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -36,6 +36,7 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default @@ -46,5 +47,6 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index bf40c334f6498..9708456df4fe7 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -36,6 +36,7 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default @@ -46,5 +47,6 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default diff --git a/system/system_monitor/launch/system_monitor.launch.py b/system/system_monitor/launch/system_monitor.launch.py deleted file mode 100644 index 8ec168c5992f6..0000000000000 --- a/system/system_monitor/launch/system_monitor.launch.py +++ /dev/null @@ -1,171 +0,0 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# 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 - -from ament_index_python.packages import get_package_share_directory -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -import yaml - - -def launch_setup(context, *args, **kwargs): - - with open(LaunchConfiguration("cpu_monitor_config_file").perform(context), "r") as f: - cpu_monitor_config = yaml.safe_load(f)["/**"]["ros__parameters"] - cpu_monitor = ComposableNode( - package="system_monitor", - plugin="CPUMonitor", - name="cpu_monitor", - parameters=[ - cpu_monitor_config, - ], - ) - with open(LaunchConfiguration("hdd_monitor_config_file").perform(context), "r") as f: - hdd_monitor_config = yaml.safe_load(f)["/**"]["ros__parameters"] - hdd_monitor = ComposableNode( - package="system_monitor", - plugin="HddMonitor", - name="hdd_monitor", - parameters=[ - hdd_monitor_config, - ], - ) - with open(LaunchConfiguration("mem_monitor_config_file").perform(context), "r") as f: - mem_monitor_config = yaml.safe_load(f)["/**"]["ros__parameters"] - mem_monitor = ComposableNode( - package="system_monitor", - plugin="MemMonitor", - name="mem_monitor", - parameters=[ - mem_monitor_config, - ], - ) - with open(LaunchConfiguration("net_monitor_config_file").perform(context), "r") as f: - net_monitor_config = yaml.safe_load(f)["/**"]["ros__parameters"] - net_monitor = ComposableNode( - package="system_monitor", - plugin="NetMonitor", - name="net_monitor", - parameters=[ - net_monitor_config, - ], - ) - with open(LaunchConfiguration("ntp_monitor_config_file").perform(context), "r") as f: - ntp_monitor_config = yaml.safe_load(f)["/**"]["ros__parameters"] - ntp_monitor = ComposableNode( - package="system_monitor", - plugin="NTPMonitor", - name="ntp_monitor", - parameters=[ - ntp_monitor_config, - ], - ) - with open(LaunchConfiguration("process_monitor_config_file").perform(context), "r") as f: - process_monitor_config = yaml.safe_load(f)["/**"]["ros__parameters"] - process_monitor = ComposableNode( - package="system_monitor", - plugin="ProcessMonitor", - name="process_monitor", - parameters=[ - process_monitor_config, - ], - ) - with open(LaunchConfiguration("gpu_monitor_config_file").perform(context), "r") as f: - gpu_monitor_config = yaml.safe_load(f)["/**"]["ros__parameters"] - gpu_monitor = ComposableNode( - package="system_monitor", - plugin="GPUMonitor", - name="gpu_monitor", - parameters=[ - gpu_monitor_config, - ], - ) - with open(LaunchConfiguration("voltage_monitor_config_file").perform(context), "r") as f: - voltage_monitor_config = yaml.safe_load(f)["/**"]["ros__parameters"] - voltage_monitor = ComposableNode( - package="system_monitor", - plugin="VoltageMonitor", - name="voltage_monitor", - parameters=[ - voltage_monitor_config, - ], - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - name="system_monitor_container", - namespace="system_monitor", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=[ - cpu_monitor, - hdd_monitor, - mem_monitor, - net_monitor, - ntp_monitor, - process_monitor, - gpu_monitor, - voltage_monitor, - ], - output="screen", - ) - return [container] - - -def generate_launch_description(): - system_monitor_path = os.path.join( - get_package_share_directory("tier4_system_launch"), "config", "system_monitor" - ) - return launch.LaunchDescription( - [ - DeclareLaunchArgument( - "cpu_monitor_config_file", - default_value=os.path.join(system_monitor_path, "cpu_monitor.param.yaml"), - ), - DeclareLaunchArgument( - "hdd_monitor_config_file", - default_value=os.path.join(system_monitor_path, "hdd_monitor.param.yaml"), - ), - DeclareLaunchArgument( - "mem_monitor_config_file", - default_value=os.path.join(system_monitor_path, "mem_monitor.param.yaml"), - ), - DeclareLaunchArgument( - "net_monitor_config_file", - default_value=os.path.join(system_monitor_path, "net_monitor.param.yaml"), - ), - DeclareLaunchArgument( - "ntp_monitor_config_file", - default_value=os.path.join(system_monitor_path, "ntp_monitor.param.yaml"), - ), - DeclareLaunchArgument( - "process_monitor_config_file", - default_value=os.path.join(system_monitor_path, "process_monitor.param.yaml"), - ), - DeclareLaunchArgument( - "gpu_monitor_config_file", - default_value=os.path.join(system_monitor_path, "gpu_monitor.param.yaml"), - ), - DeclareLaunchArgument( - "voltage_monitor_config_file", - default_value=os.path.join(system_monitor_path, "voltage_monitor.param.yaml"), - ), - OpaqueFunction(function=launch_setup), - ] - ) diff --git a/system/system_monitor/launch/system_monitor.launch.xml b/system/system_monitor/launch/system_monitor.launch.xml index e7c55c24c4c79..0d8a14981bd8a 100644 --- a/system/system_monitor/launch/system_monitor.launch.xml +++ b/system/system_monitor/launch/system_monitor.launch.xml @@ -1,37 +1,39 @@ - - - - - - - - + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/simulator_test/simulator_compatibility_test/setup.py b/tools/simulator_test/simulator_compatibility_test/setup.py index e5b5f2346c39e..93ce1ab547e7b 100644 --- a/tools/simulator_test/simulator_compatibility_test/setup.py +++ b/tools/simulator_test/simulator_compatibility_test/setup.py @@ -1,5 +1,12 @@ +from warnings import simplefilter + +from pkg_resources import PkgResourcesDeprecationWarning +from setuptools import SetuptoolsDeprecationWarning from setuptools import setup +simplefilter("ignore", category=SetuptoolsDeprecationWarning) +simplefilter("ignore", category=PkgResourcesDeprecationWarning) + package_name = "simulator_compatibility_test" clients = "simulator_compatibility_test/clients/" publishers = "simulator_compatibility_test/publishers/" diff --git a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp index d0732694cba9a..eac6f630b9ecd 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -60,7 +60,7 @@ bool CSVLoader::validateMap(const Map & map, const bool is_col_decent) const auto & vec = map.at(i); const auto & prev_vec = map.at(i - 1); // validate row data - for (size_t j = 1; j < vec.size(); j++) { + for (size_t j = 0; j < vec.size(); j++) { // validate col if (vec.at(j) <= prev_vec.at(j) && is_col_decent) { invalid_index_pair = std::make_pair(i, j);

(b(lWII6B~AIsX+)%AkH2rICBH{-grk zXSA`Qs;X&bGIS<4YcfBp$DU4e9TmrJlac#6A6f#-tf^UHt#@nBvX4B}=COfRa<^p+ zmnsK0If2Ci%Hh20e~PytR|NfyB#zi)(AYs(p&nVjbV(!3geD1bQzmzJ?ZO>`Fh8;R zv-+qvcXNMpu8au9)6)|dLhz^{m$@*A?FJi`;DW_vpn$SSV@G189%;rp-pAK=hX1>l z#aQl~67n$80`ZRP@sS9lW6Tk_VU(#UKm`$2ZN3-fQ}hsyL3_oR;;B6f<4Z?JoOT>I z08*Kl>ZC%%dYBwSXn>M!iHXfUGw;Wb$)$>WFv;Rsemf50_aqCvLnv>FFRI7{UN}w! zd?p}C1J55ZtJQ%Q5$1RINYU#!r^>;S>>#}3!l#AC5Pgzj6xI{n#bfW!fS%x1uFMYk z=dHQ+$5S3Sad5L5)=I%uO*<-S+m9qs5}(xECeBi3rs(mgxzk-4GK656sYh2opS=Cq z@etPuQ*U0mUlbSSfphg1a3-f(p<-ema`gQf15hV$nPSx({(g)=Uo6jx8%91LO&~sD z5uq=}J-~FQBjul-q|Lk{ophKv;Z&20io9fK_~5^zTRHVRS4s&`C#FlBl?bS7Z) zGI+sIB~(F&KA%YxT`T4=+*2OlA~=HJ#h$`8zHouq5Wg;CuN7lK8385H4NIOLYopk1 zvw$)UZ+U zqkj2D#DN=88{Z1Jt!t#dBkfv*HWPO(Ak#eBkga+^qmwy=DSdt6sOOtiMw3WGpa6V+ zP)>ma6+kScv&39VhO^td^#d&#p!t!(1_S``{fIh;$iT7j!E1rL$@xrKyydn3>VV)| z*XOM3j5`FoZm802s*-#E4G*}V5SM6l!XWPzT}Su~xN+gLQ0y{6@#%C(iFRtV?Oh>P zCL!#1LYLwU`?Ml-Ly0FR4wOt%Ol3Fj(i`}k;IJ3Aw6sJ_Z(t(AG>#Y95iTnzNsRrj zlhTMdQ1w7ugN`_=Zn~95koG`bLvx*veX>Imrg27E8O*+Pa@V4e2RbUMIE=ZC@?VcI zJ1+!!sc(eRNHA4EK-cip$%rzWn>TUf(aVE866X!460kW^d_&og>}yVSTv=!<;k-d= z%tJLxRs^Gw5I;~(G_UPQ?F=!*+UI(*(W@wWF$g@jTcrsi2Z#WnT*g9Ez6eXal#RCy z+Z#6-(GX)tU_Swp13tjg12F_vGe}>aTE32s-i6x*Dq{Hdz|iJaMXnl_Ve~&h_<5nR z1B#kIjpGqG&~hG+aGMaE)oY{o2qGMO<)nhz{m$z(cSVE>zyc$Xm=}=70bG%Z{vPGB zs?mCVMHDx{%0!`Bd3WYvoL)Q-5QsPT{u~1MqE6%RV5h<42{9FjRyV``DUcO@qcHjk zP!cB&B{Kvh1Shhv>LMtA8^b*fdwq&^*P20+@Ho%NLd`whv2$-Xa zsXoI`#bM=UWrvmv+XP4q5V5$v9tB`V=5A=;z-)$h!PLpdexwO7-u+YZHdw6Q>J^XN z50KSyQvu4OhKOj@yJ!TJf4uaZX0Z{+sk7&vAa!0JiIaql1W7HNP~o?*)MV9>zV7bU zy!HCH0`ZeFTeCk|)Z)g0`@9UjKG~8FIgI9u#a~)O_UzqzP@3Ddsm=??A5jy&+z`?q z%gg~kW6NwUYLEko>{DjKj#h*pY(E&EX6jKHF3yeg;r1kA{EFb}0e)?NFH;`53O=PL zYv`f_clOr$)dMKQ$wD{wr)W&%GF%-jt}I$bDq z2)yxXXY1pL$-t{1!&PwkP(f+)8S?eI|Ko~!OeK=d+1?(=_4vDrtHId@c@pa(;#afh z`u=6^MJ|!LyVw`aZ@!)~^dv#BlP#GDC zy+R{K4_QylV2t8(WKL|bPZv6A`ph()m>}S~Me(368N;p++&pm|z+RN(!$0;Bsc8&l z0@DRT5cEmfV%uAzgtQzN)+A$Qx)G&!A~qpf+^pSWyEs;4j8y25`xe7azxrvA1B69e8`Tm0W)YO6cw<24ME8w^F@!jr|GfbP|DBN%<5J{-V-&Wpm?A`M zq7f7UV9;d%n45>x8p%Pxt5?pSJNdV64Go=*YEK0Z<3>E{!`mKkC1f@Z^ZL;3>%~}b zJS9|d?11*BH{8QiAo~zcIult88iKPvN(h4+mdu;Iqmf7VISOr++?YYDb+B;;?7-CXe^$vf)7B)x*#M*8uSKNijEM5UhOiEdR_o=n@3g zQLqsh57s1rAGqvrSUQTs4uO_gT3){RAGj9$AB}b+mo7xPHAeATAr2iI0S}-|+lfT^ z=_8Hisn zQln2|7^d0_+*A{HN~8+Zymxx6@0fG2zr>x()M?u1#1ISv$yn%oEn zyjjT!C;E=q*pk$j&gB#TEpam=@;Y;$!9Kk^fJ9QtBZZEpz zID%g+g0SYW98+9<9kO!xQA|I;_CvRw2A^RZPJk++eov=U^5IX6k1pfwcZ`3-#*GFr zn9$wjiHt2swZU!11>xf-T>ns{!=?ju8!v_U?Q~d9-@`X7d3&@PmpN4PcvE=pF;e3Y zcvijVb9{-(7gPsGZUhYiX%^BHW@OWCd*7&wka{gyHewKtk_?$nuce*hXcx>y1cd`THTv3_^3Z~j1p_IX8$|FMzHg8E5Yu*43;+j-`wDbGcs|HEk-0uW z4GVA>NEJney#daX`uwQn_W*QZL!)dE@5HzU6F-zM)jQc0k<9kKo8vYu#|*-3cJ-gYoQc?2pEN7)No-& zOmw_zYHAUS^N)Jo0p_ZFX|MqYyq~xp5R;oqJ*USm%u%`0C(3v;d*_aY!$JH^6b4Y7 zL@9-*ytwy^9bP6t?fWytOyK5{Gxu^0UYGmSTD>ZoftB$rE&vFm(AhB|jDIZ&aI~}e zaX6Kv1OX2j9ewbGtgK?$O_CLS^=9R^36m-C z0b$CEl0DE)AilVGE)SO7g7O$ikATg&57^vC!rMAx`^Cw!-=bD3R@40GvFX>%7XW4* zGaELtZ1ln4#w$n9IWRXMYzHe|w4l)A2WM*1;{{WThKuNpjL)g(je3PRPHmpQdUepd z57&I_53MR+@ss`Sa^>bH2i|QOuV7Ugt7w+-oLQ!?wLXrzX7F*Yck!dTvTfC2tdixM z(lIt-^Q|SzE(zb7ceU`dE^Tz8MXvL*@i~pK70L!q@@sy|Iutp*5MBMDKwo}511?9c zd^7&~(}OQb@`-^&s}~8qA3QY$F26DFy=3j3zWmUEv@oZmt&PH5eEG)@L`lMx5a}VY z0fFNXJmaf>43%kj-kv~i{D;9biQ;P(-0iWtMN3RvJeTBcB*{ytz$sgq9|tosHD;;D z>VvY2*E+Aew zUY_wnY(vhld^I5-PCkP*1daHK{x0s=&6}m;YyN zn1_M%T4z{5BaepayNP7QYBR%Xu>DZi%VZ5-aKS9tlfQc8%2ZBbMOBhqoX_fm_pgMW z@gf0`y2ovX;YMU+4oIg_Tz}zKc6W8l_HBH2E9PE$LWP5U&7EE9%6lhmxUMs%|BfWx%Sh=&2JvX;3w znd%4P%iH)l8xJnSgYLfXnngpE8*9K7-R-R+_VM{I9Y5USvcCsb-dzBwf}wXPbl{F; z<#QQcgO4)jDnv%b9N>8)#MQ;AC%O*aGRRNeH#gyWM5I_l-J|a^!iia3Z|bjg`qd&g z74bu{oYY8A4RM_w3N7m%!CN;*Bb2c4%%z6BhI({?E-=Z(7k9^8CSej)T~qTbG7sj0 ze?}b9o@{>>CdsvmGC7#$b7xSr@xh!mX=W@@WrFY z+%3t9-e3O>kO;H=+9MREgv+nqeE0IBkrq~zpsUcC@zAqE9IP&rqMEKSu)@0eC`|hk z_h)FL6bhJE8IE$qI}e#wti`7H9JqIgweq!OGPnYz#mh!UWE>lYB4OPl%Z~`WE-13Y z03I^q)t7uLANh&Ypow~58#+<&0Mmlne!CQ9zMp~$%0B|C&0O-rfIjC2ik&=>P7K%j675-o^an+;u z;NANA`6UO3jRa4k{s_h@+@*n6U@wDeL8A71O;OyUkT)QR0ms6osuOfMOo7SxHFpoMY%B0xk;30Ri(>%p!45 z%a1iJ1oDB);y0@$Ydl+}iBI7$bIQ39W_m!%{)`E9e>0{=Yq9)DlmQzI1;6TI8PE?1 zKl2gJl>F>lS8n-cebD0CwO>a=W{(OjR*Z!dpq-p#P-X+U4F>@QKKSZjHtC2X!W7a* z{7mkRaF`fJvly+|)YMexOFzX}hMr`?7m2#(gs?1awil`}mdz$@)v}1h&?!b^C+gz} zv=L=tB__pm#)H-k`EkI)u*08iLBQ(Hz+j7I8N$|S4$SrQQ&13Nx0puEX(rC1s2#z^ zn+&+@$G`K82Y-pNV9qZb8XPlu4K^%jYqk$NMdpi&_~Dx((!griV1q9|%+N06;BeUg zf_E=25ozz^z8+WzEQ_`o?geBQxmMh8g&J^CU`tG_z^-&IZRbCZVyU`_pYxjz#5O0! zE?hf+)+lHsSoXtQc4E@u3cnLZw4uVn!d~t5ixeA} zPB(6>t-Lw(i!SY*YDeeA%mNf_>BK$GQ|>H0bpqqclSJ7L~&Q4v?OV>~V47p;`^b zBm;9aIML-?JMD;TOZ-FyA6#Ghf_9ZMEXnFE;=f9tN zF3kLb{}@{R2hBj#L`cIWL$1fGi7re}KJ~XH7jdHjilEnf!k1s@JuKRhM*`l%)UDL% z+}tG?#zGvj=ZduN{+J_sO9dO?_T^Br zB66YPE%jpOXtw<6xXS%yt#6C9Ba&@g;;d!(S)wI3y}U4#(i+%Z+O_A*p?sOjD|U`1 z4)#+;E*yX7QA?fp%>L+pY0c=`crUqEp3*R+ZO5bB{nSk9W3C+CCHkhlOOon)Dqj|@ zUtE3s{S!TnQSsVTy;VEux}h;Od#>5b9ipB)wDim&s`|K_ZS;A=$}2RJEVE7WeHULj zHc565r?63y>bvcDorWEK?j1A`w(e_oigOw+b{v|y?ROz-!QG;lJI(&kv_B4aMjZ7H zmQypMPLIcy7stl;ere@8=V~Ft_(=_z2^ehcb9Oy(`%3=Hdt?CydY(1u;2qf?)~Y%n z?=NA{t*6)>vZXt^H+Rd&jeLS$(zi7az4EuPdh@{W*xF+WCz4I$Up;AD8)YY3l+-;r zaIv6oDpA8k;`m;HCZkT8qK}LGHkx1aDXq?0yJ^`sZKq3jCQ=6#zbmkA8)!F_Wt=@W z>D@D_#!Qv_yz~NNXfz|__lUfuf@iMd?-{m`woyTsYu7C@cZl(M3_fyREhd%FxTd1h zr#iLX`T9k7$Hx}8qrN76J;POdrs~BB>K~gRu9}mM;)Ze5I~FT+-uv|01Q|WCH2k~qx2^8aEuld_EFdf!={i{otTkwCl06~kX+YAj$4Fgv1Klz$ZPUYBJ+d2i4 zIs+f4CnHNQBwJ6lrgcz9>d4RAoik?Ta>~KK*R!53#>d*j7!xyODEJtzTKrc_Gun2^ z;M?p>L!af-7WWp6hV{#_O?&3re<}BFYG_axwi|RQOkVii<)WCgOA^5@QK|GO2{iHfh}ytiy%erLRH{%g>n&fAf)v|67WkrorB2fu2sJhrQvg5gHwu4)_wQ5CWskBr|Qcj|3Q(qNV zSb|_Y`|`jYx+;nxAyP0@6Q|*_b$~|Ej-7I$pv@Tk{SIn?zt`aD)EB$LvEwwM| zH|*#33(+)eoIY9A$qGmF-E#4dl3yJ1Lrl_UH(-`%#^5ICav?!l#a z{nbGgoCYcfiac^oUrG|>nrLpi8=D$nXvbjj!a?iFi?vm`=f6ulA)|7_D*uj4oS$Q* zbl$0VVjW{%+4J(otXF9~=5=EvekvLA&aBu&GyPNLbRm+9NA|{K&;GV&7mV5Mcun3E zHpkQDes+wFIMN1Q+wxeq)szO0rYBi3gIfQ_Rwh*rrgZLg?m61sj(l)8A5H3ZnuSqw zMp5p~M#)~2g0#s-Umk-FBhHT8)DwrMN}edJY_0EU(0DHU;3aO@b+}<$ z7C-g;YO2W{p7|_SdgWWsoHnslL7quVpB7P99qj&mNvoa8vb9vC-9}f!zsj;njPskP zscP(chdZL*-u9Qqtk;ooykVRv&G;+0LQC!R#m>WqECO4_Z-+I=DQZn@uyIsKaC59{ z?%l>7^gjQE(Ls$w>S+D5cL77J`mO!8afTh0uLnEBk3{=C?Y0(`w4F*BkO@t$*}uf^ z+S&AEtf{zu^T=o2#`qgNo&)cq#~lvz4KX^`eY@JT=)05Zje-#Njop)XZpYTzndb`} z;q6g4U~oN&RsDce=Do|qToPBQu5T*?E&0T|z8Evw3R(rNcn|Dq4Q2kBP7kde>ooEX zVoA0rGwkU5(J_AUgNx4Khs+;$?WGo(-Y)r?!qAkN(RVAJxZ_=&DZi4+BN%B*6Df}` zt{V=p^Q`&le}Yl6xxsqF=VXcKho*+&>K63(e)MaUm|rwM)wmrV_6|I0Ei%V_C&Lqq zEDzovSi4H`-JhcZ_I;kkKAei$JoP>t1|Q09u0Fgj^BCLc1C>8@KAZ*ztOx%U!4hXBuKYix6lp=HJXRu}VW~;yF^gR)eEGyuxeX()h;H%#z{{&yi%Igd9A57Fl0JxOl`tDwbTnYa> zMsD}LoMCgVXQ~vrWex1Bj%Qd;7CD@Y8$a*sT;Km{M|$OB?@g5G<|c&_5sU7Zs(%Zk zI|r{nvt2ph9Lr(vy}V5BXlr%2wp=<*JD(+Hi2E`9LxyidvSjp?=4vhlnTldfnnp^} zck6`bmzVFkcVea2^E1Z2ZNs)4tQQ9AyGKy47-qWUh`p=qUURScS&Gya@59eEJszm8 zKIC8J@jz*{jz_H0=iY>Ko~r)QD=#TMsS1B{Ahuv<(*3xpwGV~r8+_W8Ia|h?wI3|^ zImgv@i8nIrX43oE;&Ewq7D3Gy-!&dA*F5LD>sdpPyr)LmE6Z)!R`+~@nlz7d6$?mr z?dMiD)OoNx-tVMO@)hB8F&XcpY5K0t$G+T)&U>}(glNmKeTUA6 zbq|sXbY*KYjXOg%LN7`>s}6 z10)USVGbpes0VNEZ(bJCCb{D2yU@vrRn`ev{?8jKy~{4jp5zm>w{bBlr%O2=iJt2C_$2a>%n*4oL z%L!Ni$0c2qnnlB1R&O*TIyct2+hhh*{xzN9*V8-w#$@F8Daw$2q}9q@&z+TidiuWH z<#cLr=P$~eGq>JE3NgKIoPKY}X{2^`G|D-{Uf$?jBaCxVg?)dpn;1PpzJKDzGeUy})pp#nh=XpMvSB zVXG(d>(q|y_R#xKZDM2=C(@sL(^rUjYE*UInVtV+pI3Kg_pdzh%cFC#())|uCz(})2Gl?+u3B#H_HUNN0x5ZZce}RCGCm)E(Z-> zU7ttC>GAzFYV^pXWsM3()2#XWMhKg;86|CP}GWe)iGfABV%fvipg|;`7O#qCH>Qb z2_Fya77zJ{yKIgrNqzvqQU}RUfKigtOd{b!%%VSwGEB(q6g~@U!^yD^%%2p z^-5%%T-alLzO2We2`?hgwk_vY+=)5MDQ9%>s*mjTT`3*x`)GUg4TMv?-dNVSJRjUF z6NTi1Det{0c@QY(ItNfEvwM-h8t1NYQ zefQ_?+wyJ$7Db`VDsip5Ga8$gY?-*Tu>NP<@~?tkUU4zP>qS2;R!Y3+7|eG5&G{8x zUTkMM1MZK0(er9bFS%{|+B9P4VJV&Eg}cm?*aLa0diWF4zEtl~Oip0m7^AHsxV2wp zLvMIy*uxM%+L+GZPWDp~wJWMyC90%q3-nv4YBje1ozJu69~?P;aYke`*K9{$fqSu#kOQeEYf!LhIv-wa*UwX^qBRuXR56wke}dKYMPw&+hrwRVy}8RlOX? zR*cvxtjxVx^J{TUCcpl*5nr1}T8*og$YsgQH@D*(ST5HZ$F_{pz{OfX_3S9$s`BFb z^4uK)+#=BTa8g_RiCmQ%#RB9VwK5Tx95WLaoqt zeNuX$PPf=HD%G>?tkGE|z1_Js=l19%`hOG-kM!#Dx)dGGo0|CL%vJvL@+*#3{JosH zim@q>Y5F0v=J)5swNgovjO63_OnvU{H0QsZ+r>_E4flRr_uN6(U3m$ z`5oQg`f#H9(f{^JmplD4ll{O#ey*Cr7L4uW6RLl`z`-eDFUR*_2| zdxuxMP|{ABC#ZU^GIS0d^j%tZj{S++d5$4=uhhW2$#=`YuNN4l34|W0?V(oueOK=p zlf1TXUvr;EPG!(?Mqr@Rs=}#zrmiy?A}Rh&X_rN!&6LF1yAL1OW|sKw!#d7H_izmf z7n|AG_O11OqJXy{8pxz}52rv{EEW1wYig_7K5onFukc;4f050CKc91>M$Ff$RcR^8pS^o`d7ind zm3gU0%Mkmzh|Yoxx1+uAypGw_KN95;X=g~u2)*7*aor`7WTvF)f9rCr;AjFzBv zTemkB_-hXre0ylrxTs#-GjPwjPtmitZ3st{q``(8XamiPjhr#M(#bm*Zo820G|6Bl z4aoFKi{^SeEk6n?IffGNbl%NeUFQBisQB-WGOzbB)qPjB<;qwckeH9$*&;Qb;Os%R&>aNxQ(25 z%OlKTQpCsLD+--BsxYgJr{-{8OTd{?Nh5u$ODmQeWRq#39c`W@xv4h%s&oAN zY7?q?qH5mg-_g?-_It#(?6c#{N|i~Q-b8IqR?DG_2A!HT*ssHr7jx}vjOA5V7v+*Z z1?R@V+(4mho4jGSfREY*$kTTt~ zFKqBgC)-y2m*=u*7LnFkM~JQ!OVManJW#=Lp}aI(!~B{s*&BDBeB8?&>fXNbq2V*JeWq6CIqGK0%r$dIRurw- zOIUu;@&?g$LP85*iUr4%S9r0xA!UOPK5vvHI>vl6V^rtS>MHqBb`nv*&HJu6>d3;ua*M*uCA#S^ue+!Ym4E7}nK~-d z`B*YAJz4kNlreWgB_d-9E!~*47=nilMtFf}%5O7>JF}uTiD)$tXyIl9f+Kq8 zpFe-@Plojwd81@0aM;n+x4Q6B$ZK|&f-pG_`2bO6jE^Q~fxtjI+Hkj|Oi!3$1j-K@ z>}a|P1%^-`2#JZs-Q3*q@px{x0%v^X?`>m2!N3p6~ z)N(=?lR{=1X_U)Ght{lnky0ztUw*a1k=djzmp3!LyWeNM#7wT+M``)}@*cdKoL{;W zMp_5P4_&ua9H@~!T)%Ud{`4wDtygj~im}VWyC2Sh47G+LDSLg_@$GIy``mKTT@UGe zY#xLo!EmKB>=Ew3%%WAQoji&8Jq2hfg5eTjqW7Ep4n8619YZjS&xHmkwl7S9AwA3( z_`V4(^+xCyAf7=-8cb0PTm%OtKlf(kO)$gI=U=O|=I+7ywMnSqkj1Oeu{6+H2f7?s z8TbL?05I#f8x`k#&4Xn z>J91m>yknVHyPpOzUP){G@vl8A2>6frSW{Ke*aBg=g<(-!;{ABPFrhfhna^?!L{JJ=6Uo?$%rR)r3{I*=fQ zys){@tp$=oQDTuIB&m>xzXFLf?0L?REPAXUhLZntBlT72A>q5-Y1pl}wMfI9>5@=^ z-{G$+uKq%yfb+50Yio|!Wl$+kM%-zY1`7p4&L246IN8X+65`4*y}Y+TFXrX>;L7fs zxKJR{+%&!!gW;dqrUxrTXhcG!@?y3 zs4Qgav*p)31O365AD7A)LV%+;H(T>Nks19iP2}#{;y3>t!Ih8pjkGuGy zmDL`$uSRPf{tdc1A@|x%3*AOk#h-S}j9ZWO?<-zz>)Eugg36bh+NeIMHnXpwys7tD z|ERG2kn~v5*!tYu-M;OM<7bbGw1|wplSX8DM){AnwzeNq`EeK)0N$49ju;Mf&1;iR zpLp!DGw5!i6?dmWN<|=HY%Gcz`9ZPbW?!VpD`6HzMDH+3hKntPvb`DGS3EgyYY9Sd zm(3CHfRU|@-MT1b-rHiu?V}shDvf}TON`u*yLNf2Tbo4@vIa^ugye$_F{3hj^?OLK zka>83x9$6ZrJu%@u>1Nvdl_DbJE_@v%#)U#)bx!bV{hbs@fdp}4RHgulVX#cXJ_BW z7VlsEW~Xre);$Cr;R0u)*25>cl&`$T4%>84Be^it=mPV_ZifC+#t=_@g=wEUK^@?(rXYH`3bHc~vUN;ikj7Htc_* zLjcQ~Op&Bhn0?3mnAhn8C*HevI+S+I1$yp}Q-f{H+UpkOE%{E%^?Bb=_x}MC zXX}B0N5OS_PZlhXjuD>j+Ink2xc`pKJl*Xr$w?L`3v*j8=L`-vH>%7fc~+X$E8FjI zTx9Y2`6=9vQq|z_D!3MPE)apB6~oOdk5iR`0aF=uAXE(c; zN*D!-LyQLA-aNQCfUJdMH}qn=Hp2N@{7q;?eEFM6P{(=#Z9sJgy9MIGP<~#24@f=eEH#~26E$R4k#LV9hJAtg<@$DK+;;T-TU(EEY!!} zoojK`87LP?U}{m!dz~HyzX+F@naaB3yKnktZ&j1iZiWnJpIy@sb6)j%QAr%dOFSn} z?&vTRQ!w3g{!Xfhz2|{Da}nODcH_w_F7cjBy(@7c_25EQyDnYsVwT9IZ3fOwidE~9 zP`By*_-LRo5e$m~fiQd{(_n#&CR4P0uIIooUYFgU2|Xz|bSisq(7>gahiDMT0wENV3Au1rrbtge8V+igvKy zmRFI?nERAWElblrNzO=BfQHZJ{B;c;GC{>Yx=jV!Lf`h7Q5dX~1GUVoa5vG|;i>Qz zS9bZ-r%}fTE!ekfelRsI-Rx{|aL>Mv{=0HjIGvY&p9667SMte|NipWxXKMyOqbgD1!};P{a7Enu>4%)7LP zYK1wkZ5Z`{_}tbBK?)EHx%^^NQ-1_ci!L=i#TVV&EoJ+VrOr6dDX=0UYI>b z8K>_54xcxV!}R!JMRjNA#w%bzgO<|&Q!w-qkNE!mKXK3D5`&l@fDEd62uZRUBTfnR ztZ&3Rfj~XfQ~~r-6)(Lk>$% zUw+u zkzRaW{OsM2UdwZ@hE0fF7>(?iA&W0EewLI=3SDbWv&w8j^`uJCa=)2Om&l!uCf5i=nTZAB|gJJXjL2N@|0r$_OZuO!U zIr2jKvdN(gr6*NZLrU|~Zp)Wu+W7~Cg3C1o7%TAl_AT5CWiLP9WtcUp7X<0B%Jjw$i}>5Z#Q z`lOmB(((!uJsu_>x)!xv;!evpz3JwWgL%J}eD0ZQxEB`_5bZxIEY9iwN$Be04=*^% z-NUtSU0oC2c9TNF#W!nVv+6yN&NAs)Zhxlo+XpGNFX0rft<9zNl7IOLVFde_tZ7HR z<-GT{fJ}y04{0}ucrg>0#64>e0NDZGU#^|`m^yj`9J5JXEm}W>MG%`}1Hm)#QGy*< zaNt^Ezu@An3o8KdiWYt68UxX$}U*_W2ru%XaF z?G8MDg;OxR`M2n+;QXrzd{djen6_1V{b>{4m#*722&y-ZOk}+uq-{&#qJB;AB_&z*)E`r2Ner zjo-h$#I&AQ?3hb0-KD+318+i_WRo7w_Ih@-6ZR-@6_%6my>ab^bjqEX>A_e`5DLMV z2M(b#Ns@Q|JYm^l@)K@-7@zzDYNKBL?6;+TVb&J^$+10O(qj^M!DLiIhes(s(vey8>FvU|^ZSb`9cacMf&+xITMp;MrnOWp5~YcBm<8q?kG~+S6tmu{s}zGYJ>NL> zJkT!-yF;J4oJ}9UYhHL!fF^^d{g{#(u2nCkTDq^sYh<1I&5%NcF9yN?lq_%3Gt4sM zF&{D?ivL4BHF(c5&^KnV{YH}RDRpPnl3ew->o1>CBMWmsVyCwuHYZs!{pognO)af0 z-iETYT!u@7ee0^(q2|P<33$p#%g2U-A`_l}44Ek*ryuvdSStlMLQ*L*WY4)Pmag-K+l8p?M)zY<_;(ipGu6(w>6n@?18sapKY)s#W9(x@E{7n5_1QTR2PK z^4XGRuaRDl5sNC5T;okrGG0+dWft#mXexdl^cw7adi-EHZ+(gS?TQzRo$o{+|O+aF|gO|JVghJ3PSg_~tZbe1si5f>Lg%Yx~@n4`t5^Id(g^Brd zk)EzDQ_$%>D*}-%K3dcmaWc3CMIJ!uOSPlhLT9O&ElST5S9UE5m%PEN!cg(A?+ZF% z7S9qapD-ct`oaZPrDzqstcq^?VsYLmn^2w%9 zQSSxsh}rqX)KrJp;@7TSBXj6cpGBn#ljXU`^OnS#7E#J2#UD!@_=0Am?ZwE2k0tmm zZ8=NCe`~j!3Ofj}>`oEbu-m?a(x$_4L1-Lp#saDJD?(}{DNX)A6=$)`>@s=S`>Igy zCcHVY!K0#H;%ncMK#g_A49U#m;q1P5TWr&3oR=2+oPGZ~KqycwT~8`1pqn%A@^SIg z?vBd^l#jk-251P%5tx=-+m3Q1j8Oy~ix!@D)_HN4&P<73$+C9hElbt~W%2WOQ~$bl zO4O3gAGD;UUh!DNpTLm)q)8g?D|AvpmjBsqpgY0I~EB0YZl|S=g1% zW?MX-8t5r#W!3mN_2Ky$hkHE^yvKdHkJnNTEyj+RPqjMc+xBBuzk#2NYPF!{Yxytf z+*$1YPn_cy7Dm58r`N7bUM_50;v>7zM3;WswST5xP8gN`4bW5ip6wW{aICu0!R&?T z2F8W{V4MA`Dj$X@WtVrvF4g1u%G|}l9p7O0yxLg#!>+lN!2x}#rf|YKQYh-aH-{pZ z#L}k%EL-mXtQ^Vp*eqq}z9DCWd$Gsw)8-5Mu5lSN4p~oTuCk10yPmlFe6*BkkYKFF zE#H@?201w!3&zinN}_|YQgigD@`;e}wgCCI-2WBT3M%!zX-17--;c7dT0kc>pHQ^}MXzuEVYs=Te%&G)6DT@; z@_#g5cJ03TO3K0hu{-$B%XhR1#nbRSd+etb=UKR3bTz(fne7gEuEzH)eA|roY)#Qq zc#augkxF$Tgb+dqA%qY@JO=&9R7-c$>gitkxEGr0>0V^=qkgFA28wP-Iu7~)bVJgR z+l0~&o*Qr6_6G&jl3ne`vrVhulBX$qP%4;;>#7eEB}J{Esg})sEuFV-d~1>Y`e#4? zwAO5IZ_{ix(OTpC{<9T)+qO-qRHD=A&~CR;O5r%pp(nX~5xIctK+OS)quYM>mHqea zh8CUBG}7;e7M;KZ!P`A$n(C)j)}6qn9Vk_7hdk1se0 zZWgFJ;MobD1CFh6ZH;T2u-vsYt_`lO@f>CDZL`j?HLjz~{m?iwhWt3Lt!1)OA%qY@ z2qA{9C}4hn`Kv1bfMxu!~}y8v|5k< z`8>;%QYfX^-rlC&ZsYqt#bPm^to@0e`=FCs#&Jv-hO}BO8jS`~6amoh_X)%BiC(1G zElg~>kxe&p=tVaD*ddH8`mse2I|PZM7dZs6O+U7X5}RIR5hm%_*rFdF_;U~OjBu?O z$4am*jiX{5OXFAxu9aY^1lLY*ERAc$CQP?t6QElOjuqqC8plqstpv|9_pZvu#JDyT zZ29{vgb+dqA%qa(=?W8@D6xpOBG#tuMwBRGZ4qgUI9pCE60L}1^EgZ(*0ve16;Wan zCWxQ?L_!JEG`Dcvgb%#1xzu z&o=kI9pgI@1v|mD;-~#oUMjBZ;`@GHL_Ik?KTW!sN++A_iH=ZUS^C4ct9D!QL}q2TPey-Xg)V%ww|-?DVmnKDIDpZIxgt z()Ai!>HM*0%P%7#gb+dqA%r*z7rN>MX zi&!%lGZbFO3F+9QdCx1JyrL80*=hJ}$GCQkXD9epgzv=oc9MtFu9Hx(qe0eS>ok*) z0=*Z7=Xn$g1&YNYg+k#-&UxU9?@S!WL{UVNBxtSkdm%FIz@isw`jJI1f*}1WPu6d$ z7eWwQbOQ4=Ak&LVo8Ud|X_c)HLg+>EX{#`OvusOaTi{rv%cdPn7MQ!vV4L|RsnV7) zGIjw#UVx8_gbewEVqSCRLis$rkIJ_+}Z9Bxv&CT1klHYr_ z#&gWA;F>wo3NDmA@SKE_3%;9B^c23UDY=@0t0_28Ey(+3F`$&fvaI};6;~gHL-)Ek z%8L`vr|M%rVnP^avl8n(9PfwP1nQBdAE-2JH;;RPq8n=Z5ozFVW-RXK6Gd=<*@(rn zB#0F}V=u^xZrZ2a(rJ%(6OLC4L*coju+z{y3+W3kSW1nIajZe#_iVglX&hUbKwk~c z<=7_RH$C9Z3lkxP5JCtc#CSoh2qOq%O{^izLgZKzCnj8uvH&@OIM#!pIo5e_-0bMI zWrhi;!${{La~MOMnD?DgIwl+rq@zLLEX2d`9gXio(IXF{eb+1(Tqt>pqNgdiibC3h z+V>PCZ!qVW@0j_d-0s4qfpr#ET}3O&H#fbQ+AO0eT4i2!cS< zk5wL^_d}fr@BJtplRq|-USvUgCy-wh;%J#Z?qFG@En~p8vsN=2+cLqrt#sak#_+fBf$wIWUu;Q3YiC0&YCbrDw)MoQS!{Uk$uMm%bsn*XfLgXF1S$ivT)km7hEX# z7Jgb?ovkz5&0`_N;Ysi<#1oe2tlf(yh)p2gOM~^S3OxuC`tcw%?}d7>Pn7LrDiiwo zdSycQFs+6b;w9zUgV1~=@IOGno(KMF=U9~gz*P5JYFy~Auod~f=0gXC%C2YE zd3JtaFgTZ$ZG(}q68sAxgb@1+V@0AtYj)jhG>~oAW5?~e9nh%p$JIIwYDq&|vp!Dr z?oc-we!L?9PH)3O^JwU-ckQcpj@3iqwN`2PY_2QO6$;r4`fZv9jYhrk$qenDfZrDj}XN5_}8sg2*Zw!We=m z`&ym357MvI1!;e`AT(d&3x>7_kF3n9eObF4AJe;Tq| z`-SYvRN{}T_BY$r(Ehu_e>d%VrZ5%xO0nx+q0N21-h2pPF}mGSpsbza6bx@4-Y;DT zI=z4T^5-_@`gZ?p)UNHL&r_w_MvpnX>`99wA)YBgtagN;!@+18w4w*W>OMiMW}hII z-C^tK{^|A|<3Ocj(`!gZuOSa}8C>)5weA)6CTWnIj0Cv5L)#<|d^HKXL)&Dil6Z9c z{=u!OFn&gU5eOl2dE3TpuB3D>CeI;l6RFa9nM^@RHV;!iA5+>U(oNfNc(!Tz=(^z8 zR^Ef!>^nH7*;nP~a_wj5wW1K>>5$-C2=Uki@nG_lNb7vrOw3~5_?X^zCPdHPti*9b z7-ie_;46wr)@v{Ai#OCy&m3cL?Ci}?7@7WiVKnHe7pB)CRV_k@mziUwA4Finb70qF zc5EArF=713KA8Wgp>^zWd}GJ{=?)LNCYA0x_V_!mLH?N+N9f`mnSfmg@$}hNHgC|N zI3W*$({R~MgJ#dp<|P`0#BRE8Hh+*aCTKS6(s_o`@Y;;e<~CA8>!0>dL?nv2=UVK9W$Htz=Hzy{e$#_h4gBlZTlVDgy4fvT<@A)esqGMY;qvX*M{Ta z$%h7CIgYZg6Q}F*$3tVIo&6<;&yjvLIf{4plN{+i8B0G&A%qY@2(iC$bGA4;+qCVN zv&FGY$n4pewi#*2?4%*H37dy2nN85_S|)&Y9CM5;P|m_+Ta5cLzq4|?AK ngV+C$BV^v|96|^o_VfP(5zA65bHtqV00000NkvXXu0mjf;~SCG diff --git a/common/tvm_utility/include/tvm_utility/pipeline.hpp b/common/tvm_utility/include/tvm_utility/pipeline.hpp index dd7a104bcda14..600d12e1e3ec8 100644 --- a/common/tvm_utility/include/tvm_utility/pipeline.hpp +++ b/common/tvm_utility/include/tvm_utility/pipeline.hpp @@ -16,7 +16,6 @@ #define TVM_UTILITY__PIPELINE_HPP_ #include -#include #include #include @@ -30,8 +29,6 @@ #include #include -using autoware::common::types::char8_t; - namespace tvm_utility { @@ -190,7 +187,7 @@ using NetworkNode = std::pair>; typedef struct { // Network info - std::array modelzoo_version; + std::array modelzoo_version; std::string network_name; std::string network_backend; @@ -315,7 +312,7 @@ class InferenceEngineTVM : public InferenceEngine * @param[in] version_from Earliest supported model version. * @return The version status. */ - Version version_check(const std::array & version_from) const + Version version_check(const std::array & version_from) const { auto x{config_.modelzoo_version[0]}; auto y{config_.modelzoo_version[1]}; @@ -339,7 +336,7 @@ class InferenceEngineTVM : public InferenceEngine tvm::runtime::PackedFunc execute; tvm::runtime::PackedFunc get_output; // Latest supported model version. - const std::array version_up_to{2, 1, 0}; + const std::array version_up_to{2, 1, 0}; }; } // namespace pipeline diff --git a/common/tvm_utility/package.xml b/common/tvm_utility/package.xml index ea2a6c8940f73..e648a3bd47b59 100644 --- a/common/tvm_utility/package.xml +++ b/common/tvm_utility/package.xml @@ -29,7 +29,6 @@ autoware_cmake ament_index_cpp - autoware_auto_common libopenblas-dev libopencv-dev tvm_vendor diff --git a/common/tvm_utility/test/yolo_v2_tiny/main.cpp b/common/tvm_utility/test/yolo_v2_tiny/main.cpp index 4ed77f0f97d83..11acf571456b8 100644 --- a/common/tvm_utility/test/yolo_v2_tiny/main.cpp +++ b/common/tvm_utility/test/yolo_v2_tiny/main.cpp @@ -120,7 +120,8 @@ class PostProcessorYoloV2Tiny : public tvm_utility::pipeline::PostProcessorstrides == nullptr); assert(input[0].getArray()->dtype.bits == sizeof(float) * 8); - // Get a pointer to the output data - float * data_ptr = reinterpret_cast( - reinterpret_cast(input[0].getArray()->data) + input[0].getArray()->byte_offset); + // Copy the inference data to CPU memory + std::vector infer( + network_output_width * network_output_height * network_output_depth, 0); + TVMArrayCopyToBytes( + input[0].getArray(), infer.data(), + network_output_width * network_output_height * network_output_depth * network_datatype_bytes); // Utility function to return data from y given index - auto get_output_data = [this, data_ptr, n_classes, n_anchors, n_coords]( + auto get_output_data = [this, infer, n_classes, n_anchors, n_coords]( auto row_i, auto col_j, auto anchor_k, auto offset) { auto box_index = (row_i * network_output_height + col_j) * network_output_depth; auto index = box_index + anchor_k * (n_classes + n_coords + 1); - return data_ptr[index + offset]; + return infer[index + offset]; }; // Vector used to check if the result is accurate, @@ -228,6 +232,7 @@ class PostProcessorYoloV2Tiny : public tvm_utility::pipeline::PostProcessor labels{}; std::vector> anchors{}; }; diff --git a/common/web_controller/CMakeLists.txt b/common/web_controller/CMakeLists.txt deleted file mode 100644 index 5e372f3fa1504..0000000000000 --- a/common/web_controller/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(web_controller) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -install(DIRECTORY - launch - www - DESTINATION share/${PROJECT_NAME}/ -) - -ament_package( -) diff --git a/common/web_controller/README.md b/common/web_controller/README.md deleted file mode 100644 index 643fc9e068158..0000000000000 --- a/common/web_controller/README.md +++ /dev/null @@ -1,42 +0,0 @@ -# web_controller - -## Purpose - -This packages is for visualizing the status of Autoware and sending topics for Autoware from a web page. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| -------------------------------------------------- | ----------------------------------------------- | ---------------------------- | -| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Gate mode (AUTO or EXTERNAL) | -| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | State of Autoware | -| `/autoware/engage` | `autoware_auto_system_msgs::msg::Engage` | Engage signal for Autoware | -| `/vehicle/engage` | `autoware_auto_system_msgs::msg::Engage` | Engage signal for a vehicle | -| `/planning/scenario_planning/max_velocity_default` | `tier4_planning_msgs::msg::VelocityLimit` | Max velocity of Autoware | - -### Output - -| Name | Type | Description | -| ------------------------------------------------------------------------------------------------------- | ----------------------------------------- | ----------------------------------------------------------------------------------------- | -| `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/path_change_approval` | `tier4_planning_msgs::msg::Approval` | Send an approval signal for path change request such as lane change or obstacle avoidance | -| `/autoware/engage` | `autoware_auto_system_msgs::msg::Engage` | Send an engage signal for Autoware | -| `/vehicle/engage` | `autoware_auto_system_msgs::msg::Engage` | Send an engage signal for a vehicle | -| `/planning/scenario_planning/max_velocity_default` | `tier4_planning_msgs::msg::VelocityLimit` | Set a max velocity of Autoware | - -## Parameter - -### Core Parameters - -None - -## Assumptions / Known limits - -`web_controller` needs `rosbridge` which is automatically launched in [tier4_autoware_api_launch](https://github.com/autowarefoundation/autoware.universe/pull/779) along with launching Autoware. - -## Usage - -1. Access to after launching Autoware. - ![select_panel](./images/web_controller.png) -2. Check the status of Autoware or send topics by the buttons. diff --git a/common/web_controller/images/web_controller.png b/common/web_controller/images/web_controller.png deleted file mode 100644 index 37f5b0ce03002c2d714e5decfdc1d326220134b6..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 72325 zcmdqIcTm$?*FTEKqaF)#EP#N3g(6+LbP)kVhmcSMDosK!0Rsk91QaPshX9clAR&Yn zkP;9%^iD{CP$E(jn)D8r=RHTy^P9Od_xa zV`5@r20VIb!o+mOfQjj3#viATzj>oAEqeUxcb^9U(?5=%7k}8lJsxxTYMT23Jsf@g zpLjbkIk|hdIf(h#c{@0``#kgTT|QZ_dOS(&Ka(`P9iI3)d$`{)b#`-L((-n`At`mk zz{Br`)Lkjb8+WA??@B64Dcms9zo7#(<=7dsW@5U*1bC=!`aEr6%-=j=!MNOh0w-( z^=kLO-RU-6=vecDSSM{Ef|q(`eQ0M42gw~$UFT}R?|Hxe#B@&OHSph2C{KgXuhGi~ zG2gEK8r{#-{(t**_($r7O!p_^QhcpDW<`68&Zn#vjsVml(1PU5PfowMJ0~5|W-rCM z&c}3>Cj}6=(f3xKK!2gtU1jnLKR>@UayjEC>o0$6G$AtaV9q0tdMV181$>a9&G(cy zPNP#7fAaP6OiXUT3286IV#gA&>nJu`@V2j{5Wv|vwxL~T^J0~HYVHZpHu;LQa{Ci zE>ncBzcjcWWwPBDa_%Alh}{elw(N~NWLbp{=}kLwQ&I*-L0@soT*AfuA)+r0DIaBN z&a0gXiX6xk9}2au5OXG=qp?ZTPX}nSuNfN7)0fCvLvRS?QxKUW(PYZmY!iNxvX8&b z%*2-x*&@5R*i*jvAS_3Uu{YrqQ1NOHhKoEmb`RizDAzhGzbItzp2mX~Qu=N`Fa6L* zU<=N)=cgJe$#Y$#Cu4UmTTee@H+(ZJFW>dPpvqBy*=i}=h{elTM2#c6ZN6v5wAymW z%ye+phWu?4w1(r1W*IBAAfchdbs|!nyuIb;K2Bvz%kEECRe!KBC8}C~n&td~;`xWj zFWKQofy|F|Mv3>iB*?n5cQev{AcMY~5{fRxDm~8r^V<9B0UuoKemmlr(7x?U_TBR{IYnmntZO3xcVW9a{G-NdTZ|`>EA_X< zd^74SpZB{Of|k1_FoR+bNG=ia8?iO&I#x|%62sm5Mcxd;{+m_7 zWR32+4(V2GEa6$_eMF7m@PdsM>gKMqH!o{XnX8+Y_1nWV6*KH>7NJ|h0D5U`$a}0c zL^nU)pNn|6I!;HmU-x$|0{px5ZhgW>1t-0*2u=k%OtoooZOEPk10@^iV3m!7CRVS| zlC7ggv-#H&p=-y)b8%oroMQ=yvk9AucqC6GMsjl5F2DQfFopRC#F|kA`sd`jiS~q z7k|BVixuS>AEj%tL!K|X*#iUew{WT5iCf94kU-_B9Z?H)Q z2GoL|<`5|z_qBX!i$;*z{_wT?AwwtC-oZga+l(Fl{5i(^y=nkjmo?p5Qlq}`9Oa`O zyZdU)<@UK@A!%}Dd-@BupS!61JaM`H=y30p%Gpn!ECjCiqL79{F0y_0q3*W1vJFpk zNwLkU49fjR; zQzIiM*#d|S`0T_vh0B;SV=#0)FJ+WD#?u?Bh@sWltQ&=eedfQa@I+39>serKqL^t#zItq~ zPP~Lz%eK*?N?fvZ(t19^vcx{h?@ii3=v;>mpu^Hz@ixCY`-2d|&f7R@`I_W;b?K-6 zb7dd1HyW!os{c&S$x2Agx09CO=TSmS8&6P=wk#KQ*Nxu?|E{ZS~hciq)@^3D(KYFeZumn zrdz6MTp0o03;Zj);&ny=&TdVkJRsP&8!i%~6|g$2ssPHKiJ+ITIMG8-Yx;DTGyC?n zs-xzgLBYNLPeu$a13oA4Y?akwB_rSg?ZeGCrX5`zFHrcW=x}T4Hi9>5Bo3u3VkFfRZyo{o%CTXYKilrLp#O6A&)2obsBsC44ilmuB-S zM0>sfOg8=6@F~tkke&|U@=1K!{dOdqeEcbPISa4|#QQ-@Kx#(_ai^Up=l3w2_X0js z$)XbSkbVYz8TuXbHf=|M$Gi|GSUpp)5J`$Xi*kWHkKk+j)@$nVV!Z;Y#Npd{kY$tX z^~fYOTKv#bcVq_qMj%|gcW5hkFlB*esGD>;z@z^RG#(f=Vo1BdkwqTvnWil{?;u~Z z*4ZP)=csFz1(qWX0Tm~X=s)8i<$$5ds~_wqjC60?ETT;)JGoFZ-`y{Y`bW=mb~icD z5^M{7)4_x%_+ zrPNqxgCwT2ZUvN!oRXgGW~{_Qy3DQ@l0jrFo_(k=Z7!Q1Wu^?Vlh;z5&NgoP&_ z-##B+3RPP;OD^u(`@jwp7tU)2IcL5ve`folyTp#HxY|~P5nG-Xs!6lzaqgf_Dj*7z zRh0x~5()?A-X*QRLC2bcU%@)whWh`M#u4cEy!)jck&Q)UCDr6m0kdjV8G#@SBh z>B3XUBVy%x`CDxf#69{c5P_)LP!@Xt)BPoTo-qYARFb zNglSQ3EKN3x21>kMF!K$Dk{1w7t@t;-D<>V6vQ##`K^7TFHn{+$@2RKO6cHC!BNd4 z{;7#4c78MN!DmFTx3@Nu_ZuZ==(k*ZPC#1YrrBU!%yG;V&92Q2;-8n$A_77xs~$Ki zYSzsX1VYX%$wxx6No4`mPF=y=1}FbHoQ?i8m$=m0I9wCb95kX6a?!QF)WM}(;d#9{ zKkeEFt;N*Lo}kUXziz5-Ju%IW)M~Sr5#Hfx9NKI9n)&*LvKVZ^7fhy4_rg;5DT)X? z+Xp8$Qn4;E8BohE+)F2T;As~T3qW=1tabIF!=G--d1a0rM~bcEH$8ObZaMAMW;&#H z^AK+k{OTre7G5}#8%?m-h7;fRN;|f^%bb_l*we6B78ZW!3@_q{i-Unos3mrasFb}L zi@yk2I!dF-(Ux$z0lSPNDC4KExq9t2@Y`w1v0nuD>{OC{bihMJZkA<^B@}n%jIynq zc(jHK`~0@F$0%&t!Li><3?=;^XHIvuFD%L#JIn~jW(BB>n{qXKw64{fIIe0gj@`2j zlowC5&6DJmFWu4K^gzg%-wSLVe%xAy%J=0L2bk$q&qlOH<~--OO(I$%bK}9K6&11Z zH8Sqrs~a%RyaC>Zh$$|@?R=;-Lx_4W9q zq@;y~g>wOlM*x>2d>*=)r9TGcU+?_VY@jT~h6`b?ZEN}IS{z?)+q2r5_=Vp6<^LEV zFgabJIC{sN9acdA0lCh10Mf2ZNS>LJ?XN##E_y}imm#>e~2 z7;GBBJUxvr3FR?lq=mp{`my7Wj<@IvHz5K zBcq28-&ATOy@4W zD1>w#ne@!59M0;}S+rw$Wn5j||LU{xA}nOp%H!nV?GdRR02^bYdi{70Zj9c6)-#Z{?ll}Bu z(&&VzH_6xS?qa0fbgwbS)cM&c>RpmRCn}_ zPvq7usMZ1xLJ5YutQTwgVC;0I*@|*QrrlYVvX|ogzBQY+tz3Uq=er8Am#N;4zTTHm zW4^exY4<3$rdHXttNXh7I)Re@$S5^3LeNLnp ziy{L%__&5AgIr9gb?fvfUv7p>L?3eo5Dd-bDIK6s;+fhJpdMOxs#}5M9WaS>J zI4TO7{poF=oAb1q5Z}FfXPU~!Bq%7j8F!FWq*XZBBH=-aFI7D*7vQACi(( zJ%95g^#t);$uLWDnT9aS`zLlH2@-$v)IT_O=ld_)C|w$Hgosy zYGPpmB)-69cTR=;T@|98NKX;xmN7=a@IWK5QQB1Q6D}i|ZUG-w&X;|^NcJ2OOf{L6 z*Uyl#-Ma;J&{4`uh|{Lyhix!3%eZqzMWq#T!jel_-EVlveZ9#G?T~sc6`ZE9ENkUn#d+Y)6eNg>BcNK{lW{jV_0w`U*PuI0VWzgC9nip;r}{_c@c)2aon%Ix-S zUm;l!sTA~3u~CyePTa{QgpVEed>WD-0k?h6;shFR=Gg{FZ0%_l@$h(8$lVbVGAQu^ zU?iVct1d3{1BA!t_z_Z4pk;&MoM+hp9i?0~RUS-eoNj(3Hi|FXEOc` zr%s(3TXy$4^L%To*}gVkqsGsf){Qu)a3wQ8-@K_=fT3fV$|qtmQj;H_R|!2WXSU}j z2C8~~2ztZeb#r$z97XVMxvAO2;qYX+&?I9V!!raq87TLmCQ5xJbAJ7xGnDGXXPp$Q z=k4hQ5;EkKeKw*Yi$BbfuN!k>i?j%2>FWXRO_^4Xw_S@h&g2U2FCU&~^%ekgfWh8g z8>alW!LrZB!5dpQXMN`;S>X` z(PTLVD?nYDIjrOpaUGO%8>!Z>iAn1lZhgeh&y^tF*4}OtT_nExgU#OCyHsGV|E#(V4)_=@B!M z+06V{wAZe8>zbvPBC5$1$2GT>yUbV!{GYOdytN#MFmx)ifbqD}s?di zmK6(u>`z%yX)x4#Y+n1`HWrpt2el=M^>AC%zgO!V$@=i)5d=^cn;o7fyt9qf=n)*i za2~b(r<(nr=f|@f_YQa)YtH;VJpQ>fo=w>KSqM8GUuF7VS8cZ*oytQGL~&Z;BWj8B z^V!U8r&PFqEzA`9*KO7`n|+sehwsYbLtuqqF>1a&iMU1ZdFn44=)7})y#8zQ%Oi^z zBTvq3vX8tl@hNZAxz+wE*jo9#_l(LG%>IZY_vrmM)?c&lPpq{5o=2YryBw;W#SE9# z2W+?s#+0X97RxcJ!7Z{yad?KBN>42(x!hReBqo1tq+vMNJ;y897CaD z5q&cjcH~OPQtj!;Lz_T}aTm`Ezj*oPt}N9+|HvB5Fw$a$xdAPrWorr$4$h6E4iVKO z7-J1?;=za$6j=H;LLQ-^nI!2lsD_rq-HkoIV6Oy?XjED>Z^_b*Xgu0Cmpq|@Vfo!VyG(N z{*~}E_9_o>ma7sz7LX7}M$NQ@^V8+;TNB(r^FJo0&nenj-(_w-ucN%-8Ys6_f?q4` z-ti023rcJucs^-GaNM5xA|C2gjqyTKEi#?uT3#k%7F`QM8usjEOpQsq?us?R!JDPE zd4%BLNR0eK-Y&VnTz)%;Pv~G&oLkui8>a69@HX8ub9%Npr9PbLoS5YxjEUDwy?o_L z*US?TI)n51?CA!{GjfcFIsjRBphZDJfr6Tx>MM-#Enwi$aG-4l@2?&C(xvhnYo6AD zibv@8!UJ+OI{otH%cTZN6ficmBDL;CQ8aM|k6{b5^7r9N64&gwBM%Y{gs6pm_x7(e zUCn*gl;UbyV}WqkPa%kU6qJWh1N!yTIZaKvunFtru!9KcQ?1V*CmERU&QFJF@&h>f zlr|2WNb9p|;l4}X9XK4~`;y5B{_Gu-8j8Dv1Ip<&q61@e9eC;C=Js4Bu~97Skh|E6RhvNVj1$VAv7!1Tj_6k8vyp zSp~}6JcNF8%WOCM&0Z1~9*tJc6j9pj=&nRTuaTyHEf69cNi?oNeXTefByDKYv?V67a4-U@W4h23ZKx$?Hzd@rJj&rBvy1 zl;S4^*hB2;HX5)F2zmTl+_|<<2@zT*=Gom=YF-A$s<6MC7#?4zrj@`+NeWk&;<3Q- z-vmI{+aD5DdIj@n1AOQCQ`}Vq0m8-=k;jXJ-A`aZ*e`0o8cs}~jD)|b81%T%N8Q>z zm-E#RtT0a+!yMJ3Qn$#Hi#l`riA_`3M30Z)LmwMiBqg4xE;Y1W z;GX+%(Xz&9#DJRr1gHjxZA%u;*7Nr9?;*KF9`3snzPqE4n9sqVFeA;*{dREL^ZAJ?mK_Q!9tTx&IwsC! zJiQ?PwvVhGT^&;G3uy80q*#$IwzWz2WM8`uY9_vYx+P9o$i)&ijA|p|>6i%>BJ3GO zBw%BNe>bql=78|yJaaHSF;;lJuQ(&u+Z|6T&*+a9rIcPVIQPeoPE{+*6DRlyE`1Nu z@0ggqB8c(>z^wE6)n&O9eO^5O$kwwSH4z#5j#GIjkpLaGct)$9rovXU`rR*Qu?dm% z8w2<5JY6wY*=lgb(GFuOa4ETF85~9U!8B8J_96m4_{$`j+>7pqkPz>W2Hth9GN{t^ zME$a>E|+uIruC?I{iCwSXJ zu3&P3V~S5*zyT90Kqp?m%W<``3+gd3V6v?L!sbMne!#)X4dwuN#8;PEG+;TpUz&K3 zu--A9rMOn};`FUBa->Ib1wdF}PYWUgTTW<6lgE`txqG zo`3PBSxBl{aNXMxOc-;tuH2O3PIGen%E_E>Y@vQ-lU6#Guz_#SGr5BB8NQ@2@e>R1z68Iu z_4(rJDpw{qhyUG6zof-<#BppdyT?w4yl{)~bjgpyqkpEKl1t@r%A=@I+nYR(nkz6v z)+bTd?;KseI`*%jg+gAT!!+B=%}`&SDiYj_F4}bABc!!l*xXFefi4^F^WL^C6{`%i z|2RjHW5$0q_3uA1`%>`4i!k43t$F2$@>SnU-dbO+Qa#vR*C>IaV%jK<9bF%oS(eT5 zKYwZ)9El~zh`NhCHZ4E#R7E4kb{Ypm3XN9eBN2?SD&JpN>dN5^%MGBWNm#Y z2*^)kl3kKARdb81oxZ{sj_VI>Y(-UqNarm7Wj`k4H;GEoK1?NNwKX{v zj*Nc+>2n_1<)_A(7{qL_#1nGL=9}+4`@tR|y75`y@JG#zULX)ACO1$1l(*6H?m}6h zou6KB&6+;vUB9e4)Or|Wgu6e|%w{k(zw^cH6CD-wN{?#hyOD*f>y)Hjzy3hRoYzBM zWhJAyO8(hE!`&)=PR{i#uqKG)V%ycXtQJ&slW;}W#q`5?a_~e@?SDBl7WI#>GhQ~J zi+24s2X<+*(D|m8f$mAm#6Zgqw#CHs9#7bMS-h{cw4nIMf*iQ=P0v%ig4`-dAN(JykjQ^aJ}!dIY6X|l^yW|t1B(sb2*$5fj+ zSp%C>Jo^6?;-qWepp=10b?H+d{qCQqWV9)`D3_Y*K9FJ^Dl2Q}%vtxTDHoB^CydB^ zD3b&g6j-P^%gGK@C&qOzjAoU=BLE%pi}6=dbz;-4oU1tdURn+a zr}}h1zLHP3(kS0tlPj&8h$dOqPu5zyR2Vall&9_FpcV6<%vc+HMCdIM64TC}!_jlYDjkxUWb=Ai+|&9tHL zHaW{AJpoXws-L&FRE>MF*N3Qyu#r-JVMPJsL<4v4;;KdfvRgz^5h$70g2|u_afx%8 z>dZ={4dAUkouVk#Ap4(;4RuG2J7X zm3uyUt5|7`(f?S%*bQE^N2P~aRwsbH&W9AW>Jy-+M$1v1NSi8F?J8D()7{*)2>k^6 zq(FZDo0b6Y56C#&j#$`tVPLaKL#c6SDq(8%uC&#^Z`vZ?E^cvV)7@fYkiJ5+D?|ss zIv`=gxa&bf4M865?refqt8K2>dP;0iS>v5u}0Kmk=T8BwABc18uMiN_2QS>eYb&L!U`Bj zjN`87kSl^CRe*MA+eQ=j;BaD9S<{ghL%5^EnC0d|^DOh<+ypDJ>-SZ5WRbU!gGb~! zeoJQ3OHM19y(lMpZfSGadw%Enugm2>^f;on?^dqLCJ|Cu;1Lt^O5%J}4maS;hC#z^-1L1Yg(X`?>%-?RH=c++F4l%>7brE9Z2 zW=2N62TN@JFa7_O*CG1ydP%fU7Z!*#o$!vzfz=LWz zvzH$H@Z$;#I7E(fyZmDiB)Ls>?0HJq07X3+p=yE#i0Ah?toHlhlFxj;TN{Wl%n*4q(ljlnr(OA z5e)kQ5uUxz%awn4alBN@K7ClBq{c=?nop8@W{R3f-5algQojij)`~xvTDxU%V&xQ=?R@`Jx=>Qt-}(1Y z-Ch>?ksELuY(RQFX@c#uO$|p6i;h`A_bacgZ;te|)tmdl3x-XIT(UBsJ~kg9ER02L zVvilBmIR=kr15|2LYr|f)KwEw=SDtUDDz?o8Z@&>*o8t~poe#Iib0%f&(9otVw3;W zH;xE!#kjAl#9yp^3AQ+KCQob8hPO}#8E}qavV_K6NAQy3h`)jQDIa>siUE6-Jp*X_ zOROHdVTs}pw3+45taa3|r22BVyz`f#ce_FTTo@1kN76QMh)b5#8hxH3Diy=%Fb3u{ z_sp+3(-`SDB)BbQz&*q=o93FOpIr?mrZ3r9FOwK5c%T!$wK_7UHf>3^5oW4DEA_tn zNb|Zb)>763VB+{-^N&8z`W|CtwX)+J#D!f6<7Oy9p%Bdmib5J8QT9?9W(64{GEY34 z3a@l=38~bEHsg?a=_|N_p{g(EpZ}{HqanXtG>FpC>xvJJiYvs62z2R1Qg%9o9A{nA zN2g}&1I=#7dr*arTiJX{mHpnbIYN>`pnk~dywkie*SJb4iZOHQc3b)3+f9^gLCG2f z_HPO8+a=>K4Fw^iX`%^16S1-S0&O3-JeLlaEH;qdy@jxpZW00RjjZ8pXlrX*7MMX?pk{*Wf5%~OovM6lov(%W)zsQ^Ev5i#4HxG(AklhuZQ)%cZDH|6#036rG34)NPmSoTy}P0 zLcVU&>QY{SWkZ9L`!koj+~nOZV|X_ov?cLMspq;eZ27m~Uns%jI%MDN;#JpEDq9;5 zxdXrRAKc3dk)tfyksi_we+Mf8(Z>%0Gy%L##J-;8vHve#@6y(!5&ZU*aes zc8eE(8af4c%C<7YVHtk)(5@b}0;DZ3obzeZp7!E z{Kn#v-QpQMW7V?U1XxDRP3^b812&wjb1iC`6nNGe=oG(XWVx}2$Yim>=ONh|{|(rm zPjTo8#L+jqmb&+M)>yAz^{uaI;=XfXxy}TS?}|1Qf4*|a^7LF^hb))#>Z_bWI}Ha! z#eA{WU=ZIS?w1lVElJDG{Jwgrep*}X4?wrd6U9uLqpK0LLH}3Fjfv&Y1|iQ7Qyi-7 z!N=$&@6Iyi^>KbRm%)TRn@?^66JL0Dr{WCP$ypjKzW6X)`p`dHi_1K+ChDq4Hg?VtsEl=iI&cN=m_9_tKPXOt#td1L~3(dx>C4tMe8^Y(NgwX~w}QJM}q3$DpQo zIHsfR>tj`PX7a9{iWhRWzK}N~{}yr~P}Dlr+1;G7J>gPkL|YOge?sHilYBqoa5zy{ zw`p)DhPoaIS=(%$T94(Zf5Z*xqJ=lye8d%3#`EJZv=HY|yP<*K^u|QRdfHxAj9BmI zSGZ~0K9Jn$kcBjrslnoy<^#XC2M#}l_vNIf=eemT$`wZSuDZs1>SG}44U2pJ{a40n zeYkQK2JmZ}qn@@OV-EW`q_V5C4y_Af>GB?oV@anZ4+w zW2~bS53U_EekEqxi|4v1B5BCUNq`qx4$!l=f)8{j#jNl(a1*0I%0F+NSG%@Q%5p6?XkJD4n~GVS;w z=Qy;Vq&K1YY z7&Y-bMBDm9tj-Mr$n$$s*4Q!8`;_6}=rKu+;Ds9S#;|42X;6oArOROtHQ|af8oIWn z=5Q)ONJ-YNxgyQ4*-oVVaf~tKG_19(9&0K)=hL>;m7^VtiFwc0R{AEdeb@=XhHmAi7B4qD z#Qrn~rlm8lfj6wCsJw*zoEoY>HW$0H5>dbDQ&m`9(ZzhjK{0zr8a%V0y?ZxJ&F{e_ zj^UG>Y1771@~s3Pae*xt))KTQ_YiLqqDjjwV@`B=K?h6<PMLd5HdalIeh(k`yO@|#+rW`i3APJG#Y_=g4e?o$-?OfYsloR!~l>W z%37iJuy1fYFRJW%JSIN=YVJ&wv;IUxL~X$Aam1VX&T!~2ObfBI zCn>A(lQ)nMM_deU$HHn-ikG_ZJxjbj*~i|_uj{d0b}g2vWu{=a9Cs9ddH~k0s?&?2 ziGd>lio|t6HyN-I|Lg*^`@F7<%Zf$Y)hxB6dwlioA*Pl2@H+vL&!cUQwRQs`g0Tm% zD<~;plOg^G9LS)ao@t{x%(0I9-c(lIvTrwF#!uc*Qc_abxiFZKGrE-)8|&R;_b=f( z$Hk4~r0UUm+nf|^_JIAI>w)c2=}!6Qe2Z*UpJ7d0>BULPTsSM9?Ce*FaV4p#- zj@h5xPo|f_$H~eE-^Yh|YC0G~i=yM%Y!Y`Wyn>|1hXu0hRF=!z_6o3?Nj_PQB%jSUz)q!ftQ2e z(ghu^2sGx_wQ6lX5@L3r{y4UL7H5Mnk2<4LAPleP(7U7z^RP8>AlNk za#La1rv>5h#XVKmYl7d&p{gt1xMB3*9S`t!#U&ugkNR#*k5$o72(c&RSC}{YTZ7O!Lge=3 z?#4X7q7xPoBC?alP+e!$M4xN z%T~t3x?xnHolfoMvRZCAejzjHvP%G7jnys)k8*9T%3Ke zTOmswT%Q0bYmPQQ$YSis4Ssnd;WE|wk#o~DnBb!DsH4YeCh!*s@k_m?XT;Zzavfsr zfz0=qxLEq9W>`_q<)Ra*)w9jh{=nBX6Zl{RLpeyp0G7w+@K7B1!B&X)`L2u5h+?)A zlrrRwvFSIUlzxoE*%yoDfh7=lnqYZ1Gf4l;hVkru#I~8`=DVkycPCknE4`Ys!k=3$ z3(gIf0B(K+W2`fC_}YXXe+Dnxn^_hL-5VW+`iar6ZFaS+Hr)rL35y95 z8r&)h`9*v#FiyozS0S4aHxZSQ7TZ_?m8HF&QL%Ymnqc4mr|gOlKNqrtde^0JeAoWf zQ;ah1QhNJT`TK335ACwM)_FyRM)mDY1M7|TYqiTEoTSg6twl~dBNn1gt7oOZoJU>4;1)7&ZT-rZ2VI(c8PJ=e@#}T$?PXEpXvGC3&&A;9X?#)ANWkgSht}f zW_jy$lx>yife6y9N)z`&v&(iHxS=gE*wcvUEOHPKB~3x1Rpj;t@`*N)5k zoa;4}K}tWOIL*iR4kI~|*zyhIBuja#7Q)~v=a$yTj*>u(4Z`qB+bIdQUcFyB{G&eW zen%y=93x@IO`t9*g$z=c9y>@r7wPTO6Fi+eKDdK zxONBu4SU`nVVPw%Fci7B9qB4e0d|#IbnH0_95+yxRk0N2gJV<$mf96CmuomEB8<{} z8}}9L+&Rg8^_vxd43z^N4bIT@A^`nxeYzQ>&P~l^2sR?&Xl;oZCyQW5&NQ_Djo9e_ zRG`fd5NenNRmN7<5eeKS_OjF>r*%X(~;n-Ur)lek$J!8HSng zvZ$%N-d>gH*S9p7IFA^Lh!?y4N@DwgSh z-*;y3dJkMJSXtYBL9GlBY}E~Ciz53h7bY3!l8dytb2BXQ8}ON1fZ2*7lSdKZR9F$B z@rwDLaxRBYP@hD-_1=y#zQQ-j)LK=Qe`#&+-vsc-jjP9{EmKz$!wj~OtJhzcU?#H* z5A?U%+MV*D_xA1-IC_aOUj)Ys-RC(i6fg+p)~Ou)LvzGzI&`|)Wh&bT z2Kr@KW&PEUv2qsi`X8H4g*lAxwM3q5z4iIkfhuZtM}?){UtzX(IWS0CdF>$GN6T@f z9q+h4_fEn2$CqZdx>zNTYKdvY#qL)lGKMpSqlc>%W2KAHW2xiyQZwN@?dcK@Unc6? zIchw1ZN?Sy?J)iqn?A<@+JXg-9fR0 z-t-o(K?TIq^<>e4YZ;J=0-ac8@4=IBVi#rD*{E!QOqL}Ec+^7z5>*lcTRJ^3*gIUw zCpVX>9xQr9KIcZ2^sqaRyTvT173%ZUt)Sp_UOCW%*GfKX_3ZW?L>7iT9Dtu&`mKZxiy-R*}wwFg%zmyy11@<_?S29M{etgZ?`K}&9 z`~JYDZE-e8B{&G}q8dz=mnzqS?%Kd-dQc<1`v1sOtuu_pfFw1tB&RG^RHd|~x*Je! z5GX2bb)l_$L`DxkQacYG#_VM&D+gF7x*zKCq?YqpWhmTQ1h?`dVMEeKfelXmPO`Tj zxKwZ1&=0I2o?9wgJ>Ue}HnpYH#Kssml#<8w^qG?}JtwT>tmfeo(TD1FUOVv%&--%~ z;O1D^XZOD;d2Cm5>GwQ9_~K^kw{|WmzlEIjYw`lf3p>`Q!EyrGQMXMzt|`e)HY=y( zC2h2`Yu8GSxg&j_(F8hd0E!nmQ{6BW)%G&;eG?lSBR-Mqm5&?JM8&OCFB2?U#>$S0 z<#wj^{T;ensiQAw`xo{G#_4s5S@Z1sBDP2EAUNX7$_`4pLbt=;Tl#NuUT%$@ww;s# zq{6Nzs+=~n--npZEP1^zoqjtZ5_!ih#m($S>`C?bU~_ag zu5vs!e8w&DSPSbDDLrkdy3NIXpQTS}8*yd*`2MT^6m@^7ml;Ri)30{i{?WA>_r9o0 zOqV(vKY_%RS3XH6EzeM`=KCE`mxgO!!0!z^_W)PlJ*k|1?GYs0Wvi%ih8fdV%YK+Z z+y7AsN6*t0au-+aIxPLVuGAm7)>Up#EvV!RNR0Xc>W;Q7IM!DtrCmdf^T)QXCp6Fw zw8Xb9EK*r^^fpIbyGZK>HT1xpLmJAkhp3w1?!(iG@JU>D8GrJ)Vx({e^7)_0<4d6P z?Xw1PIKMGfS$5?NxLcTgi@L~}gsvL5q{UrUxc7q&*UFJ-5&cjru;dLVYhMuc;78z2 zW2kK_udQ`F<|0DMwbEm170){&)8q~*;d3C5%r2l_+zD3Mt4&+S6NNxQ}Mq7cdi6~svf6NoH1P?eo?Z*GN?)N^7}i@qC3r#yNQ;^_JQRH zp5;z4QQ%YBc)=eNgNlXY#dXUli_(v=Pv})`9q`qX75HJUnTc&3pn9D*_U3_-48*>~ zUR@xD7v7~iKrds>ygObtQHGc8^1=V zzF{~P?4OZb!Us9yo|NuCCTX?hmJaVx{Se5T-X|mW!-d4#aS+p`w`Op%Y^+4kN7h1a zYp2t^nBhy@fU(l0%WdV{Kq(kT5q$)wA5neExewtP;O~lTZVgXtm3PDY;|Kr z(^MsU>PD3hDvr5X$8@2Qevc}HMjVGeZNg#Hzx0A}nHlghTRj9Q&$Yw>Kj1g_;bMA? z!?#T($2m3?QpIp-yqUj#9z}CT50z5QN?m8XC#^w^<9Hx)7Ry+C@Ik1uELzecg z>2DaWFR$O?j&#^_+sm1D0j_nPhj~E$0VVyYaJ$+-|DJ@lpP6@{_xkL_SII(Z^4x|+ zQsmsmgN;GC zHk;pEBw<|DZPIG)0s1i;0zz!v?SBCz94AWU&$*;89qi`%AZGwnw3QoDG(}0ohpp}s zL%cJRzD6p9B>7V95Aw)3q`U}T9IMwtl@lT0YOC!qA`zV_YpVfC|5Wh{YoZ=UksMM~ zrb9)=@qi@1t}^?!*sv}!YMxKcsAC7SXDsBe#AJs zl?`tO=FHCfNCh@`b|xz*^+W~UiOZm;J*l~9IH=!lVl$=!u`=u&NGU5TH;bt~{2nQK z2ixXI0Abl`yX64&ZsF(-DBB}9O4deLu+iU?{NQyBnf1bVr6nrprsj@(awB19^t&wN zBn(y8?a_wFbalemcth=1F%#oH<)+wMtYoAk3HF(hh@<8O)^F0sJvu~F)P%N1Rm86d zbOQ!6xns)>l6Yj{Gjw`N&1bur`nwWhRA!wP%<+u{d4RALK?iPQXrWo-T20rI43zqt ziuicuw0^P9^FSLN<34^w;2>Hf{Gv#`;+77InrN~9eGTsAwL)paHKEfL(G~|C@Fz3# zB5t-){yNR7E7mh&?$3JGCV9F)&)QniliA2fG&)OEmysmnhFuz!#U*uhbaF^bBis>+ z{}+329?o{&{*TUl`<>}CPZ!TTZ5dTOUDe)F)S8*;s8QRXC_y@u)Lw!#NOYdg)C@)K zB}i*a?X`;~X&JRdk`hD&u_huSBqFk(v@>&lzw7*SzSlYDpL1Q0%U?-8@4Rz=?$7Jq zUibY@;O3`yQ9IL1vchN$My54{Te82?GA=?)ejGnhd^MFtGf@9yM|adT;(rg5>67^b z^VMPGUWd8H&WnbV7>AlKcC=EOYdj#-u7q&H5yp#MCkN=T*Q2ya594 zeJL~5yt#AE2HEj_E<+c{i@TJGG0q628!VVFTp$!phivbGc>`=iGJL1TWxb$_RcaJ9 z4BjN4Xsf!9k_Rlxul27t_Jr`YNQ{@G+3P>4W=!@vA+G^j|K*7)ex?>!JDMm{x>c=z zWFsGSb$;`hj3K$<>xV8;D;y+dTI5zwQhcPDSz8TpyrMi{5fMU|3tc+iTN5Nb0oyKi zFsW{`Bk1{o$*zyv$c6CJ6x*nrXdoxQA}FQ2%*05=w&R*$oj|TjknAlaB9FcbP_~D$ zrxNRlzw0n-LYP0XW_g^n9>=z+=zh;9U0SFw^2*0YZ%wZcOGpcdtxz zBAxoLcde=ct@v*$TWN*U`jHVxERb8;zi)lL@@D4PHxf>9Stl#6q)`9lOhZCXOba}b z#(ZDT6C(LG=9Mj{HSH z68fB@Z^kw?YVFllD@^cr=Its^`b+f|(x;h%?IIOUw^vX5!V>;H;$KswUh~88(K)TH zt@#CBYfBJq`g<2W^=9V-AeKM+*d!&RskW)X+1J;8ZhC)A^#&mSvD<;WRzKTQ5ii>1s-E3_N=qa2PQ9EKJUPR;Ot z#dx}49WV5WD&x+}CvQa=s4kRycF`MH-Mq2QcMkSCz-*;80#5XH~Elv1NM7g-}!<58a!F;a5C!f8K6|OsFQ&% zM+S~Rzgqb`iJqMi1uU}AqSqcsQRq{qq1SAN!Ah%83V9$5VtYo1>A{s(8pOEuW}k%O zym20!BV2{wL}On=tvs~juv#k6Mf-j?BXEck1MC~a`U+(g*I$6x=N#Pw`@MF5gBa;= zC+$&#-aS0UQ1nSM%=K_umJ4Z(43GnNXCcOF5GK*!l5G-vms8mp-J^CGE_yPZzZ<`s`K2au z?zK!E+St;OY8BR2F?_bje2*qhOV2xeTP>{&5^*(D+BUSNZkn!c8hR3E+3Du#%YjN! zE5EUh(NM|TwJUOc(q(anmbkL!CNp&W^8oX^o4tPcw$$q;QFy$` zO2c9LbR3|&W_=|_dTBK^FAmnb=LURmRk_zHWrk^^O76RnbF`Mg*CaD-qVH8+!%H!BB zu!p;$meOb6#8`1Tdz|!+EvGukRv|ob{@kG?sAE)=^4oxo?Km$N<~vURBzWmK6>+!7 zjKm7`E)K14Yzomk-DQjvb(O~LS;E3K9kfWPOV*ZmxukW5V{=VI=ywPR7Y7;j$sYan zCENoDM1BB^<{NHAQggR;u#xR0WP@jIgts^5TW!Po(KkQcFp1xIUl5qQ)#Xy>-9w~e z)>hM){Gx2aO8a;;SgLEG5dXf_V_!5iHC07>v}`FaWkVK(Dz}iWL&ZDU#_KNzQ*%ZI zF->qxp;?*f3n+I_{rZ*OEyb>Mg@hYFTL!$~TbhPQ^^*%3K8wo*SXs{|bD@xY6QSp0 z!wd45F$Md_0gVotAGdmO6KiW(rPvz|3}9-XncA5a-&`tYSE5aH2WiT(ZjLoIICaY2 zZ>yWUc1xO`sp;%ApZ3C2v41KfMVjT@Tc#LaaP$0w|z;j?#~31(`GeYd_ABpQ~9ic*$w z&B>H8N%jvmdBZwHr`$ffm_@x_{Z1%FNk0DuRV1etUCm3j2`HZ8ICN?BMsBWZpoTB5xI-=<14w?0$!Z&AIVM z>W_V~n&v8Ucm1lJald_VhE~m$>;NP4${(A)3W)NnSx#asBm`mwOvhu^^@$^3TsuB| zbu-FU+I(iV@<-C1HMX1^AX>4a_y)Flb!xwF+FjV1hccf-AAH7bXi84qHwbPF zA91vlz0a!Gt@g|kt>myM4F;nt^WruIv4e!QCNFv?%-bNRo08Fn&D+JF+R{Scg<8h- zXA0dAf)ncPT7r+hngaIU=#)3p>+dpRz8}KsfV21eAydC4Tm15^n__if-aL5yY@kz( zf6idn)@-}7pPjNAM{hkh`nz=PSHsE96w%yMH9L&KXuE&hik*5pp+&@QA9Bp?#Q|(W zLPA)QmWYxucipzQtu@v~FZ$YIiY zdB@X%2U&R;f+)=bbI>?hFq*5Y7^eiXFB#0XLz;KM=_e5R zlF){bhyJ-$=cc2OI=-byqTTg0R02$^xaEOpyY;Olf2BF(pk|%@P#erlH-0)6O^eJ4 z?BI9=_!jfV#^N=)Z)DlB-0T-UkxWo{G ztj*7#rUW$Bu?jds08GTclkwsA;P&Ti%*^`Z4loLT^rP0KgJsKCsa}IfuYVJIq#P#Y z-el{$V@DjA6(B(Cx9~1zjF3cfb=WRi)R%o1h{N_9^=R!P=V5X?c7iK?)d+8>uny{H z&jl$^j7|Rjt-*gzX7JxMyNws)l0zGr<1%J5Z#eBVWyFA{cUTic@XL;|TzYDAZ97ff zU%vPx!0>Qs$%GJLi!9P{42bUCs_A=wy{sdS9R^3lR|tP zd+l}kvcS6a+kl}H7>4-jF;vF|;MVIwq?i?V3dk*Zt9g1@$G8vR<1gH@j=4pBT?!EQ zq%_2|b=h^YO3V25hnw*)11Rgb>j^N1T*8wMWT_{JYcm1OAN2B($GY*)DKDl;y}D|2 z>8&j3fmZGJ9=#-YYMW=q1;?x0Ch2mWqrN7+g5r+h8H3yuE{pR52@}_ZeGwKv8)q9g zrDBZ7oVSRWYbCR~?lI;uusx`@%Fk)>(6H3dZ0be(y)X3WQ*NUJ7q z(+MDiAg5i|X;aMw9{|Ob+#rfP%b#DhXeOl?(7$po1`jWmXv2ALQkK)h9CMA%8xxPX z1>}~^ng4t&fzM#FtW)zB(m1FX#csJXlJbuSM#XXVS+jySDeSJU>e*!pEK(XEdPpjY zV{>uOAFBYsjh-1bk#}xxlW{La37!y+Fq`nYse^@E%@K_m$Bdame4Kw>6x&_mo66s! zC)Y=`8T-3r)6|KE`5nxUu&{wc7zTQHeaB5DyiRE{Ol#~L3}X}>M~&6wzXuqNxhEqGQV|*M4@~9zZT=4=zmKB; zW)wLICZgbDG=C`c+Rk0gnyq8Weg+y5Y#|!NdKA>t2pgSZ1#JqOOC@O|W=@unPQ!z@ zecxLobGzcc@5aKDS3Psea1R|3yc~;i(Uw22xKhJTCF06rRc=T0c^g zUmzBvM3Z0yzlQX7>M1-N_6hx^PjfpcI2u?M*k1?`s_ED~`)#P_B6_Gohqv@ICX+iC zx^esv1OyjpK0CgP4BKF5(E5&Bp0i{UeemytbATG^k zI^H!zkj`dJS3z7{>h$rsfW~7b0%@s>Iv~92 z!6((hUcO@yontv^N2AV6cl`^x(3+A?;GHsA3NdXWI-<%aGnqN7Z$0 zfQ@l#D#9_>)pBobwGD<^Ru{c2cg(dEg7Wd(z@9>D&z#4I2eDWTlzXbvaKuyy%Ht~s zoqWEh{yZ-g1||Qkw}U=>p_bTr9#_&vx_(7A=XJ4`80fH7u0=S#4@gh|Oj@wtd*qaq zY~JpR_45t!gGLvXC}I0TaqJ*WL{G&&yFlH2me&?TU!+2&I>S6?{&8KtU#CNfb8%!7 zBxr}=7Yj9%M|(|vT@z0Svu(pgB~6f+QSIi4`-2B~L;+2itzpV`Q%h!a4MY7=Adny> zw`U$fz)eOjVb#sz*9*R*XJciH>>N*Haa5M}{8w5=Nmmr`aWsUzH*Jx^m?|ofFZA_X zimG4CHgED>ShZkI;s7N+ZQsuN8jLhh@#vuFKwr#^?vMKVMQ&z&@%!Tjce)wD^qtN8 z!*TN8Km6^bm7i1cuftOJ;y)yPnmi-_Zoo+8=#6U^W4?Pzx`;*PCRoRo?vZ&Y3#l1U3VeV2i{tNfzuqOSh zCPQMQ%Ug<6Dq^OFJr_f^4KY<$e+bxHI)&N&W!X~vEZRj-K-nr$SOqtmHMwIRPE<^H zwNax*ob%a?O%%dzSaZK6g{j62ZC=caV+Uj|NBJ`MUPawct|v}s>JY}sE|u@Sn;YVK zOX2M2^QqG+yyXRn+2`HIHkp9345gmfNoXBzq#l3t8F97Sy}R}6YChf=n=0R)tQ!<9 z?DFEf*PS6&`*kkLv2UGG{*U8gFDW14#>%C-v{YjSR@fum?@Aqo3+15B=#J>OVfarE zGd9ZTcO&VUsXql8-OV35y1=c} z-&?*Hyg}m@@=5~uZFu4+k)s3?Cmm3s8=UuVXTO8Qe|Csq8`?h1G5u!$bpcvg{UJJ7 z7@a9Q@p)9PGH>uCtX?Q`UrdT)3V!HhypJ8=0zSNaJHMdXg{#RE;fF)R1PswekuiP) ziGBT!6-&<&G}f;G!lQh=HRLG?ee?nwV0KG)P6A8 z-l)t>zpdz6mbYI0xP~PNmN3nG{o`_jbJ_F*=b;&G;??B%@L8MvNq;zf(x5ml$_N8? zxAL<1?>dTXivsJh!$?Yi