Skip to content

Commit

Permalink
refactor(motion_utils): delete wall marker explicitly (autowarefounda…
Browse files Browse the repository at this point in the history
…tion#2506)

* delete wall marker in intersection module

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete wall marker in stop_line module

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete wall marker in blind_spot module

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete wall marker in no_stopping_area

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete wall marker in run_out module

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* make motion util function to delete wall marker

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add common function

* temp

* fix(default_ad_api): fix autoware state to add wait time (autowarefoundation#2407)

* fix(default_ad_api): fix autoware state to add wait time

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* 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 <isamu.takagi@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* chore(behaviror_velocity_planner): changed logging level for intersection (autowarefoundation#2459)

changed logging level

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* fix(tvm_utility): copy test result to CPU (autowarefoundation#2414)

Also remove dependency to autoware_auto_common.

Issue-Id: SCM-5401
Signed-off-by: Ambroise Vincent <ambroise.vincent@arm.com>
Change-Id: I83b859742df2f2ff7df1d0bd2d287bfe0aa04c3d

Signed-off-by: Ambroise Vincent <ambroise.vincent@arm.com>
Co-authored-by: Xinyu Wang <93699235+angry-crab@users.noreply.github.com>

* fix(raw_vehicle_cmd_converter): fix column index for map validation (autowarefoundation#2450)

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* revert(default_ad_api): fix autoware state to add wait time (autowarefoundation#2407) (autowarefoundation#2460)

Revert "fix(default_ad_api): fix autoware state to add wait time (autowarefoundation#2407)"

This reverts commit c422485.

* feat(behavior_path_planner): cut overlapped path (autowarefoundation#2451)

* feat(behavior_path_planner): cut overlapped path

Signed-off-by: yutaka <purewater0901@gmail.com>

* clean code

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

* perf(behavior_velocity_planner): remove unnecessary debug data (autowarefoundation#2462)

Signed-off-by: veqcc <ryuta.kambe@tier4.jp>

* docs: update training data for traffic light (autowarefoundation#2464)

* update traffic light cnn classifier README.md

* correct to upper case

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* feat(motion_utils): add points resample function (autowarefoundation#2465)

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

* feat: remove web controller (autowarefoundation#2405)

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* fix(motion_utils): rename sampling function (autowarefoundation#2469)

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

* fix: rename `use_external_emergency_stop` to  `check_external_emergency_heartbeat` (autowarefoundation#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>

* fix(tier4_autoware_api_launch): add rosbridge_server dependency (autowarefoundation#2470)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix: remove unnecessary DEBUG_INFO declarations (autowarefoundation#2457)

Signed-off-by: kminoda <koji.minoda@tier4.jp>

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* feat(gyro_odometer): publish twist when both data arrives (autowarefoundation#2423)

* feat(gyro_odometer): publish when both data arrive

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* remove unnecessary commentouts

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* ci(pre-commit): autofix

* use latest timestamp

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* small fix

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* debugged

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* update gyro_odometer

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* ci(pre-commit): autofix

* add comments

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* add comments

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* ci(pre-commit): autofix

* fix timestamp validation flow

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* ci(pre-commit): autofix

* remove unnecessary commentouts

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* pre-commit

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* ci(pre-commit): autofix

Signed-off-by: kminoda <koji.minoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* fix(freespace_planning_algorithms): comment out failing tests (autowarefoundation#2440)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* feat(behavior_velocity_planner): add velocity factors (autowarefoundation#1985)

* (editting) add intersection_coordination to stop reason

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (editting) add intersection coordination to stop reasons

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (Editting) add v2x to stop reason

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (editting) add stop reason2 publisher

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (editting) add stop reason2 to  scene modules

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* add stop reason2 to obstacle stop planner and surround obstacle checker

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* Modify files including unintended change by rebase

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* 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 <takumi.kozaka@tier4.jp>

* fix error

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* ci(pre-commit): autofix

* modification for renaming stop_reason2 to motion_factor

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (Editting) rename variables

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* bug fix

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (WIP) Add motion factor message. Modify scene modules due to new motion factor. Moving motion factor aggregator.

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (WIP) Save current work. Modify aggregator, CMakeList. Add launcher

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (WIP) Solved build error, but not launched

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (WIP) fixing error in launch

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (WIP) fixing error in launch

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (WIP) fixing launch error

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* Fix error in launching motion factor aggregator

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* Delete unnecessary comment-out in CMakelists. Change remapping in launcher.

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* ci(pre-commit): autofix

* pull the latest foundation/main

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (fix for pre-commit.ci) Add <memory> to motion_factor_aggregator.hpp

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* ci(pre-commit): autofix

* feat: add velocity factor interface

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: fix build error

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: stop sign

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* WIP

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: update visualizer

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: modify traffic light manager

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: update velocity factors

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: update api

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: move adapi msgs

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: remove old aggregator

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: move api

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: rename message

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: add using

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: add distance

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: fix build error

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: use nan as default distance

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: set virtual traffic light detail

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: remove debug code

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: copyright

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* fix(emergency_handler): fix mrm handling when mrm behavior is none (autowarefoundation#2476)

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* feat(behavior_velocity_planner): clean walkway module (autowarefoundation#2480)

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

* fix(trajectory_follower): fix mpc trajectory z pos (autowarefoundation#2482)

Signed-off-by: takahoribe <horibe.takamasa@gmail.com>

Signed-off-by: takahoribe <horibe.takamasa@gmail.com>

* fix(ground-segmentation): recheck gnd cluster pointcloud (autowarefoundation#2448)

* fix: reclassify ground cluster pcl

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: add lowest-based recheck

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: refactoring

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: refactoring

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>

* fix(freespace_planning_algorithms): fix rrtstar can't arrive goal error (autowarefoundation#2350)

Signed-off-by: NorahXiong <norah.xiong@autocore.ai>

Signed-off-by: NorahXiong <norah.xiong@autocore.ai>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>

* revert(behavior_path): revert removal of refineGoalFunction (autowarefoundation#2340)" (autowarefoundation#2485)

This reverts commit 8e13ced.

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix(behavior_path_planner): minimum distance for lane change (autowarefoundation#2413)

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix(behavior_path_planner): lane change candidate resolution (autowarefoundation#2426)

* fix(behavior_path_planner): lane change candidate resolution

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* rework sampling based  on current speed

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* refactor code

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* use util's resampler

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* consider min_resampling_points and resampling dt

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

* simplify code

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

* chore(simulator_compatibility_test): suppress setuptools warnings (autowarefoundation#2483)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* docs(tier4_state_rviz_plugin): update readme (autowarefoundation#2475)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(trajectory_follower): pub steer converged marker (autowarefoundation#2441)

* feat(trajectory_follower): pub steer converged marker

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Revert "feat(trajectory_follower): pub steer converged marker"

This reverts commit a6f6917.

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add steer converged debug marker in contoller_node

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* feat: replace python launch with xml launch for system monitor (autowarefoundation#2430)

* feat: replace python launch with xml launch for system monitor

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

* ci(pre-commit): autofix

* update figure

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>

Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* refactor(vehicle_cmd_gate): remove old emergency topics (autowarefoundation#2403)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(behavior_path_planner, obstacle_avoidance_planner): add new drivable area (autowarefoundation#2472)

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update obstacle avoidance planner

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* clean code

Signed-off-by: yutaka <purewater0901@gmail.com>

* uddate

Signed-off-by: yutaka <purewater0901@gmail.com>

* clean code

Signed-off-by: yutaka <purewater0901@gmail.com>

* remove resample

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* add orientation

Signed-off-by: yutaka <purewater0901@gmail.com>

* change color

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* remove drivable area

Signed-off-by: yutaka <purewater0901@gmail.com>

* add flag

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update color

Signed-off-by: yutaka <purewater0901@gmail.com>

* fix some codes

Signed-off-by: yutaka <purewater0901@gmail.com>

* change to makerker array

Signed-off-by: yutaka <purewater0901@gmail.com>

* change avoidance utils

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

* feat(default_ad_api): split parameters into file (autowarefoundation#2488)

* feat(default_ad_api): split parameters into file

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: remove old parameter

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: test

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: add default config

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* docs(default_ad_api): add readme (autowarefoundation#2491)

* docs(default_ad_api): add readme

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: update table

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(map_loader): add differential map loading interface (autowarefoundation#2417)

* first commit

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* ci(pre-commit): autofix

* added module load in _node.cpp

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* ci(pre-commit): autofix

* create pcd metadata dict when either of the flag is true

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* ci(pre-commit): autofix

* fix readme

* ci(pre-commit): autofix

Signed-off-by: kminoda <koji.minoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* feat(behavior_path_planner): change turn signal output timing (autowarefoundation#2493)

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

* feat(behavior_path_planner): remove unnecessary code and clean turn signal decider (autowarefoundation#2494)

* feat(behavior_path_planner): clean drivable area code

Signed-off-by: yutaka <purewater0901@gmail.com>

* make a function for turn signal decider

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

* feat(trajectory_visualizer): update for steer limit, remove tf for pose source (autowarefoundation#2267)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* feat(mission_planner): check goal footprint (autowarefoundation#2088)

Signed-off-by: ismet atabay <ismet@leodrive.ai>

* feat(behavior_path_planner): fix overlap checker (autowarefoundation#2498)

* feat(behavior_path_planner): fix overlap checker

Signed-off-by: yutaka <purewater0901@gmail.com>

* remove reserve

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

* fix(behavior_path_planner): fix find nearest function from lateral distance (autowarefoundation#2499)

* feat(behavior_path_planner): fix find nearest function from lateral distance

* empty commit

* fix(behavior_path_planner): fix planner data copy (autowarefoundation#2501)

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

* refactor(simple_planning_simulator): make function for duplicated code (autowarefoundation#2502)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix build error

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* ci(pre-commit): autofix

* revert intersection module change

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete wallmarker in intersection module

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* compare pose with resample_uitils

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete wallmarker in merge_from_private module

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete wallmarker in no_stopping_area

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* ci(pre-commit): autofix

* add dead line creator

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* fix base function

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete wall marker in each module

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* ci(pre-commit): autofix

* empty gurad

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* ci(pre-commit): autofix

* instantiation wall maeker creator

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* initialize pointer

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* ci(pre-commit): autofix

* define create_wall_function and delete_wall_function in VirtualWallMarkerCreator

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* change instance name

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Signed-off-by: Ambroise Vincent <ambroise.vincent@arm.com>
Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>
Signed-off-by: yutaka <purewater0901@gmail.com>
Signed-off-by: veqcc <ryuta.kambe@tier4.jp>
Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Signed-off-by: kminoda <koji.minoda@tier4.jp>
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>
Signed-off-by: takahoribe <horibe.takamasa@gmail.com>
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
Signed-off-by: NorahXiong <norah.xiong@autocore.ai>
Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>
Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp>
Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Signed-off-by: ismet atabay <ismet@leodrive.ai>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Co-authored-by: Ambroise Vincent <ambroise.vincent@arm.com>
Co-authored-by: Xinyu Wang <93699235+angry-crab@users.noreply.github.com>
Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com>
Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: Ryuta Kambe <veqcc.c@gmail.com>
Co-authored-by: Mingyu1991 <115005477+Mingyu1991@users.noreply.github.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com>
Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Co-authored-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com>
Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
Co-authored-by: NorahXiong <103234047+NorahXiong@users.noreply.github.com>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Co-authored-by: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com>
  • Loading branch information
1 parent e898097 commit 2e840b9
Show file tree
Hide file tree
Showing 22 changed files with 235 additions and 42 deletions.
57 changes: 51 additions & 6 deletions common/motion_utils/include/motion_utils/marker/marker_helper.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,23 +15,28 @@
#ifndef MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
#define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_

#include "motion_utils/resample/resample_utils.hpp"
#include "tier4_autoware_utils/tier4_autoware_utils.hpp"

#include <functional>
#include <string>
#include <vector>

namespace motion_utils
{
using geometry_msgs::msg::Pose;

visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
const int32_t id, const double longitudinal_offset = 0.0);
const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
const double longitudinal_offset = 0.0);

visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
const int32_t id, const double longitudinal_offset = 0.0);
const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
const double longitudinal_offset = 0.0);

visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
const int32_t id, const double longitudinal_offset = 0.0);
const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
const double longitudinal_offset = 0.0);

visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
const rclcpp::Time & now, const int32_t id);
Expand All @@ -41,6 +46,46 @@ visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker(

visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
const rclcpp::Time & now, const int32_t id);

visualization_msgs::msg::MarkerArray createVirtualWallMarkerFromPreviousPoses(
const std::vector<Pose> & stop_poses, const std::vector<Pose> & previous_poses,
const rclcpp::Time & now, int32_t id);

class VirtualWallMarkerCreator
{
public:
virtual ~VirtualWallMarkerCreator() = default;

using create_wall_function = std::function<visualization_msgs::msg::MarkerArray(
const geometry_msgs::msg::Pose & pose, const std::string & module_name,
const rclcpp::Time & now, const int32_t id, const double longitudinal_offset)>;

using delete_wall_function =
std::function<visualization_msgs::msg::MarkerArray(const rclcpp::Time & now, const int32_t id)>;

visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
int32_t id, const double longitudinal_offset = 0.0);

visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
const std::vector<Pose> & slow_down_poses, const std::string & module_name,
const rclcpp::Time & now, int32_t id, const double longitudinal_offset = 0.0);

visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
const std::vector<Pose> & dead_line_poses, const std::string & module_name,
const rclcpp::Time & now, int32_t id, const double longitudinal_offset = 0.0);

private:
visualization_msgs::msg::MarkerArray handleVirtualWallMarker(
const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
int32_t id, create_wall_function function_create_wall_marker,
delete_wall_function function_delete_wall_marker,
std::vector<geometry_msgs::msg::Pose> & previous_poses, const double longitudinal_offset = 0.0);

std::vector<Pose> previous_stop_poses_;
std::vector<Pose> previous_slow_down_poses_;
std::vector<Pose> previous_dead_line_poses_;
};
} // namespace motion_utils

#endif // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
78 changes: 78 additions & 0 deletions common/motion_utils/src/marker/marker_helper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,13 +16,16 @@

#include <string>

using tier4_autoware_utils::appendMarkerArray;
using tier4_autoware_utils::createDefaultMarker;
using tier4_autoware_utils::createDeletedDefaultMarker;
using tier4_autoware_utils::createMarkerColor;
using tier4_autoware_utils::createMarkerScale;
using visualization_msgs::msg::MarkerArray;

namespace
{

inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray(
const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name,
const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id,
Expand Down Expand Up @@ -128,4 +131,79 @@ visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
{
return createDeletedVirtualWallMarkerArray("dead_line_", now, id);
}

visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWallMarker(
const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
int32_t id, create_wall_function function_create_wall_marker,
delete_wall_function function_delete_wall_marker,
std::vector<geometry_msgs::msg::Pose> & previous_virtual_wall_poses,
const double longitudinal_offset)
{
size_t id_to_create = id;
size_t id_to_delete = id;
visualization_msgs::msg::MarkerArray wall_marker;

if (poses.size() == 0 || previous_virtual_wall_poses.empty()) {
return wall_marker;
}

for (const auto & p : previous_virtual_wall_poses) {
const bool previous_stop_pose_is_in_stop_pose =
std::any_of(poses.begin(), poses.end(), [&](const geometry_msgs::msg::Pose & elem) {
std::vector<Pose> poses;
poses.push_back(p);
poses.push_back(elem);
return resample_utils::validate_points_duplication(poses);
});

if (!previous_stop_pose_is_in_stop_pose) {
appendMarkerArray(function_delete_wall_marker(now, id_to_delete), &wall_marker, now);
}
id_to_delete++;
}

for (const auto & p : poses) {
appendMarkerArray(
function_create_wall_marker(p, module_name, now, id_to_create++, longitudinal_offset),
&wall_marker);
}
previous_virtual_wall_poses = poses;
return wall_marker;
}

visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createStopVirtualWallMarker(
const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
int32_t id, const double longitudinal_offset)
{
create_wall_function creator = motion_utils::createStopVirtualWallMarker;
delete_wall_function deleter = motion_utils::createDeletedStopVirtualWallMarker;

return handleVirtualWallMarker(
stop_poses, module_name, now, id, creator, deleter, previous_stop_poses_, longitudinal_offset);
}

visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createSlowDownVirtualWallMarker(
const std::vector<Pose> & slow_down_poses, const std::string & module_name,
const rclcpp::Time & now, int32_t id, const double longitudinal_offset)
{
create_wall_function creator = motion_utils::createSlowDownVirtualWallMarker;
delete_wall_function deleter = motion_utils::createDeletedSlowDownVirtualWallMarker;

return handleVirtualWallMarker(
slow_down_poses, module_name, now, id, creator, deleter, previous_slow_down_poses_,
longitudinal_offset);
}

visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createDeadLineVirtualWallMarker(
const std::vector<Pose> & dead_line_poses, const std::string & module_name,
const rclcpp::Time & now, int32_t id, const double longitudinal_offset)
{
create_wall_function creator = motion_utils::createDeadLineVirtualWallMarker;
delete_wall_function deleter = motion_utils::createDeletedDeadLineVirtualWallMarker;

return handleVirtualWallMarker(
dead_line_poses, module_name, now, id, creator, deleter, previous_dead_line_poses_,
longitudinal_offset);
}

} // namespace motion_utils
Original file line number Diff line number Diff line change
Expand Up @@ -217,6 +217,9 @@ class BlindSpotModule : public SceneModuleInterface

// Debug
mutable DebugData debug_data_;

std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
std::make_shared<motion_utils::VirtualWallMarkerCreator>();
};
} // namespace behavior_velocity_planner

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
Expand Down Expand Up @@ -174,6 +175,9 @@ class CrosswalkModule : public SceneModuleInterface

// whether ego passed safety_slow_point
bool passed_safety_slow_point_;

std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_crosswalk_ =
std::make_shared<motion_utils::VirtualWallMarkerCreator>();
};
} // namespace behavior_velocity_planner

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_routing/RoutingGraphContainer.h>

#include <memory>
#include <utility>
#include <vector>

Expand Down Expand Up @@ -72,6 +73,9 @@ class WalkwayModule : public SceneModuleInterface

// Debug
DebugData debug_data_;

std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_walkway_ =
std::make_shared<motion_utils::VirtualWallMarkerCreator>();
};
} // namespace behavior_velocity_planner

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,9 @@ class DetectionAreaModule : public SceneModuleInterface

// Debug
DebugData debug_data_;

std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
std::make_shared<motion_utils::VirtualWallMarkerCreator>();
};
} // namespace behavior_velocity_planner

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#ifndef SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_
#define SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_

#include <motion_utils/motion_utils.hpp>
#include <rclcpp/rclcpp.hpp>
#include <scene_module/scene_module_interface.hpp>
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
Expand Down Expand Up @@ -256,6 +257,9 @@ class IntersectionModule : public SceneModuleInterface

// Debug
mutable DebugData debug_data_;

std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
std::make_shared<motion_utils::VirtualWallMarkerCreator>();
};
} // namespace behavior_velocity_planner

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,9 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface

// Debug
mutable DebugData debug_data_;

std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
std::make_shared<motion_utils::VirtualWallMarkerCreator>();
};
} // namespace behavior_velocity_planner

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -174,6 +174,9 @@ class NoStoppingAreaModule : public SceneModuleInterface

// Debug
DebugData debug_data_;

std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
std::make_shared<motion_utils::VirtualWallMarkerCreator>();
};
} // namespace behavior_velocity_planner

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,9 @@ class OcclusionSpotModule : public SceneModuleInterface

// Debug
mutable DebugData debug_data_;

std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
std::make_shared<motion_utils::VirtualWallMarkerCreator>();
};
} // namespace behavior_velocity_planner

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include "scene_module/run_out/dynamic_obstacle.hpp"

#include <memory>
#include <string>
#include <vector>
namespace behavior_velocity_planner
Expand Down Expand Up @@ -130,6 +131,8 @@ class RunOutDebug
DebugValues debug_values_;
AccelReason accel_reason_;
double height_{0};
std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
std::make_shared<motion_utils::VirtualWallMarkerCreator>();
};

} // namespace behavior_velocity_planner
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,9 @@ class StopLineModule : public SceneModuleInterface

// Debug
DebugData debug_data_;

std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
std::make_shared<motion_utils::VirtualWallMarkerCreator>();
};
} // namespace behavior_velocity_planner

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,8 @@ class VirtualTrafficLightModule : public SceneModuleInterface
tier4_v2x_msgs::msg::InfrastructureCommand command_;
MapData map_data_;
ModuleData module_data_;
std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
std::make_shared<motion_utils::VirtualWallMarkerCreator>();

void updateInfrastructureCommand();

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -78,8 +78,8 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createVirtualWallMarkerArr

if (!isActivated() && !is_over_pass_judge_line_) {
appendMarkerArray(
motion_utils::createStopVirtualWallMarker(
debug_data_.virtual_wall_pose, "blind_spot", now, module_id_),
virtual_wall_marker_creator_->createStopVirtualWallMarker(
{debug_data_.virtual_wall_pose}, "blind_spot", now, module_id_),
&wall_marker, now);
}
return wall_marker;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -223,17 +223,26 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createVirtualWallMarkerArr
{
const auto now = this->clock_->now();
auto id = module_id_;
std::vector<Pose> stop_poses;
std::vector<Pose> slow_down_poses;

visualization_msgs::msg::MarkerArray wall_marker;
for (const auto & p : debug_data_.stop_poses) {
const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0);
appendMarkerArray(createStopVirtualWallMarker(p_front, "crosswalk", now, id++), &wall_marker);
stop_poses.push_back(p_front);
}
appendMarkerArray(
virtual_wall_marker_creator_crosswalk_->createStopVirtualWallMarker(
stop_poses, "crosswalk", now, id),
&wall_marker);
for (const auto & p : debug_data_.slow_poses) {
const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0);
appendMarkerArray(
createSlowDownVirtualWallMarker(p_front, "crosswalk", now, id++), &wall_marker);
slow_down_poses.push_back(p_front);
}
appendMarkerArray(
virtual_wall_marker_creator_crosswalk_->createSlowDownVirtualWallMarker(
slow_down_poses, "crosswalk", now, id),
&wall_marker);

return wall_marker;
}
Expand All @@ -242,12 +251,17 @@ visualization_msgs::msg::MarkerArray WalkwayModule::createVirtualWallMarkerArray
{
const auto now = this->clock_->now();
auto id = module_id_;
std::vector<Pose> stop_poses;

visualization_msgs::msg::MarkerArray wall_marker;
for (const auto & p : debug_data_.stop_poses) {
const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0);
appendMarkerArray(createStopVirtualWallMarker(p_front, "walkway", now, id++), &wall_marker);
stop_poses.push_back(p_front);
}
appendMarkerArray(
virtual_wall_marker_creator_walkway_->createStopVirtualWallMarker(
stop_poses, "walkway", now, id),
&wall_marker);
return wall_marker;
}

Expand Down
Loading

0 comments on commit 2e840b9

Please sign in to comment.