Skip to content

Commit

Permalink
Squashed commit of the following:
Browse files Browse the repository at this point in the history 
commit c7d3b7d
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 (autowarefoundation#2504)

    Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

    Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

commit 6731e0c
Author: kminoda <44218668+kminoda@users.noreply.github.com>
Date:   Fri Dec 16 09:29:35 2022 +0900

    feat(pose_initializer): partial map loading (autowarefoundation#2500)

    * first commit

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

    * move function

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

    * now works

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

    * ci(pre-commit): autofix

    * update readme

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

    * ci(pre-commit): autofix

    * clarify how to enable partial mao loading interface

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

    * ci(pre-commit): autofix

    * update readme

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

    * 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 <koji.minoda@tier4.jp>

    Signed-off-by: kminoda <koji.minoda@tier4.jp>
    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 efb4ff1
Author: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date:   Thu Dec 15 17:29:44 2022 +0900

    feat(trajectory_follower): extend mpc trajectory for terminal yaw (autowarefoundation#2447)

    * feat(trajectory_follower): extend mpc trajectory for terminal yaw

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

    * make mpc min vel param

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

    * add mpc extended point after smoothing

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

    * Revert "make mpc min vel param"

    This reverts commit 02157b6.

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

    * add comment and hypot

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

    * remove min vel

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

    * add flag for extending traj

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

    * add extend param to default param

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

    * fix typo

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

    * fix from TakaHoribe review

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

    * fix typo

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

    * refactor

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

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

commit ad2ae78
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  (autowarefoundation#2503)

    * feat(component_interface_utils): add service log checker

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

    * feat(component_interface_tools): add service log checker

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

    * feat(component_interface_tools): add diagnostics

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

    * feat: update system error monitor config

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

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

commit 4a13cc5
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 (autowarefoundation#2508)

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

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

commit 77b1c36
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 (autowarefoundation#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 <zulfaqar.azmi@tier4.jp>

    * 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 <zulfaqar.azmi@tier4.jp>
    Co-authored-by: tanaka3 <ttatcoder@outlook.jp>
    Co-authored-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
    Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

commit 9183c4f
Author: Takamasa Horibe <horibe.takamasa@gmail.com>
Date:   Wed Dec 14 19:59:00 2022 +0900

    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>

commit ed992b1
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 (autowarefoundation#2501)

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

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

commit 0c6c46b
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 (autowarefoundation#2499)

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

    * empty commit

commit a26b69d
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 (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>

commit 3a24859
Author: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com>
Date:   Tue Dec 13 16:51:59 2022 +0300

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

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

commit b6a1885
Author: Takamasa Horibe <horibe.takamasa@gmail.com>
Date:   Tue Dec 13 22:46:24 2022 +0900

    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>

commit f1a9a96
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 (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>

commit fafe1d8
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 (autowarefoundation#2493)

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

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

commit c48b9cf
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 (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>

commit 9a3613b
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 (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>

commit 49aa10b
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 (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>

commit 7f0138c
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 (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>

commit c855e23
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 (autowarefoundation#2403)

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

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

commit fa04d54
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 (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>

commit 4a6990c
Author: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date:   Mon Dec 12 15:01:39 2022 +0900

    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>

commit 3c01f15
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 (autowarefoundation#2475)

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

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

commit d8ece00
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 (autowarefoundation#2483)

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

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

commit 727586b
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 (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>

commit 284548c
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 (autowarefoundation#2413)

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

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

commit 469d892
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 (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>

commit d924f85
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 (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>

commit b2ded82
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 (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>

commit 8906a1e
Author: Takamasa Horibe <horibe.takamasa@gmail.com>
Date:   Fri Dec 9 16:29:45 2022 +0900

    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>

commit d493905
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 (autowarefoundation#2480)

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

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

commit d3b86a3
Author: Makoto Kurihara <mkuri8m@gmail.com>
Date:   Thu Dec 8 22:59:32 2022 +0900

    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>

commit 2dde073
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 (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>

commit 9a5057e
Author: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date:   Thu Dec 8 13:42:50 2022 +0900

    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>

commit cddb8c7
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 (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>

commit f0f513c
Author: kminoda <44218668+kminoda@users.noreply.github.com>
Date:   Thu Dec 8 11:08:29 2022 +0900

    fix: remove unnecessary DEBUG_INFO declarations (autowarefoundation#2457)

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

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

commit 01daebf
Author: Takayuki Murooka <takayuki5168@gmail.com>
Date:   Thu Dec 8 00:28:35 2022 +0900

    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>

commit 26ef817
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` (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>

commit 024b993
Author: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com>
Date:   Wed Dec 7 18:00:32 2022 +0900

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

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

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

commit c240ce2
Author: Yukihiro Saito <yukky.saito@gmail.com>
Date:   Wed Dec 7 16:33:44 2022 +0900

    feat: remove web controller (autowarefoundation#2405)

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

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

commit 2992b1c
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 (autowarefoundation#2465)

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

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

commit 4a75d7c
Author: Mingyu1991 <115005477+Mingyu1991@users.noreply.github.com>
Date:   Wed Dec 7 14:42:33 2022 +0900

    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>

commit a428716
Author: Ryuta Kambe <veqcc.c@gmail.com>
Date:   Wed Dec 7 12:21:49 2022 +0900

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

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

commit 0a5b285
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 (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>

commit 65003dc
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 (autowarefoundation#2407) (autowarefoundation#2460)

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

    This reverts commit c422485.

commit fab1867
Author: Makoto Kurihara <mkuri8m@gmail.com>
Date:   Wed Dec 7 10:32:41 2022 +0900

    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>

commit a1d3c80
Author: Ambroise Vincent <ambroise.vincent@arm.com>
Date:   Tue Dec 6 10:39:02 2022 +0100

    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>

commit eb99468
Author: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date:   Tue Dec 6 18:11:41 2022 +0900

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

    changed logging level

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

commit c422485
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 (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>

commit f984fbb
Author: Takamasa Horibe <horibe.takamasa@gmail.com>
Date:   Tue Dec 6 13:55:17 2022 +0900

    feat(transition_manager): add param to ignore autonomous transition condition (autowarefoundation#2453)

    * feat(transition_manager): add param to ignore autonomous transition condition

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

    * same for modeChangeCompleted

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

    * remove debug print

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

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

commit d3e640d
Author: Tomoya Kimura <tomoya.kimura@tier4.jp>
Date:   Tue Dec 6 13:01:06 2022 +0900

    feat(operation_mode_transition_manager): transition to auto quickly when vehicle stops (autowarefoundation#2427)

    Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

    Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
  • Loading branch information
@HansRobo
HansRobo committed Dec 16, 2022
1 parent 79d0719 commit f5b5ade
Show file tree
Hide file tree
Showing 204 changed files with 6,211 additions and 3,569 deletions.
7 changes: 7 additions & 0 deletions common/component_interface_tools/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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)
3 changes: 3 additions & 0 deletions common/component_interface_tools/launch/service_log_checker.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
<launch>
<node pkg="component_interface_tools" exec="service_log_checker" name="service_log_checker"/>
</launch>
28 changes: 28 additions & 0 deletions common/component_interface_tools/package.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>component_interface_tools</name>
<version>0.1.0</version>
<description>The component_interface_tools package</description>
<maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
<maintainer email="makoto.yabuta@tier4.jp">yabuta</maintainer>
<maintainer email="kahhooi.tan@tier4.jp">Kah Hooi Tan</maintainer>
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake_auto</buildtool_depend>

<build_depend>autoware_cmake</build_depend>

<depend>diagnostic_updater</depend>
<depend>fmt</depend>
<depend>rclcpp</depend>
<depend>tier4_system_msgs</depend>
<depend>yaml_cpp_vendor</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
110 changes: 110 additions & 0 deletions common/component_interface_tools/src/service_log_checker.cpp
View file
Original file line number Diff line number Diff line change
@@ -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 <yaml-cpp/yaml.h>

#include <memory>
#include <string>

#define FMT_HEADER_ONLY
#include <fmt/format.h>

ServiceLogChecker::ServiceLogChecker() : Node("service_log_checker"), diagnostics_(this)
{
sub_ = create_subscription<ServiceLog>(
"/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<bool>();
if (!success) {
const auto message = status["message"].as<std::string>();
const auto code = status["code"].as<uint16_t>();
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<ServiceLogChecker>();
executor.add_node(node);
executor.spin();
executor.remove_node(node);
rclcpp::shutdown();
}
42 changes: 42 additions & 0 deletions common/component_interface_tools/src/service_log_checker.hpp
View file
Original file line number Diff line number Diff line change
@@ -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 <diagnostic_updater/diagnostic_updater.hpp>
#include <rclcpp/rclcpp.hpp>

#include <tier4_system_msgs/msg/service_log.hpp>

#include <string>
#include <unordered_map>

class ServiceLogChecker : public rclcpp::Node
{
public:
ServiceLogChecker();

private:
using ServiceLog = tier4_system_msgs::msg::ServiceLog;
rclcpp::Subscription<ServiceLog>::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<std::string, std::string> errors_;
};

#endif // SERVICE_LOG_CHECKER_HPP_
1 change: 0 additions & 1 deletion common/component_interface_utils/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,5 +3,4 @@ project(component_interface_utils)

find_package(autoware_cmake REQUIRED)
autoware_package()
ament_export_dependencies(tier4_system_msgs)
ament_auto_package()
1 change: 1 addition & 0 deletions common/component_interface_utils/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>

<build_depend>autoware_cmake</build_depend>
<build_depend>tier4_system_msgs</build_depend>

<depend>autoware_adapi_v1_msgs</depend>
<depend>rclcpp</depend>
Expand Down
38 changes: 37 additions & 1 deletion common/motion_utils/include/motion_utils/resample/resample.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<geometry_msgs::msg::Point> resamplePointVector(
const std::vector<geometry_msgs::msg::Point> & points,
const std::vector<double> & 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<geometry_msgs::msg::Point> resamplePointVector(
const std::vector<geometry_msgs::msg::Point> & 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
Expand Down Expand Up @@ -70,7 +106,7 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
* @return resampled path(poses)
*/
std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
const std::vector<geometry_msgs::msg::Pose> & points, const double resampled_interval,
const std::vector<geometry_msgs::msg::Pose> & points, const double resample_interval,
const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true);

/**
Expand Down
90 changes: 74 additions & 16 deletions common/motion_utils/src/resample/resample.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@

namespace motion_utils
{
std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
const std::vector<geometry_msgs::msg::Pose> & points,
std::vector<geometry_msgs::msg::Point> resamplePointVector(
const std::vector<geometry_msgs::msg::Point> & points,
const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy,
const bool use_lerp_for_z)
{
Expand All @@ -40,17 +40,17 @@ std::vector<geometry_msgs::msg::Pose> 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
Expand All @@ -65,15 +65,71 @@ std::vector<geometry_msgs::msg::Pose> 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<geometry_msgs::msg::Pose> resampled_points;
std::vector<geometry_msgs::msg::Point> 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<geometry_msgs::msg::Point> resamplePointVector(
const std::vector<geometry_msgs::msg::Point> & 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<double> 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<geometry_msgs::msg::Pose> resamplePoseVector(
const std::vector<geometry_msgs::msg::Pose> & points,
const std::vector<double> & 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<geometry_msgs::msg::Point> 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<geometry_msgs::msg::Pose> 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;
}

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
Loading

0 comments on commit f5b5ade

Please sign in to comment.