From 25f85e8f73212d9c3ce6dd4200e081e855119dc7 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 26 Sep 2022 10:38:59 +0900 Subject: [PATCH 01/38] chore: sync files (#1949) Signed-off-by: GitHub Signed-off-by: GitHub Co-authored-by: kenji-miyake --- .github/workflows/build-and-test-differential.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index f8db906263843..97c9c4495bb02 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -80,7 +80,7 @@ jobs: - name: Get modified files id: get-modified-files - uses: tj-actions/changed-files@v29 + uses: tj-actions/changed-files@v31 with: files: | **/*.cpp From 8ac5a3f11e1d9f888ec830c3bb170d3e226337c4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 26 Sep 2022 10:42:32 +0900 Subject: [PATCH 02/38] fix(trajectory_follower): fixed stop distance calculation in transitioning to emergency_stop (#1915) Signed-off-by: Mamoru Sobue --- ...udinalControllerStateTransition.drawio.svg | 352 +----------------- .../src/pid_longitudinal_controller.cpp | 9 +- 2 files changed, 11 insertions(+), 350 deletions(-) diff --git a/control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg b/control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg index 94706f229733e..e391161723048 100644 --- a/control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg +++ b/control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg @@ -1,348 +1,4 @@ - - - - - - - - - - - - -
-
-
- - - 0. DRIVE - - -
-
-
- - v = interpolateTargetVel() - -
-
- - a = applyVelocityFeedback() - -
-
-
-
- - - 0. DRIVE... - - - - - - - - - - - -
-
-
- - - 1. STOPPING - -
- -
-
- - v = stopped_vel_ - -
-
- - a = smooth_stop_.calculate() - -
-
-
-
- - - 1. STOPPING... - - - - - - - - - -
-
-
- - 2. STOPPED -
- -
-
- - v = stopped_vel_ - -
-
- - a = stopped_acc_ - -
-
-
-
- - - 2. STOPPED... - - - - - - - - - -
-
-
- - 3. EMERGENCY -
- -
-
- - v = emergency_vel_ - -
-
- - a = emergency_acc_ - -
-
-
-
- - - 3. EMERGENCY... - - - - - - - -
-
-
- stop condition -
-
-
- - - stop condition - - - - - - - -
-
-
-

- - strict departure condition - - : Distance to stop is larger than A1. -
- - loose departure condition - - : Distance to stop is larger than A1 + A2. -
- - stopping condition - - : Distance to stop is smaller than B1. -
- - stop condition - - : For 500 [ms], self velocity is smaller than C1, and self acceleration is smaller than C2. -
- - emergency condition - - - : Distance to stop is smaller than D1, or self pose is not within D2, D3 from trajectory. - -

-
-
-
- - - strict departure condition : Distance to stop is larger than... - - - - - - - -
-
-
- strict departure -
- condition -
-
-
- - - strict departure... - - - - - - - -
-
-
- stopping -
- condition -
-
-
- - - stopping... - - - - - - - -
-
-
- loose departure -
- condition -
-
-
- - - loose departure... - - - - - - - -
-
-
- emergency condition -
-
-
- - - emergency con... - - - - - - - -
-
-
- - stop condition - -
- and -
- not - - emergency condition - -
-
-
- - - stop condition... - - - - - - - -
-
-
-

- A1 : drive_state_stop_dist -
- A2 : drive_state_offset_stop_dist -
- B1 : stopping_state_stop_dist -
- C1 : stopped_state_entry_vel -
- C2 : stopped_state_entry_acc -
- D1 : emergency_state_overshoot_stop_dist -
- D2 : emergency_state_traj_trans_dev -
- D3 : emergency_state_traj_rot_dev -

-
-
-
- - - A1 : drive_state_stop_dist... - - - - - - - - - Viewer does not support full SVG 1.1 - - - - + + + +
0. DRIVE

v = interpolateTargetVel()
a = applyVelocityFeedback() 
0. DRIVE...
1. STOPPING

v = stopped_vel_
a = smooth_stop_.calculate()
1. STOPPING...
2. STOPPED

v = stopped_vel_
a = stopped_acc_
2. STOPPED...
3. EMERGENCY

v = emergency_vel_
a = emergency_acc_
3. EMERGENCY...
stop condition
stop condition

strict departure condition : Distance to stop is larger than A1.
loose departure condition : Distance to stop is larger than A1 + A2.
stopping condition : Distance to stop is smaller than B1.
stop condition : For 500 [ms], self velocity is smaller than C1, and self acceleration is smaller than C2.
emergency condition : Speed command is zero velocity, and distance to stop is smaller than D1, or self pose is not within D2, D3 from trajectory.

strict departure condition : Distance to stop is larger than...
strict departure
condition
strict departure...
stopping
condition
stopping...
loose departure
condition
loose departure...
emergency condition
emergency con...
stop condition
and
not emergency condition
stop condition...

A1 : drive_state_stop_dist
A2 : drive_state_offset_stop_dist
B1 : stopping_state_stop_dist
C1 : stopped_state_entry_vel
C2 : stopped_state_entry_acc
D1 : emergency_state_overshoot_stop_dist
D2 : emergency_state_traj_trans_dev
D3 : emergency_state_traj_rot_dev

A1 : drive_state_stop_dist...Text is not SVG - cannot display \ No newline at end of file diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index 53c3f71fc11bd..471dccebe26ba 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -507,8 +507,13 @@ PidLongitudinalController::ControlState PidLongitudinalController::updateControl ? (node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time : false; - const bool8_t emergency_condition = - m_enable_overshoot_emergency && stop_dist < -p.emergency_state_overshoot_stop_dist; + static constexpr double vel_epsilon = + 1e-3; // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity + const float64_t current_vel_cmd = + std::fabs(m_trajectory_ptr->points.at(control_data.nearest_idx).longitudinal_velocity_mps); + const bool8_t emergency_condition = m_enable_overshoot_emergency && + stop_dist < -p.emergency_state_overshoot_stop_dist && + current_vel_cmd < vel_epsilon; // transit state if (current_control_state == ControlState::DRIVE) { From 6dda23b897b7563831a219315549da857d60811a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 26 Sep 2022 11:10:16 +0900 Subject: [PATCH 03/38] chore(behavior_path_planner): add code owner (#1952) Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- planning/behavior_path_planner/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 54bfa019ee0aa..51f1ea57ba6bd 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -15,6 +15,7 @@ Fumiya Watanabe Takamasa Horibe + Satoshi Ota Yutaka Shimizu @@ -28,6 +29,7 @@ Apache License 2.0 Takamasa Horibe + Satoshi Ota Fumiya Watanabe Zulfaqar Azmi Kosuke Takeuchi From 78c66380b7b322e08fb1ac3823e1bd2a1985dc0e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 26 Sep 2022 12:48:57 +0900 Subject: [PATCH 04/38] fix(behavior_path_planner): fix shift pull out (#1946) * fix(behavior_path_planner): fix shift pull out Signed-off-by: kosuke55 * fix typo Signed-off-by: kosuke55 * fix typo Signed-off-by: kosuke55 * precommmit Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../src/scene_module/pull_out/geometric_pull_out.cpp | 4 ++-- .../src/scene_module/pull_out/shift_pull_out.cpp | 4 ++-- .../src/scene_module/utils/path_shifter.cpp | 5 +++++ 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp index eec8731bbda4e..a668deaac9023 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp @@ -36,7 +36,7 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p { PullOutPath output; - // conbine road lane and shoulder lane + // combine road lane and shoulder lane const auto road_lanes = util::getCurrentLanes(planner_data_); const auto shoulder_lanes = getPullOutLanes(road_lanes, planner_data_); auto lanes = road_lanes; @@ -50,7 +50,7 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p return {}; } - // collsion check with objects in shoulder lanes + // collision check with objects in shoulder lanes const auto arc_path = planner_.getArcPath(); const auto shoulder_lane_objects = util::filterObjectsByLanelets(*(planner_data_->dynamic_object), shoulder_lanes); 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 7371ad99fb80d..50de3ba11cb97 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 @@ -69,7 +69,7 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) auto & shift_path = pull_out_path.partial_paths.front(); // shift path is not separate but only one. - // check lane_depature and collsion with path between current to pull_out_end + // check lane_departure and collision with path between current to pull_out_end PathWithLaneId path_current_to_shift_end; { const auto current_idx = findNearestIndex(shift_path.points, current_pose); @@ -167,6 +167,7 @@ std::vector ShiftPullOut::calcPullOutPaths( double s_end = arc_position_goal.length; road_lane_reference_path = route_handler.getCenterLinePath(road_lanes, s_start, s_end); } + path_shifter.setPath(road_lane_reference_path); // get shift point start/end const auto shift_point_start = shoulder_reference_path.points.back(); @@ -186,7 +187,6 @@ std::vector ShiftPullOut::calcPullOutPaths( shift_point.length = distance_to_road_center; } path_shifter.addShiftPoint(shift_point); - path_shifter.setPath(road_lane_reference_path); // offset front side ShiftedPath shifted_path; diff --git a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp index c2b4a4e7f9afc..c9e4639c75d80 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp @@ -259,6 +259,11 @@ std::vector PathShifter::calcLateralJerk() const void PathShifter::updateShiftPointIndices(ShiftPointArray & shift_points) const { + if (reference_path_.points.empty()) { + RCLCPP_ERROR( + logger_, "reference path is empty, setPath is needed before addShiftPoint/setShiftPoints."); + } + for (auto & p : shift_points) { // TODO(murooka) remove findNearestIndex for except // lane_following to support u-turn & crossing path From dd190a35f73c9ddd2b9b3a2533add63ae705ab85 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Mon, 26 Sep 2022 15:53:42 +0900 Subject: [PATCH 05/38] chore(perception_util): add maintainer (#1951) Signed-off-by: scepter914 Signed-off-by: scepter914 --- common/perception_utils/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 79d08bc0ed15a..5d1fa5cae84a6 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -6,6 +6,8 @@ The perception_utils package Takayuki Murooka Satoshi Tanaka + Yusuke Muramatsu + Shunsuke Miura Apache License 2.0 ament_cmake_auto From ee790c593f07a72d4740041350e64dd092dcd524 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 26 Sep 2022 20:39:48 +0900 Subject: [PATCH 06/38] chore(tier4_planning_launch): add maintainers (#1955) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- launch/tier4_planning_launch/package.xml | 43 ++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index f9cbe4541961e..96b5d05c21d35 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -4,7 +4,50 @@ tier4_planning_launch 0.1.0 The tier4_planning_launch package + + + Zulfaqar Azmi + + Kosuke Takeuchi + + Kosuke Takeuchi + + Fumiya Watanabe + Takamasa Horibe + Satoshi Ota + + Yutaka Shimizu + + Takayuki Murooka + + + + Mamoru Sobue + + Satoshi Ota + + Kyoichi Sugahara + + Taiki Tanaka + + Kosuke Takeuchi + + Satoshi Ota + + Mamoru Sobue + + Yutaka Shimizu + + Kosuke Takeuchi + + Tomohito Ando + + + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + Apache License 2.0 ament_cmake_auto From dcca4cf987d3c8c1b0d97359938f80b412e35d82 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 26 Sep 2022 11:46:26 +0000 Subject: [PATCH 07/38] chore: update CODEOWNERS (#1943) Signed-off-by: GitHub Signed-off-by: GitHub Co-authored-by: kenji-miyake --- .github/CODEOWNERS | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 90f0aa3ef40fe..473405dbdcfc3 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -23,7 +23,7 @@ common/motion_utils/** yutaka.shimizu@tier4.jp satoshi.ota@tier4.jp takayuki.mur common/neural_networks_provider/** ambroise.vincent@arm.com common/osqp_interface/** maxime.clement@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp -common/perception_utils/** takayuki.murooka@tier4.jp +common/perception_utils/** takayuki.murooka@tier4.jp satoshi.tanaka@tier4.jp yusuke.muramatsu@tier4.jp shunsuke.miura@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/signal_processing/** takayuki.murooka@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp @@ -63,7 +63,7 @@ launch/tier4_control_launch/** takamasa.horibe@tier4.jp launch/tier4_localization_launch/** yamato.ando@tier4.jp launch/tier4_map_launch/** ryohsuke.mitsudome@tier4.jp launch/tier4_perception_launch/** yukihiro.saito@tier4.jp -launch/tier4_planning_launch/** takamasa.horibe@tier4.jp +launch/tier4_planning_launch/** zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp satoshi.ota@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp mamoru.sobue@tier4.jp yutaka.shimizu@tier4.jp kosuke.takeuchi@tier4.jp tomohito.ando@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp launch/tier4_system_launch/** kenji.miyake@tier4.jp fumihito.ito@tier4.jp @@ -108,7 +108,7 @@ perception/traffic_light_classifier/** yukihiro.saito@tier4.jp perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp -planning/behavior_path_planner/** zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp +planning/behavior_path_planner/** zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp satoshi.ota@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp planning/behavior_velocity_planner/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp mamoru.sobue@tier4.jp yutaka.shimizu@tier4.jp kosuke.takeuchi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp planning/costmap_generator/** kenji.miyake@tier4.jp planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp From 205522293660fa43862422a0955b1a4bbab6142b Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 26 Sep 2022 20:50:45 +0900 Subject: [PATCH 08/38] chore: update CODEOWNERS (#1957) Signed-off-by: GitHub Signed-off-by: GitHub Co-authored-by: kenji-miyake --- .github/CODEOWNERS | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 473405dbdcfc3..dcc03b291d1d8 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -19,15 +19,15 @@ common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/kalman_filter/** yukihiro.saito@tier4.jp common/motion_common/** christopherj.ho@gmail.com common/motion_testing/** christopherj.ho@gmail.com -common/motion_utils/** yutaka.shimizu@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp taiki.tanaka@tier4.jp +common/motion_utils/** satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp common/neural_networks_provider/** ambroise.vincent@arm.com common/osqp_interface/** maxime.clement@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp -common/perception_utils/** takayuki.murooka@tier4.jp satoshi.tanaka@tier4.jp yusuke.muramatsu@tier4.jp shunsuke.miura@tier4.jp +common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/signal_processing/** takayuki.murooka@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp -common/tier4_autoware_utils/** takamasa.horibe@tier4.jp kenji.miyake@tier4.jp +common/tier4_autoware_utils/** kenji.miyake@tier4.jp takamasa.horibe@tier4.jp common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp @@ -63,12 +63,12 @@ launch/tier4_control_launch/** takamasa.horibe@tier4.jp launch/tier4_localization_launch/** yamato.ando@tier4.jp launch/tier4_map_launch/** ryohsuke.mitsudome@tier4.jp launch/tier4_perception_launch/** yukihiro.saito@tier4.jp -launch/tier4_planning_launch/** zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp satoshi.ota@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp mamoru.sobue@tier4.jp yutaka.shimizu@tier4.jp kosuke.takeuchi@tier4.jp tomohito.ando@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp +launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp -launch/tier4_system_launch/** kenji.miyake@tier4.jp fumihito.ito@tier4.jp +launch/tier4_system_launch/** fumihito.ito@tier4.jp kenji.miyake@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp -localization/ekf_localizer/** takamasa.horibe@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp +localization/ekf_localizer/** koji.minoda@tier4.jp takamasa.horibe@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** yamato.ando@tier4.jp localization/initial_pose_button_panel/** yamato.ando@tier4.jp localization/localization_error_monitor/** yamato.ando@tier4.jp @@ -76,10 +76,10 @@ localization/ndt/** yamato.ando@tier4.jp localization/ndt_pcl_modified/** yamato.ando@tier4.jp localization/ndt_scan_matcher/** yamato.ando@tier4.jp localization/pose2twist/** yamato.ando@tier4.jp -localization/pose_initializer/** yamato.ando@tier4.jp isamu.takagi@tier4.jp +localization/pose_initializer/** isamu.takagi@tier4.jp yamato.ando@tier4.jp localization/stop_filter/** koji.minoda@tier4.jp yamato.ando@tier4.jp localization/twist2accel/** koji.minoda@tier4.jp yamato.ando@tier4.jp -map/map_loader/** ryohsuke.mitsudome@tier4.jp kenji.miyake@tier4.jp +map/map_loader/** kenji.miyake@tier4.jp ryohsuke.mitsudome@tier4.jp map/map_tf_generator/** azumi.suzuki@tier4.jp map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp perception/compare_map_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp @@ -96,7 +96,7 @@ perception/image_projection_based_fusion/** yukihiro.saito@tier4.jp yusuke.muram perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp perception/lidar_centerpoint/** yusuke.muramatsu@tier4.jp perception/map_based_prediction/** yutaka.shimizu@tier4.jp -perception/multi_object_tracker/** yukihiro.saito@tier4.jp jilada.eccleston@tier4.jp +perception/multi_object_tracker/** jilada.eccleston@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** yukihiro.saito@tier4.jp perception/object_range_splitter/** yukihiro.saito@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp @@ -108,8 +108,8 @@ perception/traffic_light_classifier/** yukihiro.saito@tier4.jp perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp -planning/behavior_path_planner/** zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp satoshi.ota@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp -planning/behavior_velocity_planner/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp mamoru.sobue@tier4.jp yutaka.shimizu@tier4.jp kosuke.takeuchi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp +planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp planning/costmap_generator/** kenji.miyake@tier4.jp planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp planning/freespace_planner/** takamasa.horibe@tier4.jp @@ -119,7 +119,7 @@ planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp planning/obstacle_avoidance_planner/** takayuki.murooka@tier4.jp planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp planning/obstacle_stop_planner/** satoshi.ota@tier4.jp -planning/planning_debug_tools/** takamasa.horibe@tier4.jp taiki.tanaka@tier4.jp +planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp planning/planning_error_monitor/** yutaka.shimizu@tier4.jp planning/planning_evaluator/** maxime.clement@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp @@ -148,7 +148,7 @@ system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.j system/dummy_diag_publisher/** kenji.miyake@tier4.jp system/dummy_infrastructure/** kenji.miyake@tier4.jp system/emergency_handler/** kenji.miyake@tier4.jp -system/system_error_monitor/** kenji.miyake@tier4.jp fumihito.ito@tier4.jp +system/system_error_monitor/** fumihito.ito@tier4.jp kenji.miyake@tier4.jp system/system_monitor/** fumihito.ito@tier4.jp system/topic_state_monitor/** kenji.miyake@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp @@ -157,4 +157,4 @@ vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** tomoya.kimura@t vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** takamasa.horibe@tier4.jp vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp -vehicle/vehicle_info_util/** yamato.ando@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp +vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp yamato.ando@tier4.jp From 272af54df771265ad96bddc95bf4c19d3918b4d4 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 26 Sep 2022 20:51:39 +0900 Subject: [PATCH 09/38] feat(behavior_path_planner): add pull_over base class (#1911) * feat(behavior_path_planner): add pull_over base class Signed-off-by: kosuke55 * modify calculation of velocity abs Signed-off-by: kosuke55 * modify from review Signed-off-by: kosuke55 * add const Signed-off-by: kosuke55 * refactor shift pull over Signed-off-by: kosuke55 * not use shared_ptr for lane_departure_checker Signed-off-by: kosuke55 * fix deceleration Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Signed-off-by: kosuke55 * fix werror Signed-off-by: kosuke55 * fix build for main Signed-off-by: kosuke55 Signed-off-by: kosuke55 Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../pull_over/pull_over.param.yaml | 4 +- planning/behavior_path_planner/CMakeLists.txt | 2 + .../config/pull_over/pull_over.param.yaml | 4 +- .../pull_over/geometric_pull_over.hpp | 68 +++ .../pull_over/pull_over_module.hpp | 104 +--- .../pull_over/pull_over_parameters.hpp | 86 ++++ .../scene_module/pull_over/pull_over_path.hpp | 7 +- .../pull_over/pull_over_planner_base.hpp | 72 +++ .../pull_over/shift_pull_over.hpp | 61 +++ .../scene_module/pull_over/util.hpp | 22 +- .../src/behavior_path_planner_node.cpp | 4 +- .../pull_over/geometric_pull_over.cpp | 81 ++++ .../pull_over/pull_over_module.cpp | 447 ++++++------------ .../pull_over/shift_pull_over.cpp | 353 ++++++++++++++ .../src/scene_module/pull_over/util.cpp | 306 ++---------- .../utils/geometric_parallel_parking.cpp | 7 +- 16 files changed, 937 insertions(+), 691 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/geometric_pull_over.hpp create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp create mode 100644 planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp create mode 100644 planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index 98488a82d8a78..281d91f574867 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -45,8 +45,8 @@ arc_path_interval: 1.0 pull_over_max_steer_angle: 0.35 # 20deg # hazard on when parked - hazard_on_threshold_dis: 1.0 - hazard_on_threshold_vel: 0.5 + hazard_on_threshold_distance: 1.0 + hazard_on_threshold_velocity: 0.5 # check safety with dynamic objects. Not used now. pull_over_duration: 2.0 pull_over_prepare_duration: 4.0 diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index fa9e610d02026..05b4f1f9436c0 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -26,6 +26,8 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/scene_module/lane_change/debug.cpp src/scene_module/pull_over/pull_over_module.cpp src/scene_module/pull_over/util.cpp + src/scene_module/pull_over/shift_pull_over.cpp + src/scene_module/pull_over/geometric_pull_over.cpp src/scene_module/pull_out/pull_out_module.cpp src/scene_module/pull_out/util.cpp src/scene_module/pull_out/shift_pull_out.cpp diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index 98488a82d8a78..281d91f574867 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -45,8 +45,8 @@ arc_path_interval: 1.0 pull_over_max_steer_angle: 0.35 # 20deg # hazard on when parked - hazard_on_threshold_dis: 1.0 - hazard_on_threshold_vel: 0.5 + hazard_on_threshold_distance: 1.0 + hazard_on_threshold_velocity: 0.5 # check safety with dynamic objects. Not used now. pull_over_duration: 2.0 pull_over_prepare_duration: 4.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/geometric_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/geometric_pull_over.hpp new file mode 100644 index 0000000000000..2662aac4d13ef --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/geometric_pull_over.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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_ + +#include "behavior_path_planner/scene_module/pull_over/pull_over_path.hpp" +#include "behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp" +#include "behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp" +#include "behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp" + +#include + +#include +#include + +namespace behavior_path_planner +{ +class GeometricPullOver : public PullOverPlannerBase +{ +public: + GeometricPullOver( + rclcpp::Node & node, const PullOverParameters & parameters, + const ParallelParkingParameters & parallel_parking_parameters, + const std::shared_ptr occupancy_grid_map, + const bool is_forward); + + PullOverPlannerType getPlannerType() const override + { + return is_forward_ ? PullOverPlannerType::ARC_FORWARD : PullOverPlannerType::ARC_BACKWARD; + } + Pose getCr() const { return planner_.getCr(); } + Pose getCl() const { return planner_.getCl(); } + + boost::optional plan(const Pose & goal_pose) override; + + std::vector generatePullOverPaths( + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const Pose & goal_pose) const; + std::vector selectValidPaths( + const std::vector & paths, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const bool is_in_goal_route_section, + const Pose & goal_pose) const; + bool hasEnoughDistance( + const PullOverPath & path, const lanelet::ConstLanelets & road_lanes, + const bool is_in_goal_route_section, const Pose & goal_pose) const; + +protected: + ParallelParkingParameters parallel_parking_parameters_; + std::shared_ptr occupancy_grid_map_; + bool is_forward_{true}; + + GeometricParallelParking planner_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 26aa9c4393edd..36c10fde1e0e9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -15,7 +15,10 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_ +#include "behavior_path_planner/scene_module/pull_over/geometric_pull_over.hpp" +#include "behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp" #include "behavior_path_planner/scene_module/pull_over/pull_over_path.hpp" +#include "behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp" #include "behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp" @@ -40,68 +43,6 @@ using nav_msgs::msg::OccupancyGrid; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -struct PullOverParameters -{ - double request_length; - double th_arrived_distance; - double th_stopped_velocity; - double th_stopped_time; - double margin_from_boundary; - double decide_path_distance; - double maximum_deceleration; - // goal research - std::string search_priority; // "efficient_path" or "close_goal" - bool enable_goal_research; - double forward_goal_search_length; - double backward_goal_search_length; - double goal_search_interval; - double goal_to_obstacle_margin; - // occupancy grid map - bool use_occupancy_grid; - double occupancy_grid_collision_check_margin; - double theta_size; - double obstacle_threshold; - // object recognition - bool use_object_recognition; - double object_recognition_collision_check_margin; - // shift path - bool enable_shift_parking; - int pull_over_sampling_num; - double maximum_lateral_jerk; - double minimum_lateral_jerk; - double deceleration_interval; - double pull_over_velocity; - double pull_over_minimum_velocity; - double after_pull_over_straight_distance; - double before_pull_over_straight_distance; - // parallel parking - bool enable_arc_forward_parking; - bool enable_arc_backward_parking; - double after_forward_parking_straight_distance; - double after_backward_parking_straight_distance; - double forward_parking_velocity; - double backward_parking_velocity; - double forward_parking_lane_departure_margin; - double backward_parking_lane_departure_margin; - double arc_path_interval; - double pull_over_max_steer_angle; - // hazard - double hazard_on_threshold_dis; - double hazard_on_threshold_vel; - // check safety with dynamic objects. Not used now. - double pull_over_duration; - double pull_over_prepare_duration; - double min_stop_distance; - double stop_time; - double hysteresis_buffer_distance; - double prediction_time_resolution; - bool enable_collision_check_at_prepare_phase; - bool use_predicted_path_outside_lanelet; - bool use_all_predicted_path; - // debug - bool print_debug_info; -}; - enum PathType { NONE = 0, SHIFT, @@ -111,24 +52,24 @@ enum PathType { struct PUllOverStatus { - PathWithLaneId path{}; - PathWithLaneId full_path{}; - std::shared_ptr prev_stop_path = nullptr; + std::shared_ptr planner{}; + PullOverPath pull_over_path{}; + size_t current_path_idx{0}; + std::shared_ptr prev_stop_path{nullptr}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets pull_over_lanes{}; lanelet::ConstLanelets lanes{}; // current + pull_over - bool has_decided_path = false; - int path_type = PathType::NONE; - bool is_safe = false; - bool prev_is_safe = false; - bool has_decided_velocity = false; - bool has_requested_approval_ = false; + bool has_decided_path{false}; + bool is_safe{false}; + bool prev_is_safe{false}; + bool has_decided_velocity{false}; + bool has_requested_approval{false}; }; struct GoalCandidate { Pose goal_pose{}; - double distance_from_original_goal = 0.0; + double distance_from_original_goal{0.0}; bool operator<(const GoalCandidate & other) const noexcept { @@ -161,42 +102,44 @@ class PullOverModule : public SceneModuleInterface private: PullOverParameters parameters_; - ShiftParkingPath shift_parking_path_; + std::vector> pull_over_planners_; + + PullOverPath shift_parking_path_; vehicle_info_util::VehicleInfo vehicle_info_; - const double pull_over_lane_length_ = 200.0; - const double check_distance_ = 100.0; + const double pull_over_lane_length_{200.0}; + const double check_distance_{100.0}; rclcpp::Subscription::SharedPtr occupancy_grid_sub_; rclcpp::Publisher::SharedPtr goal_pose_pub_; PUllOverStatus status_; - OccupancyGridBasedCollisionDetector occupancy_grid_map_; + std::shared_ptr occupancy_grid_map_; Pose modified_goal_pose_; Pose refined_goal_pose_; std::vector goal_candidates_; GeometricParallelParking parallel_parking_planner_; ParallelParkingParameters parallel_parking_parameters_; std::deque odometry_buffer_; - std::unique_ptr lane_departure_checker_; + std::shared_ptr lane_departure_checker_; tier4_autoware_utils::LinearRing2d vehicle_footprint_; std::unique_ptr last_received_time_; std::unique_ptr last_approved_time_; + void incrementPathIndex(); + PathWithLaneId getCurrentPath() const; + PathWithLaneId getFullPath() const; PathWithLaneId getReferencePath() const; PathWithLaneId generateStopPath() const; Pose calcRefinedGoal() const; - Pose getParkingStartPose() const; ParallelParkingParameters getGeometricPullOverParameters() const; bool isLongEnoughToParkingStart( const PathWithLaneId & path, const Pose & parking_start_pose) const; bool isLongEnough( const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer = 0) const; - bool isArcPath() const; double calcMinimumShiftPathDistance() const; std::pair calcDistanceToPathChange() const; - bool planShiftPath(const Pose goal_pose); bool isStopped(); bool hasFinishedCurrentPath(); bool hasFinishedPullOver(); @@ -206,7 +149,6 @@ class PullOverModule : public SceneModuleInterface bool checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; bool checkCollisionWithPose(const Pose & pose) const; - bool checkCollisionWithPath(const PathWithLaneId & path) const; // turn signal std::pair getHazardInfo() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp new file mode 100644 index 0000000000000..7d9b27c5c51a5 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp @@ -0,0 +1,86 @@ + +// 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ + +#include + +namespace behavior_path_planner +{ +struct PullOverParameters +{ + double request_length; + double th_arrived_distance; + double th_stopped_velocity; + double th_stopped_time; + double margin_from_boundary; + double decide_path_distance; + double maximum_deceleration; + // goal research + std::string search_priority; // "efficient_path" or "close_goal" + bool enable_goal_research; + double forward_goal_search_length; + double backward_goal_search_length; + double goal_search_interval; + double goal_to_obstacle_margin; + // occupancy grid map + bool use_occupancy_grid; + double occupancy_grid_collision_check_margin; + double theta_size; + double obstacle_threshold; + // object recognition + bool use_object_recognition; + double object_recognition_collision_check_margin; + // shift path + bool enable_shift_parking; + int pull_over_sampling_num; + double maximum_lateral_jerk; + double minimum_lateral_jerk; + double deceleration_interval; + double pull_over_velocity; + double pull_over_minimum_velocity; + double after_pull_over_straight_distance; + double before_pull_over_straight_distance; + // parallel parking + bool enable_arc_forward_parking; + bool enable_arc_backward_parking; + double after_forward_parking_straight_distance; + double after_backward_parking_straight_distance; + double forward_parking_velocity; + double backward_parking_velocity; + double forward_parking_lane_departure_margin; + double backward_parking_lane_departure_margin; + double arc_path_interval; + double pull_over_max_steer_angle; + // hazard + double hazard_on_threshold_distance; + double hazard_on_threshold_velocity; + // check safety with dynamic objects. Not used now. + double pull_over_duration; + double pull_over_prepare_duration; + double min_stop_distance; + double stop_time; + double hysteresis_buffer_distance; + double prediction_time_resolution; + bool enable_collision_check_at_prepare_phase; + bool use_predicted_path_outside_lanelet; + bool use_all_predicted_path; + // debug + bool print_debug_info; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp index 5d72e5c87d077..d79c2c3013cf7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp @@ -19,13 +19,18 @@ #include +#include + namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; -struct ShiftParkingPath +struct PullOverPath { PathWithLaneId path{}; + std::vector partial_paths{}; PathWithLaneId straight_path{}; + Pose start_pose{}; + Pose end_pose{}; ShiftedPath shifted_path{}; ShiftPoint shift_point{}; double acceleration{0.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp new file mode 100644 index 0000000000000..2f41beb9e43ba --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp @@ -0,0 +1,72 @@ +// 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/parameters.hpp" +#include "behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp" +#include "behavior_path_planner/scene_module/pull_over/pull_over_path.hpp" +#include "behavior_path_planner/util/create_vehicle_footprint.hpp" + +#include +#include + +#include + +#include +#include + +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; +using tier4_autoware_utils::LinearRing2d; + +namespace behavior_path_planner +{ +enum class PullOverPlannerType { + NONE = 0, + SHIFT, + ARC_FORWARD, + ARC_BACKWARD, +}; + +class PullOverPlannerBase +{ +public: + PullOverPlannerBase(rclcpp::Node & node, const PullOverParameters & parameters) + { + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + parameters_ = parameters; + } + virtual ~PullOverPlannerBase() = default; + + void setPlannerData(const std::shared_ptr planner_data) + { + planner_data_ = planner_data; + } + + virtual PullOverPlannerType getPlannerType() const = 0; + virtual boost::optional plan(const Pose & goal_pose) = 0; + +protected: + std::shared_ptr planner_data_; + vehicle_info_util::VehicleInfo vehicle_info_; + LinearRing2d vehicle_footprint_; + PullOverParameters parameters_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp new file mode 100644 index 0000000000000..fadfee19b75e6 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp @@ -0,0 +1,61 @@ +// 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__SHIFT_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__SHIFT_PULL_OVER_HPP_ + +#include "behavior_path_planner/scene_module/pull_over/pull_over_path.hpp" +#include "behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp" +#include "behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp" + +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using lane_departure_checker::LaneDepartureChecker; + +class ShiftPullOver : public PullOverPlannerBase +{ +public: + ShiftPullOver( + rclcpp::Node & node, const PullOverParameters & parameters, + const LaneDepartureChecker & lane_departure_checker, + const std::shared_ptr occupancy_grid_map); + + PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; + boost::optional plan(const Pose & goal_pose) override; + + std::vector generatePullOverPaths( + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const Pose & goal_pose) const; + std::vector selectValidPaths( + const std::vector & paths, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const bool is_in_goal_route_section, + const Pose & goal_pose) const; + bool hasEnoughDistance( + const PullOverPath & path, const lanelet::ConstLanelets & road_lanes, + const bool is_in_goal_route_section, const Pose & goal_pose) const; + +protected: + LaneDepartureChecker lane_departure_checker_{}; + std::shared_ptr occupancy_grid_map_{}; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__SHIFT_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp index 7c8ba496bb55c..86c18c1f40d4b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp @@ -43,25 +43,11 @@ using geometry_msgs::msg::Twist; // TODO(sugahara) move to util PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2); - -std::vector generateShiftParkingPaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Pose & goal_pose, - const BehaviorPathPlannerParameters & common_parameter, - const behavior_path_planner::PullOverParameters & parameter); - -std::vector selectValidPaths( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const bool is_in_goal_route_section, const Pose & goal_pose, - const lane_departure_checker::LaneDepartureChecker & lane_departure_checker); -bool selectSafePath( - const std::vector & paths, - const OccupancyGridBasedCollisionDetector & occupancy_grid_map, ShiftParkingPath & selected_path); -bool hasEnoughDistance( - const ShiftParkingPath & path, const lanelet::ConstLanelets & current_lanes, - const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose); lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler); +bool hasEnoughDistanceToParkingStart( + const PathWithLaneId & path, const Pose & current_pose, const Pose & start_pose, + const double current_vel, const double maximum_deceleration, const double decide_path_distance, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); // debug Marker createPullOverAreaMarker( 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 34b6f61631a9e..1eb43822654c2 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -414,8 +414,8 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.arc_path_interval = dp("arc_path_interval", 1.0); p.pull_over_max_steer_angle = dp("pull_over_max_steer_angle", 0.35); // 20deg // hazard - p.hazard_on_threshold_dis = dp("hazard_on_threshold_dis", 1.0); - p.hazard_on_threshold_vel = dp("hazard_on_threshold_vel", 0.5); + p.hazard_on_threshold_distance = dp("hazard_on_threshold_distance", 1.0); + p.hazard_on_threshold_velocity = dp("hazard_on_threshold_velocity", 0.5); // safety with dynamic objects. Not used now. p.pull_over_duration = dp("pull_over_duration", 4.0); p.pull_over_prepare_duration = dp("pull_over_prepare_duration", 2.0); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp new file mode 100644 index 0000000000000..d9d5ef4060b50 --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp @@ -0,0 +1,81 @@ +// 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 "behavior_path_planner/scene_module/pull_over/geometric_pull_over.hpp" + +#include "behavior_path_planner/path_utilities.hpp" +#include "behavior_path_planner/scene_module/pull_over/util.hpp" + +#include +#include + +#include +#include + +namespace behavior_path_planner +{ +GeometricPullOver::GeometricPullOver( + rclcpp::Node & node, const PullOverParameters & parameters, + const ParallelParkingParameters & parallel_parking_parameters, + const std::shared_ptr occupancy_grid_map, + const bool is_forward) +: PullOverPlannerBase{node, parameters}, + parallel_parking_parameters_{parallel_parking_parameters}, + occupancy_grid_map_{occupancy_grid_map}, + is_forward_{is_forward} +{ +} + +boost::optional GeometricPullOver::plan(const Pose & goal_pose) +{ + const auto & route_handler = planner_data_->route_handler; + + const auto road_lanes = util::getCurrentLanes(planner_data_); + const auto shoulder_lanes = pull_over_utils::getPullOverLanes(*route_handler); + if (road_lanes.empty() || shoulder_lanes.empty()) { + return {}; + } + + // todo: set param only once + planner_.setData(planner_data_, parallel_parking_parameters_); + const bool found_valid_path = + planner_.planPullOver(goal_pose, road_lanes, shoulder_lanes, is_forward_); + if (!found_valid_path) { + return {}; + } + + // collision check + const auto arc_path = planner_.getArcPath(); + if (parameters_.use_occupancy_grid || !occupancy_grid_map_) { + const bool check_out_of_range = false; + if (occupancy_grid_map_->hasObstacleOnPath(arc_path, check_out_of_range)) { + return {}; + } + } + if (parameters_.use_object_recognition) { + if (util::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint_, arc_path, *(planner_data_->dynamic_object), + parameters_.object_recognition_collision_check_margin)) { + return {}; + } + } + + PullOverPath pull_over_path{}; + pull_over_path.partial_paths = planner_.getPaths(); + pull_over_path.start_pose = planner_.getStartPose(); + pull_over_path.end_pose = planner_.getArcEndPose(); + + return pull_over_path; +} +} // namespace behavior_path_planner 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 cce0105f52f33..8d88a523c06f4 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 @@ -32,15 +32,11 @@ #include #include -using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using nav_msgs::msg::OccupancyGrid; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; using tier4_autoware_utils::inverseTransformPose; namespace behavior_path_planner @@ -56,9 +52,26 @@ PullOverModule::PullOverModule( goal_pose_pub_ = node.create_publisher("/planning/scenario_planning/modified_goal", 1); - lane_departure_checker_ = std::make_unique(); - lane_departure_checker_->setVehicleInfo( - vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); + LaneDepartureChecker lane_departure_checker_{}; + lane_departure_checker_.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); + + occupancy_grid_map_ = std::make_shared(); + + // set enabled planner + if (parameters_.enable_shift_parking) { + pull_over_planners_.push_back(std::make_shared( + node, parameters, lane_departure_checker_, occupancy_grid_map_)); + } + if (parameters_.enable_arc_forward_parking) { + const bool is_forward = true; + pull_over_planners_.push_back(std::make_shared( + node, parameters, getGeometricPullOverParameters(), occupancy_grid_map_, is_forward)); + } + if (parameters_.enable_arc_backward_parking) { + const bool is_forward = false; + pull_over_planners_.push_back(std::make_shared( + node, parameters, getGeometricPullOverParameters(), occupancy_grid_map_, is_forward)); + } // for collision check with objects vehicle_footprint_ = createVehicleFootprint(vehicle_info_); @@ -68,11 +81,8 @@ PullOverModule::PullOverModule( void PullOverModule::resetStatus() { - status_.has_decided_path = false; - status_.path_type = PathType::NONE; - status_.is_safe = false; - status_.has_decided_velocity = false; - status_.has_requested_approval_ = false; + PUllOverStatus initial_status{}; + status_ = initial_status; } // This function is needed for waiting for planner_data_ @@ -82,7 +92,7 @@ void PullOverModule::updateOccupancyGrid() RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "occupancy_grid is not ready"); return; } - occupancy_grid_map_.setMap(*(planner_data_->occupancy_grid)); + occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); } BehaviorModuleOutput PullOverModule::run() @@ -94,7 +104,7 @@ BehaviorModuleOutput PullOverModule::run() ParallelParkingParameters PullOverModule::getGeometricPullOverParameters() const { - ParallelParkingParameters params; + ParallelParkingParameters params{}; params.th_arrived_distance = parameters_.th_arrived_distance; params.th_stopped_velocity = parameters_.th_stopped_velocity; @@ -121,7 +131,7 @@ void PullOverModule::onEntry() // Initialize occupancy grid map if (parameters_.use_occupancy_grid) { - OccupancyGridMapParam occupancy_grid_map_param; + OccupancyGridMapParam occupancy_grid_map_param{}; const double margin = parameters_.occupancy_grid_collision_check_margin; occupancy_grid_map_param.vehicle_shape.length = planner_data_->parameters.vehicle_length + 2 * margin; @@ -131,12 +141,12 @@ void PullOverModule::onEntry() planner_data_->parameters.base_link2rear + margin; occupancy_grid_map_param.theta_size = parameters_.theta_size; occupancy_grid_map_param.obstacle_threshold = parameters_.obstacle_threshold; - occupancy_grid_map_.setParam(occupancy_grid_map_param); + occupancy_grid_map_->setParam(occupancy_grid_map_param); } // initialize when receiving new route if ( - last_received_time_.get() == nullptr || + !last_received_time_ || *last_received_time_ != planner_data_->route_handler->getRouteHeader().stamp) { // Initialize parallel parking planner status parallel_parking_parameters_ = getGeometricPullOverParameters(); @@ -323,22 +333,22 @@ bool PullOverModule::checkCollisionWithLongitudinalDistance( // check forward collision const Pose ego_pose_moved_forward = calcOffsetPose(ego_pose, offset, 0, 0); const Pose forward_pose_grid_coords = - global2local(occupancy_grid_map_.getMap(), ego_pose_moved_forward); + global2local(occupancy_grid_map_->getMap(), ego_pose_moved_forward); const auto forward_idx = pose2index( - occupancy_grid_map_.getMap(), forward_pose_grid_coords, - occupancy_grid_map_.getParam().theta_size); - if (occupancy_grid_map_.detectCollision(forward_idx, check_out_of_range)) { + occupancy_grid_map_->getMap(), forward_pose_grid_coords, + occupancy_grid_map_->getParam().theta_size); + if (occupancy_grid_map_->detectCollision(forward_idx, check_out_of_range)) { return true; } // check backward collision const Pose ego_pose_moved_backward = calcOffsetPose(ego_pose, -offset, 0, 0); const Pose backward_pose_grid_coords = - global2local(occupancy_grid_map_.getMap(), ego_pose_moved_backward); + global2local(occupancy_grid_map_->getMap(), ego_pose_moved_backward); const auto backward_idx = pose2index( - occupancy_grid_map_.getMap(), backward_pose_grid_coords, - occupancy_grid_map_.getParam().theta_size); - if (occupancy_grid_map_.detectCollision(backward_idx, check_out_of_range)) { + occupancy_grid_map_->getMap(), backward_pose_grid_coords, + occupancy_grid_map_->getParam().theta_size); + if (occupancy_grid_map_->detectCollision(backward_idx, check_out_of_range)) { return true; } } @@ -358,11 +368,11 @@ bool PullOverModule::checkCollisionWithLongitudinalDistance( bool PullOverModule::checkCollisionWithPose(const Pose & pose) const { if (parameters_.use_occupancy_grid) { - const Pose pose_grid_coords = global2local(occupancy_grid_map_.getMap(), pose); + const Pose pose_grid_coords = global2local(occupancy_grid_map_->getMap(), pose); const auto idx = pose2index( - occupancy_grid_map_.getMap(), pose_grid_coords, occupancy_grid_map_.getParam().theta_size); + occupancy_grid_map_->getMap(), pose_grid_coords, occupancy_grid_map_->getParam().theta_size); const bool check_out_of_range = false; - if (occupancy_grid_map_.detectCollision(idx, check_out_of_range)) { + if (occupancy_grid_map_->detectCollision(idx, check_out_of_range)) { return true; } } @@ -377,167 +387,42 @@ bool PullOverModule::checkCollisionWithPose(const Pose & pose) const return false; } -bool PullOverModule::checkCollisionWithPath(const PathWithLaneId & path) const -{ - if (parameters_.use_occupancy_grid) { - const bool check_out_of_range = false; - if (occupancy_grid_map_.hasObstacleOnPath(path, check_out_of_range)) { - return true; - } - } - - if (parameters_.use_object_recognition) { - if (util::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path, *(planner_data_->dynamic_object), - parameters_.object_recognition_collision_check_margin)) { - return true; - } - } - return false; -} - bool PullOverModule::planWithEfficientPath() { - // shift parking path - if (parameters_.enable_shift_parking) { + for (const auto & planner : pull_over_planners_) { for (const auto goal_candidate : goal_candidates_) { - if ( - planShiftPath(goal_candidate.goal_pose) && - isLongEnoughToParkingStart( - shift_parking_path_.path, shift_parking_path_.shift_point.start)) { - // shift parking plan already confirms safety and no lane departure in it's own function. - modified_goal_pose_ = goal_candidate.goal_pose; - status_.path = shift_parking_path_.path; - status_.full_path = shift_parking_path_.path; - status_.path_type = PathType::SHIFT; - status_.is_safe = true; - return true; - } - } - } - - // forward arc path - if (parameters_.enable_arc_forward_parking) { - for (const auto goal_candidate : goal_candidates_) { - parallel_parking_planner_.setData(planner_data_, parallel_parking_parameters_); - if ( - parallel_parking_planner_.planPullOver( - goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, true) && - isLongEnoughToParkingStart( - parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose()) && - !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && - !lane_departure_checker_->checkPathWillLeaveLane( - status_.lanes, parallel_parking_planner_.getArcPath())) { - modified_goal_pose_ = goal_candidate.goal_pose; - status_.path = parallel_parking_planner_.getCurrentPath(); - status_.full_path = parallel_parking_planner_.getFullPath(); - status_.path_type = PathType::ARC_FORWARD; - status_.is_safe = true; - return true; - } - } - } - - // backward arc path - if (parameters_.enable_arc_backward_parking) { - for (const auto goal_candidate : goal_candidates_) { - parallel_parking_planner_.setData(planner_data_, parallel_parking_parameters_); - if ( - parallel_parking_planner_.planPullOver( - goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, false) && - isLongEnoughToParkingStart( - parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose()) && - !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && - !lane_departure_checker_->checkPathWillLeaveLane( - status_.lanes, parallel_parking_planner_.getArcPath())) { - modified_goal_pose_ = goal_candidate.goal_pose; - status_.path = parallel_parking_planner_.getCurrentPath(); - status_.full_path = parallel_parking_planner_.getFullPath(); - status_.path_type = PathType::ARC_BACKWARD; - status_.is_safe = true; - return true; + planner->setPlannerData(planner_data_); + const auto pull_over_path = planner->plan(goal_candidate.goal_pose); + if (!pull_over_path) { + continue; } + modified_goal_pose_ = goal_candidate.goal_pose; + status_.pull_over_path = *pull_over_path; + status_.planner = planner; + return true; // found safe path } } - return false; + return false; // not found safe path } bool PullOverModule::planWithCloseGoal() { for (const auto goal_candidate : goal_candidates_) { - if (status_.is_safe) break; - modified_goal_pose_ = goal_candidate.goal_pose; - - // Generate arc shift path. - if ( - parameters_.enable_shift_parking && planShiftPath(goal_candidate.goal_pose) && - isLongEnoughToParkingStart(shift_parking_path_.path, shift_parking_path_.shift_point.start)) { - // shift parking plan already confirms safety and no lane departure in it's own function. - modified_goal_pose_ = goal_candidate.goal_pose; - status_.path = shift_parking_path_.path; - status_.full_path = shift_parking_path_.path; - status_.path_type = PathType::SHIFT; - status_.is_safe = true; - return true; - } - - parallel_parking_planner_.setData(planner_data_, parallel_parking_parameters_); - // Generate arc forward path. - if ( - parameters_.enable_arc_forward_parking && - parallel_parking_planner_.planPullOver( - goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, true) && - isLongEnoughToParkingStart( - parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose()) && - !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && - !lane_departure_checker_->checkPathWillLeaveLane( - status_.lanes, parallel_parking_planner_.getArcPath())) { - modified_goal_pose_ = goal_candidate.goal_pose; - status_.path = parallel_parking_planner_.getCurrentPath(); - status_.full_path = parallel_parking_planner_.getFullPath(); - status_.path_type = PathType::ARC_FORWARD; - status_.is_safe = true; - return true; - } - - // Generate arc backward path. - if ( - parameters_.enable_arc_backward_parking && - parallel_parking_planner_.planPullOver( - goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, false) && - isLongEnoughToParkingStart( - parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose()) && - !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && - !lane_departure_checker_->checkPathWillLeaveLane( - status_.lanes, parallel_parking_planner_.getArcPath())) { + for (const auto & planner : pull_over_planners_) { + planner->setPlannerData(planner_data_); + const auto pull_over_path = planner->plan(goal_candidate.goal_pose); + if (!pull_over_path) { + continue; + } modified_goal_pose_ = goal_candidate.goal_pose; - status_.path = parallel_parking_planner_.getCurrentPath(); - status_.full_path = parallel_parking_planner_.getFullPath(); - status_.path_type = PathType::ARC_BACKWARD; - status_.is_safe = true; - return true; + status_.pull_over_path = *pull_over_path; + status_.planner = planner; + return true; // found safe path } } - return false; -} - -Pose PullOverModule::getParkingStartPose() const -{ - Pose parking_start_pose; - if (status_.path_type == PathType::SHIFT) { - // offset shift start point backward to ensure enough distance - // for winker operation even when stopped - const double offset = parameters_.pull_over_minimum_velocity * - planner_data_->parameters.turn_light_on_threshold_time; - const auto offset_pose = calcLongitudinalOffsetPose( - shift_parking_path_.path.points, shift_parking_path_.shift_point.start.position, -offset); - parking_start_pose = offset_pose ? *offset_pose : shift_parking_path_.shift_point.start; - } else if (isArcPath()) { - parking_start_pose = parallel_parking_planner_.getStartPose(); - } - return parking_start_pose; + return false; // not found safe path } BehaviorModuleOutput PullOverModule::plan() @@ -552,10 +437,9 @@ BehaviorModuleOutput PullOverModule::plan() // Check if it needs to decide path if (status_.is_safe) { - const Pose parking_start_pose = getParkingStartPose(); const auto dist_to_parking_start_pose = calcSignedArcLength( - status_.path.points, planner_data_->self_pose->pose, parking_start_pose.position, - std::numeric_limits::max(), M_PI_2); + getCurrentPath().points, planner_data_->self_pose->pose, + status_.pull_over_path.start_pose.position, std::numeric_limits::max(), M_PI_2); if (*dist_to_parking_start_pose < parameters_.decide_path_distance) { status_.has_decided_path = true; @@ -564,39 +448,40 @@ BehaviorModuleOutput PullOverModule::plan() // Use decided path if (status_.has_decided_path) { - if (!status_.has_requested_approval_) { + if (!status_.has_requested_approval) { // request approval again one the final path is decided waitApproval(); removeRTCStatus(); steering_factor_interface_ptr_->clearSteeringFactors(); uuid_ = generateUUID(); current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop - status_.has_requested_approval_ = true; + status_.has_requested_approval = true; } else if (isActivated() && isWaitingApproval()) { + // When it is approved again after path is decided clearWaitingApproval(); last_approved_time_ = std::make_unique(clock_->now()); // decide velocity to guarantee turn signal lighting time if (!status_.has_decided_velocity) { - const float vel = static_cast(std::max( + auto & first_path = status_.pull_over_path.partial_paths.front(); + const auto vel = static_cast(std::max( planner_data_->self_odometry->twist.twist.linear.x, parameters_.pull_over_minimum_velocity)); - for (auto & p : status_.path.points) { + for (auto & p : first_path.points) { p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); } } status_.has_decided_velocity = true; } - if (isActivated() && isArcPath() && last_approved_time_ != nullptr) { + if (isActivated() && last_approved_time_ != nullptr) { // if using arc_path and finishing current_path, get next path // enough time for turn signal const bool has_passed_enough_time = (clock_->now() - *last_approved_time_).seconds() > planner_data_->parameters.turn_light_on_threshold_time; - if (isActivated() && hasFinishedCurrentPath() && has_passed_enough_time) { - parallel_parking_planner_.incrementPathIndex(); - status_.path = parallel_parking_planner_.getCurrentPath(); + if (hasFinishedCurrentPath() && has_passed_enough_time) { + incrementPathIndex(); } } @@ -604,14 +489,11 @@ BehaviorModuleOutput PullOverModule::plan() // Research goal when enabling research and final path has not been decided if (parameters_.enable_goal_research) researchGoal(); - status_.path_type = PathType::NONE; - status_.is_safe = false; - - bool has_found_safe_path = false; + // plan paths with several goals and planner if (parameters_.search_priority == "efficient_path") { - has_found_safe_path = planWithEfficientPath(); + status_.is_safe = planWithEfficientPath(); } else if (parameters_.search_priority == "close_goal") { - has_found_safe_path = planWithCloseGoal(); + status_.is_safe = planWithCloseGoal(); } else { RCLCPP_ERROR( getLogger(), "search_priority should be efficient_path or close_goal, but %s is given.", @@ -620,28 +502,31 @@ BehaviorModuleOutput PullOverModule::plan() } // Decelerate before the minimum shift distance from the goal search area. - if (has_found_safe_path) { + if (status_.is_safe) { + auto & first_path = status_.pull_over_path.partial_paths.front(); const auto arc_coordinates = lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose_); const Pose search_start_pose = calcOffsetPose( refined_goal_pose_, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0); - status_.path = util::setDecelerationVelocity( - status_.path, parameters_.pull_over_velocity, search_start_pose, + first_path = util::setDecelerationVelocity( + first_path, parameters_.pull_over_velocity, search_start_pose, -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); } } - // generate drivable area - { + + // generate drivable area for each partial path + 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; - status_.path.drivable_area = util::generateDrivableArea( - status_.path, status_.lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); + path.drivable_area = util::generateDrivableArea( + path, status_.lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); } BehaviorModuleOutput output; // safe: use pull over path if (status_.is_safe) { - output.path = std::make_shared(status_.path); - output.path_candidate = std::make_shared(status_.full_path); + output.path = std::make_shared(getCurrentPath()); + output.path_candidate = std::make_shared(getFullPath()); } else { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path. Stop in road lane."); @@ -693,7 +578,7 @@ BehaviorModuleOutput PullOverModule::plan() // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( - {getParkingStartPose(), modified_goal_pose_}, + {status_.pull_over_path.start_pose, modified_goal_pose_}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, steering_factor_direction, SteeringFactor::TURNING, ""); @@ -717,9 +602,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() const auto path = *(plan().path); out.path_candidate = std::make_shared(path); out.path = std::make_shared(getReferencePath()); - if (status_.is_safe && isArcPath()) { - out.path_candidate = std::make_shared(parallel_parking_planner_.getFullPath()); - } + out.path_candidate = std::make_shared(getFullPath()); const auto distance_to_path_change = calcDistanceToPathChange(); updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); @@ -734,7 +617,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() }); steering_factor_interface_ptr_->updateSteeringFactor( - {getParkingStartPose(), modified_goal_pose_}, + {status_.pull_over_path.start_pose, modified_goal_pose_}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, steering_factor_direction, SteeringFactor::APPROACHING, ""); waitApproval(); @@ -744,13 +627,12 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() std::pair PullOverModule::calcDistanceToPathChange() const { - const Pose parking_start_pose = getParkingStartPose(); - const Pose parking_end_pose = modified_goal_pose_; + const auto & full_path = getFullPath(); const auto dist_to_parking_start_pose = calcSignedArcLength( - status_.path.points, planner_data_->self_pose->pose, parking_start_pose.position, + full_path.points, planner_data_->self_pose->pose, status_.pull_over_path.start_pose.position, std::numeric_limits::max(), M_PI_2); const double dist_to_parking_finish_pose = calcSignedArcLength( - status_.path.points, planner_data_->self_pose->pose.position, parking_end_pose.position); + full_path.points, planner_data_->self_pose->pose.position, modified_goal_pose_.position); const double start_distance_to_path_change = dist_to_parking_start_pose ? *dist_to_parking_start_pose : std::numeric_limits::max(); return {start_distance_to_path_change, dist_to_parking_finish_pose}; @@ -775,7 +657,7 @@ PathWithLaneId PullOverModule::getReferencePath() const const Pose search_start_pose = calcOffsetPose( refined_goal_pose_, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0); // if not approved, stop parking start position or goal search start position. - const Pose stop_pose = status_.is_safe ? getParkingStartPose() : search_start_pose; + const Pose stop_pose = status_.is_safe ? status_.pull_over_path.start_pose : search_start_pose; // generate center line path to stop_pose const auto arc_position_stop_pose = @@ -852,54 +734,37 @@ PathWithLaneId PullOverModule::generateStopPath() const return stop_path; } -bool PullOverModule::planShiftPath(const Pose goal_pose) +void PullOverModule::incrementPathIndex() { - std::vector valid_paths; - - const auto & route_handler = planner_data_->route_handler; - const auto & current_pose = planner_data_->self_pose->pose; - const auto & common_parameters = planner_data_->parameters; - - if (!isLongEnough(status_.current_lanes, goal_pose) || status_.pull_over_lanes.empty()) { - return false; - } - - // find candidate paths - const auto pull_over_paths = pull_over_utils::generateShiftParkingPaths( - *route_handler, status_.current_lanes, status_.pull_over_lanes, current_pose, goal_pose, - common_parameters, parameters_); - if (pull_over_paths.empty()) { - return false; - } + status_.current_path_idx = + std::min(status_.current_path_idx + 1, status_.pull_over_path.partial_paths.size() - 1); +} - // select valid path - valid_paths = pull_over_utils::selectValidPaths( - pull_over_paths, status_.current_lanes, status_.pull_over_lanes, current_pose, - route_handler->isInGoalRouteSection(status_.current_lanes.back()), goal_pose, - *lane_departure_checker_); - if (valid_paths.empty()) { - return false; - } +PathWithLaneId PullOverModule::getCurrentPath() const +{ + return status_.pull_over_path.partial_paths.at(status_.current_path_idx); +} - // select safe path - bool found_safe_path = false; - for (const auto & path : valid_paths) { - if (!checkCollisionWithPath(path.shifted_path.path)) { - shift_parking_path_ = path; - found_safe_path = true; - break; +PathWithLaneId PullOverModule::getFullPath() const +{ + PathWithLaneId path{}; + const auto & paths = status_.pull_over_path.partial_paths; + for (size_t i = 0; i < paths.size(); ++i) { + if (i == 0) { + path.points.insert(path.points.end(), paths.at(i).points.begin(), paths.at(i).points.end()); + } else { + // skip overlapping point + path.points.insert( + path.points.end(), next(paths.at(i).points.begin()), paths.at(i).points.end()); } } + path.points = motion_utils::removeOverlapPoints(path.points); - shift_parking_path_.is_safe = found_safe_path; - shift_parking_path_.path.header = route_handler->getRouteHeader(); - - return found_safe_path; + return path; } double PullOverModule::calcMinimumShiftPathDistance() const { - PathShifter path_shifter; const double maximum_jerk = parameters_.maximum_lateral_jerk; const double pull_over_velocity = parameters_.pull_over_velocity; const auto current_pose = planner_data_->self_pose->pose; @@ -914,7 +779,7 @@ double PullOverModule::calcMinimumShiftPathDistance() const parameters_.margin_from_boundary; // calculate minimum pull over distance at pull over velocity, maximum jerk and side offset - const double pull_over_distance_min = path_shifter.calcLongitudinalDistFromJerk( + const double pull_over_distance_min = PathShifter::calcLongitudinalDistFromJerk( abs(offset_from_center_line), maximum_jerk, pull_over_velocity); const double pull_over_total_distance_min = distance_after_pull_over + pull_over_distance_min + distance_before_pull_over; @@ -957,8 +822,8 @@ bool PullOverModule::isStopped() bool PullOverModule::hasFinishedCurrentPath() { - const auto current_path_end = status_.path.points.back(); - const auto self_pose = planner_data_->self_pose->pose; + const auto & current_path_end = getCurrentPath().points.back(); + const auto & self_pose = planner_data_->self_pose->pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < parameters_.th_arrived_distance; @@ -977,58 +842,42 @@ bool PullOverModule::hasFinishedPullOver() std::pair PullOverModule::getHazardInfo() const { - HazardLightsCommand hazard_signal; - const double max_distance = std::numeric_limits::max(); - - double distance_to_target_pose; // distance from current pose to target pose on target lanes - double distance_to_target_point; // distance from vehicle front to target point on target lanes. - { - const auto arc_position_target_pose = - lanelet::utils::getArcCoordinates(status_.pull_over_lanes, modified_goal_pose_); - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(status_.pull_over_lanes, planner_data_->self_pose->pose); - distance_to_target_pose = arc_position_target_pose.length - arc_position_current_pose.length; - distance_to_target_point = distance_to_target_pose - planner_data_->parameters.base_link2front; - } + HazardLightsCommand hazard_signal{}; + + const auto arc_position_goal_pose = + lanelet::utils::getArcCoordinates(status_.pull_over_lanes, modified_goal_pose_); + const auto arc_position_current_pose = + lanelet::utils::getArcCoordinates(status_.pull_over_lanes, planner_data_->self_pose->pose); + const double distance_to_goal = arc_position_goal_pose.length - arc_position_current_pose.length; - const auto velocity = planner_data_->self_odometry->twist.twist.linear.x; + const double velocity = std::abs(planner_data_->self_odometry->twist.twist.linear.x); if ( - (distance_to_target_pose < parameters_.hazard_on_threshold_dis && - abs(velocity) < parameters_.hazard_on_threshold_vel) || - status_.path_type == PathType::ARC_BACKWARD) { + (distance_to_goal < parameters_.hazard_on_threshold_distance && + velocity < parameters_.hazard_on_threshold_velocity) || + status_.planner->getPlannerType() == PullOverPlannerType::ARC_BACKWARD) { hazard_signal.command = HazardLightsCommand::ENABLE; - return std::make_pair(hazard_signal, distance_to_target_point); + const double distance_from_front_to_goal = + distance_to_goal - planner_data_->parameters.base_link2front; + return std::make_pair(hazard_signal, distance_from_front_to_goal); } - return std::make_pair(hazard_signal, max_distance); + return std::make_pair(hazard_signal, std::numeric_limits::max()); } std::pair PullOverModule::getTurnInfo() const { - std::pair turn_info; + std::pair turn_info{}; - double distance_from_vehicle_front; - { + const double distance_from_vehicle_front = std::invoke([&]() { const auto arc_position_current_pose = lanelet::utils::getArcCoordinates(status_.current_lanes, planner_data_->self_pose->pose); - - Pose parking_end_pose; - { - if (status_.path_type == PathType::SHIFT) { - parking_end_pose = shift_parking_path_.shift_point.end; - } else if (isArcPath()) { - parking_end_pose = parallel_parking_planner_.getArcEndPose(); - } - } - const auto arc_position_end_pose = - lanelet::utils::getArcCoordinates(status_.current_lanes, parking_end_pose); - - distance_from_vehicle_front = arc_position_end_pose.length - arc_position_current_pose.length - - planner_data_->parameters.base_link2front; - } + lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_over_path.end_pose); + return arc_position_end_pose.length - arc_position_current_pose.length - + planner_data_->parameters.base_link2front; + }); - TurnIndicatorsCommand turn_signal; + TurnIndicatorsCommand turn_signal{}; const bool is_before_parking_end = distance_from_vehicle_front >= 0.0; turn_signal.command = is_before_parking_end ? TurnIndicatorsCommand::ENABLE_LEFT : TurnIndicatorsCommand::NO_COMMAND; @@ -1037,17 +886,11 @@ std::pair PullOverModule::getTurnInfo() const return turn_info; } -bool PullOverModule::isArcPath() const -{ - return status_.path_type == PathType::ARC_FORWARD || status_.path_type == PathType::ARC_BACKWARD; -} - void PullOverModule::setDebugData() { debug_marker_.markers.clear(); using marker_utils::createPathMarkerArray; - using marker_utils::createPolygonMarkerArray; using marker_utils::createPoseMarkerArray; const auto add = [this](const MarkerArray & added) { @@ -1071,20 +914,12 @@ void PullOverModule::setDebugData() } // Visualize path and related pose - if (!status_.is_safe) { - return; - } - if (isArcPath()) { - add(createPoseMarkerArray(parallel_parking_planner_.getCl(), "Cl", 0, 0.9, 0.3, 0.3)); - add(createPoseMarkerArray(parallel_parking_planner_.getCr(), "Cr", 0, 0.3, 0.9, 0.3)); + if (status_.is_safe) { add(createPoseMarkerArray( - parallel_parking_planner_.getStartPose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); + status_.pull_over_path.start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - parallel_parking_planner_.getArcEndPose(), "pull_over_arc_end_pose", 0, 0.9, 0.9, 0.3)); - add(createPathMarkerArray( - parallel_parking_planner_.getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); - } else { - add(createPathMarkerArray(status_.path, "full_path", 0, 0.0, 0.5, 0.9)); + status_.pull_over_path.end_pose, "pull_over_end_pose", 0, 0.9, 0.9, 0.3)); + add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); } } diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp new file mode 100644 index 0000000000000..c83da0f0bd64f --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp @@ -0,0 +1,353 @@ +// 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 "behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp" + +#include "behavior_path_planner/path_utilities.hpp" +#include "behavior_path_planner/scene_module/pull_over/util.hpp" + +#include +#include + +#include +#include + +namespace behavior_path_planner + +{ +ShiftPullOver::ShiftPullOver( + rclcpp::Node & node, const PullOverParameters & parameters, + const LaneDepartureChecker & lane_departure_checker, + const std::shared_ptr occupancy_grid_map) +: PullOverPlannerBase{node, parameters}, + lane_departure_checker_{lane_departure_checker}, + occupancy_grid_map_{occupancy_grid_map} +{ +} + +boost::optional ShiftPullOver::plan(const Pose & goal_pose) +{ + const auto & route_handler = planner_data_->route_handler; + + const auto road_lanes = util::getCurrentLanes(planner_data_); + const auto shoulder_lanes = pull_over_utils::getPullOverLanes(*route_handler); + if (road_lanes.empty() || shoulder_lanes.empty()) { + return {}; + } + + lanelet::ConstLanelets lanes; + lanes.insert(lanes.end(), road_lanes.begin(), road_lanes.end()); + lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + + // generate candidate paths + const auto pull_over_paths = generatePullOverPaths(road_lanes, shoulder_lanes, goal_pose); + if (pull_over_paths.empty()) { + return {}; + } + + // select valid paths which have enough distance and no lane departure + const auto valid_paths = selectValidPaths( + pull_over_paths, road_lanes, shoulder_lanes, + route_handler->isInGoalRouteSection(road_lanes.back()), goal_pose); + if (valid_paths.empty()) { + return {}; + } + + // select safe path + for (const auto & path : valid_paths) { + if (parameters_.use_occupancy_grid || !occupancy_grid_map_) { + const bool check_out_of_range = false; + if (occupancy_grid_map_->hasObstacleOnPath(path.shifted_path.path, check_out_of_range)) { + continue; + } + } + + if (parameters_.use_object_recognition) { + if (util::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint_, path.shifted_path.path, *(planner_data_->dynamic_object), + parameters_.object_recognition_collision_check_margin)) { + continue; + } + } + + // found safe path + return path; + } + + // not found safe path + return {}; +} + +std::vector ShiftPullOver::generatePullOverPaths( + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const Pose & goal_pose) const +{ + // rename parameter + const auto & route_handler = planner_data_->route_handler; + const auto & common_parameters = planner_data_->parameters; + const Pose & current_pose = planner_data_->self_pose->pose; + const double backward_path_length = common_parameters.backward_path_length; + const double pull_over_velocity = parameters_.pull_over_velocity; + const double after_pull_over_straight_distance = parameters_.after_pull_over_straight_distance; + const double margin = parameters_.margin_from_boundary; + const double minimum_lateral_jerk = parameters_.minimum_lateral_jerk; + const double maximum_lateral_jerk = parameters_.maximum_lateral_jerk; + const double deceleration_interval = parameters_.deceleration_interval; + const int pull_over_sampling_num = parameters_.pull_over_sampling_num; + const double jerk_resolution = + std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_over_sampling_num; + + const double distance_to_shoulder_lane_boundary = + util::getDistanceToShoulderBoundary(shoulder_lanes, current_pose); + const double offset_from_current_pose = + distance_to_shoulder_lane_boundary + common_parameters.vehicle_width / 2 + margin; + + // shift end point in shoulder lane + const auto shift_end_point = std::invoke([&]() { + const auto arc_position_goal = lanelet::utils::getArcCoordinates(shoulder_lanes, goal_pose); + const double s_start = arc_position_goal.length - after_pull_over_straight_distance; + const double s_end = s_start + std::numeric_limits::epsilon(); + const auto path = route_handler->getCenterLinePath(shoulder_lanes, s_start, s_end, true); + return path.points.front(); + }); + + std::vector candidate_paths; + for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; + lateral_jerk += jerk_resolution) { + PathShifter path_shifter; + ShiftedPath shifted_path; + PullOverPath candidate_path; + + const double pull_over_distance = path_shifter.calcLongitudinalDistFromJerk( + abs(offset_from_current_pose), lateral_jerk, pull_over_velocity); + + // calculate straight distance before pull over + const double straight_distance = std::invoke([&]() { + const auto arc_position_goal = lanelet::utils::getArcCoordinates(road_lanes, goal_pose); + const auto arc_position_pose = lanelet::utils::getArcCoordinates(road_lanes, current_pose); + return arc_position_goal.length - after_pull_over_straight_distance - pull_over_distance - + arc_position_pose.length; + }); + + PathWithLaneId road_lane_reference_path; + { + const auto arc_position = lanelet::utils::getArcCoordinates(road_lanes, current_pose); + const auto arc_position_ref2_front = + lanelet::utils::getArcCoordinates(road_lanes, shift_end_point.point.pose); + const double s_start = arc_position.length - backward_path_length; + const double s_end = arc_position_ref2_front.length - pull_over_distance; + road_lane_reference_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); + // decelerate velocity linearly to minimum pull over velocity + // ( or accelerate if original velocity is lower than minimum velocity ) + for (auto & point : road_lane_reference_path.points) { + const auto arclength = + lanelet::utils::getArcCoordinates(road_lanes, point.point.pose).length; + const double distance_to_pull_over_start = std::max(0.0, s_end - arclength); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast( + (distance_to_pull_over_start / deceleration_interval) * + (point.point.longitudinal_velocity_mps - pull_over_velocity) + + pull_over_velocity)); + } + } + + if (road_lane_reference_path.points.empty()) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("pull_over").get_child("util"), + "reference path is empty!! something wrong..."); + continue; + } + + PathWithLaneId target_lane_reference_path; + { + const lanelet::ArcCoordinates pull_over_start_arc_position = + lanelet::utils::getArcCoordinates( + shoulder_lanes, road_lane_reference_path.points.back().point.pose); + const double s_start = pull_over_start_arc_position.length; + const auto arc_position_goal = lanelet::utils::getArcCoordinates(shoulder_lanes, goal_pose); + const double s_end = arc_position_goal.length; + target_lane_reference_path = route_handler->getCenterLinePath(shoulder_lanes, s_start, s_end); + // distance between shoulder lane's left boundary and shoulder lane center + const double distance_shoulder_to_left_bound = + util::getDistanceToShoulderBoundary(shoulder_lanes, shift_end_point.point.pose); + + // distance between shoulder lane center and target line + const double distance_shoulder_to_target = + distance_shoulder_to_left_bound + common_parameters.vehicle_width / 2 + margin; + + // Apply shifting shoulder lane to adjust to target line + const double offset = -distance_shoulder_to_target; + for (size_t i = 0; i < target_lane_reference_path.points.size(); ++i) { + { + if (fabs(offset) < 1.0e-8) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("pull_over").get_child("util"), + "no offset from current lane center."); + } + + auto & p = target_lane_reference_path.points.at(i).point.pose; + p = tier4_autoware_utils::calcOffsetPose(p, 0, offset, 0); + } + path_shifter.setPath(util::resamplePathWithSpline(target_lane_reference_path, 1.0)); + } + } + ShiftPoint shift_point; + { + shift_point.start = road_lane_reference_path.points.back().point.pose; + shift_point.end = shift_end_point.point.pose; + + // distance between shoulder lane's left boundary and current lane center + const double distance_road_to_left_boundary = util::getDistanceToShoulderBoundary( + shoulder_lanes, road_lane_reference_path.points.back().point.pose); + // distance between shoulder lane's left boundary and current lane center + const double distance_road_to_target = + distance_road_to_left_boundary + common_parameters.vehicle_width / 2 + margin; + + shift_point.length = distance_road_to_target; + path_shifter.addShiftPoint(shift_point); + } + + // offset front side from reference path + const bool offset_back = false; + if (!path_shifter.generate(&shifted_path, offset_back)) { + continue; + } + + const auto shift_end_idx = + motion_utils::findNearestIndex(shifted_path.path.points, shift_end_point.point.pose); + const auto goal_idx = motion_utils::findNearestIndex(shifted_path.path.points, goal_pose); + if (shift_end_idx && goal_idx) { + // get target shoulder lane + lanelet::ConstLanelet target_shoulder_lanelet; + lanelet::utils::query::getClosestLanelet( + shoulder_lanes, shifted_path.path.points.back().point.pose, &target_shoulder_lanelet); + + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + + // add road lane_ids if not found + for (const auto id : road_lane_reference_path.points.back().lane_ids) { + if (std::find(point.lane_ids.begin(), point.lane_ids.end(), id) == point.lane_ids.end()) { + point.lane_ids.push_back(id); + } + } + + // add shoulder lane_id if not found + if ( + std::find(point.lane_ids.begin(), point.lane_ids.end(), target_shoulder_lanelet.id()) == + point.lane_ids.end()) { + point.lane_ids.push_back(target_shoulder_lanelet.id()); + } + + // set velocity + if (i < *shift_end_idx) { + // set velocity during shift + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + road_lane_reference_path.points.back().point.longitudinal_velocity_mps); + continue; + } else if (i >= *goal_idx) { + // set velocity after goal + point.point.longitudinal_velocity_mps = 0.0; + continue; + } + point.point.longitudinal_velocity_mps = pull_over_velocity; + } + + candidate_path.straight_path = road_lane_reference_path; + candidate_path.path = + pull_over_utils::combineReferencePath(road_lane_reference_path, shifted_path.path); + // shift path is connected to one, so partial_paths have only one + candidate_path.partial_paths.push_back( + pull_over_utils::combineReferencePath(road_lane_reference_path, shifted_path.path)); + candidate_path.shifted_path = shifted_path; + shift_point.start_idx = path_shifter.getShiftPoints().front().start_idx; + shift_point.end_idx = path_shifter.getShiftPoints().front().end_idx; + candidate_path.start_pose = path_shifter.getShiftPoints().front().start; + candidate_path.end_pose = path_shifter.getShiftPoints().front().end; + candidate_path.shifted_path.shift_length = shifted_path.shift_length; + candidate_path.shift_point = shift_point; + candidate_path.preparation_length = straight_distance; + candidate_path.pull_over_length = pull_over_distance; + } else { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("pull_over").get_child("util"), + "lane change end idx not found on target path."); + continue; + } + + candidate_paths.push_back(candidate_path); + } + + return candidate_paths; +} + +std::vector ShiftPullOver::selectValidPaths( + const std::vector & paths, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const bool is_in_goal_route_section, + const Pose & goal_pose) const +{ + // combine road and shoulder lanes + lanelet::ConstLanelets lanes = road_lanes; + lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + + std::vector available_paths; + for (const auto & path : paths) { + if (!hasEnoughDistance(path, road_lanes, is_in_goal_route_section, goal_pose)) { + continue; + } + + if (lane_departure_checker_.checkPathWillLeaveLane(lanes, path.shifted_path.path)) { + continue; + } + + available_paths.push_back(path); + } + + return available_paths; +} + +bool ShiftPullOver::hasEnoughDistance( + const PullOverPath & path, const lanelet::ConstLanelets & road_lanes, + const bool is_in_goal_route_section, const Pose & goal_pose) const +{ + const auto & current_pose = planner_data_->self_pose->pose; + const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; + + if (!pull_over_utils::hasEnoughDistanceToParkingStart( + path.path, current_pose, path.start_pose, current_vel, parameters_.maximum_deceleration, + parameters_.decide_path_distance, planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold)) { + return false; + } + + const double pull_over_prepare_distance = path.preparation_length; + const double pull_over_distance = path.pull_over_length; + const double pull_over_total_distance = pull_over_prepare_distance + pull_over_distance; + + if (pull_over_total_distance > util::getDistanceToEndOfLane(current_pose, road_lanes)) { + return false; + } + + if ( + is_in_goal_route_section && + pull_over_total_distance > util::getSignedDistance(current_pose, goal_pose, road_lanes)) { + return false; + } + + return true; +} + +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index bbe78c0d3459c..49b9579b2c217 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -34,6 +34,7 @@ #include #include +using motion_utils::calcSignedArcLength; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPoint; @@ -52,283 +53,6 @@ PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLa return path; } -std::vector generateShiftParkingPaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, const Pose & current_pose, const Pose & goal_pose, - const BehaviorPathPlannerParameters & common_parameter, const PullOverParameters & parameter) -{ - std::vector candidate_paths; - - if (original_lanelets.empty() || target_lanelets.empty()) { - return candidate_paths; - } - - // rename parameter - const double backward_path_length = common_parameter.backward_path_length; - const double pull_over_velocity = parameter.pull_over_velocity; - const double after_pull_over_straight_distance = parameter.after_pull_over_straight_distance; - const double margin = parameter.margin_from_boundary; - const double minimum_lateral_jerk = parameter.minimum_lateral_jerk; - const double maximum_lateral_jerk = parameter.maximum_lateral_jerk; - const double deceleration_interval = parameter.deceleration_interval; - const int pull_over_sampling_num = parameter.pull_over_sampling_num; - const double jerk_resolution = - std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_over_sampling_num; - - const double distance_to_shoulder_lane_boundary = - util::getDistanceToShoulderBoundary(route_handler.getShoulderLanelets(), current_pose); - const double offset_from_current_pose = - distance_to_shoulder_lane_boundary + common_parameter.vehicle_width / 2 + margin; - - // shift end point in shoulder lane - PathPointWithLaneId shift_end_point; - { - const auto arc_position_goal = lanelet::utils::getArcCoordinates(target_lanelets, goal_pose); - const double s_start = arc_position_goal.length - after_pull_over_straight_distance; - const double s_end = s_start + std::numeric_limits::epsilon(); - const auto path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end, true); - shift_end_point = path.points.front(); - } - - for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; - lateral_jerk += jerk_resolution) { - PathShifter path_shifter; - ShiftedPath shifted_path; - ShiftParkingPath candidate_path; - - double pull_over_distance = path_shifter.calcLongitudinalDistFromJerk( - abs(offset_from_current_pose), lateral_jerk, pull_over_velocity); - - // calculate straight distance before pull over - double straight_distance; - { - const auto arc_position_goal = - lanelet::utils::getArcCoordinates(original_lanelets, goal_pose); - const auto arc_position_pose = - lanelet::utils::getArcCoordinates(original_lanelets, current_pose); - straight_distance = arc_position_goal.length - after_pull_over_straight_distance - - pull_over_distance - arc_position_pose.length; - } - - PathWithLaneId road_lane_reference_path; - { - const auto arc_position = lanelet::utils::getArcCoordinates(original_lanelets, current_pose); - const auto arc_position_ref2_front = - lanelet::utils::getArcCoordinates(original_lanelets, shift_end_point.point.pose); - const double s_start = arc_position.length - backward_path_length; - const double s_end = arc_position_ref2_front.length - pull_over_distance; - road_lane_reference_path = route_handler.getCenterLinePath(original_lanelets, s_start, s_end); - // decelerate velocity linearly to minimum pull over velocity - // ( or accelerate if original velocity is lower than minimum velocity ) - for (auto & point : road_lane_reference_path.points) { - const auto arclength = - lanelet::utils::getArcCoordinates(original_lanelets, point.point.pose).length; - const double distance_to_pull_over_start = std::max(0.0, s_end - arclength); - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - static_cast( - (distance_to_pull_over_start / deceleration_interval) * - (point.point.longitudinal_velocity_mps - pull_over_velocity) + - pull_over_velocity)); - } - } - - if (road_lane_reference_path.points.empty()) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_over").get_child("util"), - "reference path is empty!! something wrong..."); - continue; - } - - PathWithLaneId target_lane_reference_path; - { - const lanelet::ArcCoordinates pull_over_start_arc_position = - lanelet::utils::getArcCoordinates( - target_lanelets, road_lane_reference_path.points.back().point.pose); - const double s_start = pull_over_start_arc_position.length; - const auto arc_position_goal = lanelet::utils::getArcCoordinates(target_lanelets, goal_pose); - const double s_end = arc_position_goal.length; - target_lane_reference_path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); - // distance between shoulder lane's left boundary and shoulder lane center - double distance_shoulder_to_left_bound = util::getDistanceToShoulderBoundary( - route_handler.getShoulderLanelets(), shift_end_point.point.pose); - - // distance between shoulder lane center and target line - double distance_shoulder_to_target = - distance_shoulder_to_left_bound + common_parameter.vehicle_width / 2 + margin; - - // Apply shifting shoulder lane to adjust to target line - double offset = -distance_shoulder_to_target; - for (size_t i = 0; i < target_lane_reference_path.points.size(); ++i) { - { - if (fabs(offset) < 1.0e-8) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_over").get_child("util"), - "no offset from current lane center."); - } - - auto & p = target_lane_reference_path.points.at(i).point.pose; - double yaw = tf2::getYaw(p.orientation); - p.position.x -= std::sin(yaw) * offset; - p.position.y += std::cos(yaw) * offset; - } - path_shifter.setPath(util::resamplePathWithSpline(target_lane_reference_path, 1.0)); - } - } - ShiftPoint shift_point; - { - shift_point.start = road_lane_reference_path.points.back().point.pose; - shift_point.end = shift_end_point.point.pose; - - // distance between shoulder lane's left boundary and current lane center - double distance_road_to_left_boundary = util::getDistanceToShoulderBoundary( - route_handler.getShoulderLanelets(), road_lane_reference_path.points.back().point.pose); - // distance between shoulder lane's left boundary and current lane center - double distance_road_to_target = - distance_road_to_left_boundary + common_parameter.vehicle_width / 2 + margin; - - shift_point.length = distance_road_to_target; - path_shifter.addShiftPoint(shift_point); - } - - // offset front side from reference path - bool offset_back = false; - if (!path_shifter.generate(&shifted_path, offset_back)) { - continue; - } - - const auto shift_end_idx = - motion_utils::findNearestIndex(shifted_path.path.points, shift_end_point.point.pose); - const auto goal_idx = motion_utils::findNearestIndex(shifted_path.path.points, goal_pose); - if (shift_end_idx && goal_idx) { - // get target shoulder lane - lanelet::ConstLanelet target_shoulder_lanelet; - lanelet::utils::query::getClosestLanelet( - target_lanelets, shifted_path.path.points.back().point.pose, &target_shoulder_lanelet); - - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - - // add road lane_ids if not found - for (const auto id : road_lane_reference_path.points.back().lane_ids) { - if (std::find(point.lane_ids.begin(), point.lane_ids.end(), id) == point.lane_ids.end()) { - point.lane_ids.push_back(id); - } - } - - // add shoulder lane_id if not found - if ( - std::find(point.lane_ids.begin(), point.lane_ids.end(), target_shoulder_lanelet.id()) == - point.lane_ids.end()) { - point.lane_ids.push_back(target_shoulder_lanelet.id()); - } - - // set velocity - if (i < *shift_end_idx) { - // set velocity during shift - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - road_lane_reference_path.points.back().point.longitudinal_velocity_mps); - continue; - } else if (i >= *goal_idx) { - // set velocity after goal - point.point.longitudinal_velocity_mps = 0.0; - continue; - } - point.point.longitudinal_velocity_mps = pull_over_velocity; - } - - candidate_path.straight_path = road_lane_reference_path; - candidate_path.path = combineReferencePath(road_lane_reference_path, shifted_path.path); - candidate_path.shifted_path = shifted_path; - shift_point.start_idx = path_shifter.getShiftPoints().front().start_idx; - shift_point.end_idx = path_shifter.getShiftPoints().front().end_idx; - candidate_path.shifted_path.shift_length = shifted_path.shift_length; - candidate_path.shift_point = shift_point; - // candidate_path.acceleration = acceleration; - candidate_path.preparation_length = straight_distance; - candidate_path.pull_over_length = pull_over_distance; - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_over").get_child("util"), - "lane change end idx not found on target path."); - continue; - } - - candidate_paths.push_back(candidate_path); - } - - return candidate_paths; -} - -std::vector selectValidPaths( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const bool is_in_goal_route_section, const Pose & goal_pose, - const lane_departure_checker::LaneDepartureChecker & lane_departure_checker) -{ - // combine road and shoulder lanes - lanelet::ConstLanelets lanes = current_lanes; - lanes.insert(lanes.end(), target_lanes.begin(), target_lanes.end()); - - std::vector available_paths; - for (const auto & path : paths) { - if (!hasEnoughDistance( - path, current_lanes, current_pose, is_in_goal_route_section, goal_pose)) { - continue; - } - - if (lane_departure_checker.checkPathWillLeaveLane(lanes, path.shifted_path.path)) { - continue; - } - - available_paths.push_back(path); - } - - return available_paths; -} - -bool selectSafePath( - const std::vector & paths, - const OccupancyGridBasedCollisionDetector & occupancy_grid_map, ShiftParkingPath & selected_path) -{ - for (const auto & path : paths) { - if (!occupancy_grid_map.hasObstacleOnPath(path.shifted_path.path, false)) { - selected_path = path; - return true; - } - } - - // set first path for force pull over if no valid path found - if (!paths.empty()) { - selected_path = paths.front(); - return false; - } - - return false; -} - -bool hasEnoughDistance( - const ShiftParkingPath & path, const lanelet::ConstLanelets & current_lanes, - const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose) -{ - const double pull_over_prepare_distance = path.preparation_length; - const double pull_over_distance = path.pull_over_length; - const double pull_over_total_distance = pull_over_prepare_distance + pull_over_distance; - - if (pull_over_total_distance > util::getDistanceToEndOfLane(current_pose, current_lanes)) { - return false; - } - - if ( - is_in_goal_route_section && - pull_over_total_distance > util::getSignedDistance(current_pose, goal_pose, current_lanes)) { - return false; - } - - return true; -} - lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler) { lanelet::ConstLanelets pull_over_lanes; @@ -344,6 +68,34 @@ lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler) return pull_over_lanes; } +bool hasEnoughDistanceToParkingStart( + const PathWithLaneId & path, const Pose & current_pose, const Pose & start_pose, + const double current_vel, const double maximum_deceleration, const double decide_path_distance, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) +{ + const size_t ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + const size_t start_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, start_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + const double dist_to_start_pose = calcSignedArcLength( + path.points, current_pose.position, ego_segment_idx, start_pose.position, start_segment_idx); + + // const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; + const double current_to_stop_distance = std::pow(current_vel, 2) / maximum_deceleration / 2; + + // once stopped, it cannot start again if start_pose is close. + // so need enough distance to restart + constexpr double eps_vel = 0.01; + // dist to restart should be less than decide_path_distance. + // otherwise, the goal would change immediately after departure. + const double dist_to_restart = decide_path_distance / 2; + if (std::abs(current_vel) < eps_vel && dist_to_start_pose < dist_to_restart) { + return false; + } + + return dist_to_start_pose > current_to_stop_distance; +} + Marker createPullOverAreaMarker( const Pose & start_pose, const Pose & end_pose, const int32_t id, const std_msgs::msg::Header & header, const double base_link2front, const double base_link2rear, diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index d50f79d67035b..1ba5d2469cd51 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -446,11 +446,14 @@ std::vector GeometricParallelParking::planOneTrial( paths_.push_back(path_turn_left); paths_.push_back(path_turn_right); + // set pull_over start and end pose + // todo: make start and end pose for pull_out + start_pose_ = start_pose; + arc_end_pose_ = arc_end_pose; + // debug Cr_ = Cr; Cl_ = Cl; - start_pose_ = start_pose; - arc_end_pose_ = arc_end_pose; return paths_; } From 51af89c37d7db8b88b4e238fb7099afa394c4212 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Tue, 27 Sep 2022 11:05:20 +0900 Subject: [PATCH 10/38] fix(tier4_autoware_utils): fix covariance enum name (#1950) * fix(tier4_autoware_utils): change XYZRPY covariance enum name Signed-off-by: scepter914 * fix comment Signed-off-by: scepter914 * fix covariance index Signed-off-by: scepter914 * fix covariance index Signed-off-by: scepter914 * fix typo Signed-off-by: scepter914 * fix covariance variable name Signed-off-by: scepter914 Signed-off-by: scepter914 --- .../ros/msg_covariance.hpp | 8 +-- .../src/radar_fusion_to_detected_object.cpp | 2 +- .../radar_tracks_msgs_converter_node.cpp | 67 ++++++++++--------- 3 files changed, 40 insertions(+), 37 deletions(-) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp index fdaaf84d8efbb..4ebf81d4bfda5 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp @@ -54,14 +54,14 @@ enum RPY_COV_IDX { }; } // namespace rpy_covariance_index -namespace pose_covariance_index +namespace xyzrpy_covariance_index { -/// Covariance for 6-DOF pose. +/// Covariance for 6-DOF. /// Used at /// - geometry_msgs/msg/AccelWithCovariance.msg: msg.covariance /// - geometry_msgs/msg/TwistWithCovariance.msg: msg.covariance /// - geometry_msgs/msg/PoseWithCovariance.msg: msg.covariance -enum POSE_COV_IDX { +enum XYZRPY_COV_IDX { X_X = 0, X_Y = 1, X_Z = 2, @@ -99,7 +99,7 @@ enum POSE_COV_IDX { YAW_PITCH = 34, YAW_YAW = 35 }; -} // namespace pose_covariance_index +} // namespace xyzrpy_covariance_index namespace xyz_upper_covariance_index { diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 396696ad188b1..4f0933e993de8 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -133,7 +133,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( bool RadarFusionToDetectedObject::hasTwistCovariance( const TwistWithCovariance & twist_with_covariance) { - using IDX = tier4_autoware_utils::pose_covariance_index::POSE_COV_IDX; + using IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; auto covariance = twist_with_covariance.covariance; if (covariance[IDX::X_X] == 0.0 && covariance[IDX::Y_Y] == 0.0 && covariance[IDX::Z_Z] == 0.0) { return false; diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index 42465ad4d9851..cfe6ca3e4cea0 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -162,8 +162,8 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() TrackedObjects tracked_objects; tracked_objects.header = radar_data_->header; tracked_objects.header.frame_id = node_param_.new_frame_id; - using P_IDX = tier4_autoware_utils::pose_covariance_index::POSE_COV_IDX; - using R_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; for (auto & radar_track : radar_data_->tracks) { TrackedObject tracked_object; @@ -187,16 +187,17 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; { - auto & covariance = kinematics.pose_with_covariance.covariance; - covariance[P_IDX::X_X] = radar_track.position_covariance[R_IDX::X_X]; - covariance[P_IDX::X_Y] = radar_track.position_covariance[R_IDX::X_Y]; - covariance[P_IDX::X_Z] = radar_track.position_covariance[R_IDX::X_Z]; - covariance[P_IDX::Y_X] = radar_track.position_covariance[R_IDX::X_Y]; - covariance[P_IDX::Y_Y] = radar_track.position_covariance[R_IDX::Y_Y]; - covariance[P_IDX::Y_Z] = radar_track.position_covariance[R_IDX::Y_Z]; - covariance[P_IDX::Z_X] = radar_track.position_covariance[R_IDX::X_Z]; - covariance[P_IDX::Z_Y] = radar_track.position_covariance[R_IDX::Y_Z]; - covariance[P_IDX::Z_Z] = radar_track.position_covariance[R_IDX::Z_Z]; + auto & pose_cov = kinematics.pose_with_covariance.covariance; + auto & radar_position_cov = radar_track.position_covariance; + pose_cov[POSE_IDX::X_X] = radar_position_cov[RADAR_IDX::X_X]; + pose_cov[POSE_IDX::X_Y] = radar_position_cov[RADAR_IDX::X_Y]; + pose_cov[POSE_IDX::X_Z] = radar_position_cov[RADAR_IDX::X_Z]; + pose_cov[POSE_IDX::Y_X] = radar_position_cov[RADAR_IDX::X_Y]; + pose_cov[POSE_IDX::Y_Y] = radar_position_cov[RADAR_IDX::Y_Y]; + pose_cov[POSE_IDX::Y_Z] = radar_position_cov[RADAR_IDX::Y_Z]; + pose_cov[POSE_IDX::Z_X] = radar_position_cov[RADAR_IDX::X_Z]; + pose_cov[POSE_IDX::Z_Y] = radar_position_cov[RADAR_IDX::Y_Z]; + pose_cov[POSE_IDX::Z_Z] = radar_position_cov[RADAR_IDX::Z_Z]; } // convert by tf @@ -218,28 +219,30 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() } { - auto & covariance = kinematics.twist_with_covariance.covariance; - covariance[P_IDX::X_X] = radar_track.velocity_covariance[R_IDX::X_X]; - covariance[P_IDX::X_Y] = radar_track.velocity_covariance[R_IDX::X_Y]; - covariance[P_IDX::X_Z] = radar_track.velocity_covariance[R_IDX::X_Z]; - covariance[P_IDX::Y_X] = radar_track.velocity_covariance[R_IDX::X_Y]; - covariance[P_IDX::Y_Y] = radar_track.velocity_covariance[R_IDX::Y_Y]; - covariance[P_IDX::Y_Z] = radar_track.velocity_covariance[R_IDX::Y_Z]; - covariance[P_IDX::Z_X] = radar_track.velocity_covariance[R_IDX::X_Z]; - covariance[P_IDX::Z_Y] = radar_track.velocity_covariance[R_IDX::Y_Z]; - covariance[P_IDX::Z_Z] = radar_track.velocity_covariance[R_IDX::Z_Z]; + auto & twist_cov = kinematics.twist_with_covariance.covariance; + auto & radar_vel_cov = radar_track.velocity_covariance; + twist_cov[POSE_IDX::X_X] = radar_vel_cov[RADAR_IDX::X_X]; + twist_cov[POSE_IDX::X_Y] = radar_vel_cov[RADAR_IDX::X_Y]; + twist_cov[POSE_IDX::X_Z] = radar_vel_cov[RADAR_IDX::X_Z]; + twist_cov[POSE_IDX::Y_X] = radar_vel_cov[RADAR_IDX::X_Y]; + twist_cov[POSE_IDX::Y_Y] = radar_vel_cov[RADAR_IDX::Y_Y]; + twist_cov[POSE_IDX::Y_Z] = radar_vel_cov[RADAR_IDX::Y_Z]; + twist_cov[POSE_IDX::Z_X] = radar_vel_cov[RADAR_IDX::X_Z]; + twist_cov[POSE_IDX::Z_Y] = radar_vel_cov[RADAR_IDX::Y_Z]; + twist_cov[POSE_IDX::Z_Z] = radar_vel_cov[RADAR_IDX::Z_Z]; } { - auto & covariance = kinematics.acceleration_with_covariance.covariance; - covariance[P_IDX::X_X] = radar_track.acceleration_covariance[R_IDX::X_X]; - covariance[P_IDX::X_Y] = radar_track.acceleration_covariance[R_IDX::X_Y]; - covariance[P_IDX::X_Z] = radar_track.acceleration_covariance[R_IDX::X_Z]; - covariance[P_IDX::Y_X] = radar_track.acceleration_covariance[R_IDX::X_Y]; - covariance[P_IDX::Y_Y] = radar_track.acceleration_covariance[R_IDX::Y_Y]; - covariance[P_IDX::Y_Z] = radar_track.acceleration_covariance[R_IDX::Y_Z]; - covariance[P_IDX::Z_X] = radar_track.acceleration_covariance[R_IDX::X_Z]; - covariance[P_IDX::Z_Y] = radar_track.acceleration_covariance[R_IDX::Y_Z]; - covariance[P_IDX::Z_Z] = radar_track.acceleration_covariance[R_IDX::Z_Z]; + auto & accel_cov = kinematics.acceleration_with_covariance.covariance; + auto & radar_accel_cov = radar_track.acceleration_covariance; + accel_cov[POSE_IDX::X_X] = radar_accel_cov[RADAR_IDX::X_X]; + accel_cov[POSE_IDX::X_Y] = radar_accel_cov[RADAR_IDX::X_Y]; + accel_cov[POSE_IDX::X_Z] = radar_accel_cov[RADAR_IDX::X_Z]; + accel_cov[POSE_IDX::Y_X] = radar_accel_cov[RADAR_IDX::X_Y]; + accel_cov[POSE_IDX::Y_Y] = radar_accel_cov[RADAR_IDX::Y_Y]; + accel_cov[POSE_IDX::Y_Z] = radar_accel_cov[RADAR_IDX::Y_Z]; + accel_cov[POSE_IDX::Z_X] = radar_accel_cov[RADAR_IDX::X_Z]; + accel_cov[POSE_IDX::Z_Y] = radar_accel_cov[RADAR_IDX::Y_Z]; + accel_cov[POSE_IDX::Z_Z] = radar_accel_cov[RADAR_IDX::Z_Z]; } tracked_object.kinematics = kinematics; From 88f1dfd6220ffefba47c1d26a6bd6d8ff2788d2d Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 27 Sep 2022 11:48:34 +0900 Subject: [PATCH 11/38] fix(behavior_path_planner): fix bug of turn signal with overlap lane (#1886) Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../motion_utils/trajectory/trajectory.hpp | 35 ++++++++++- .../test/src/trajectory/test_trajectory.cpp | 61 +++++++++++++++++++ .../behavior_path_planner/utilities.hpp | 3 +- 3 files changed, 95 insertions(+), 4 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 705542e769d81..5cc81aa04b750 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -347,7 +347,8 @@ boost::optional findNearestSegmentIndex( */ template double calcLateralOffset( - const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false) + const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false) { const auto overlap_removed_points = removeOverlapPoints(points, 0); @@ -371,8 +372,6 @@ double calcLateralOffset( return std::nan(""); } - const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target); - const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)); const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); @@ -383,6 +382,36 @@ double calcLateralOffset( return cross_vec(2) / segment_vec.norm(); } +template +double calcLateralOffset( + const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false) +{ + const auto overlap_removed_points = removeOverlapPoints(points, 0); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return std::nan(""); + } + } + + if (overlap_removed_points.size() == 1) { + const std::runtime_error e("Same points are given."); + if (throw_exception) { + throw e; + } + std::cerr << e.what() << std::endl; + return std::nan(""); + } + + const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target); + return calcLateralOffset(points, p_target, seg_idx, throw_exception); +} + /** * @brief calcSignedArcLength from index to index */ diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 12628934bc2bb..9bdcf1208540a 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -564,6 +564,67 @@ TEST(trajectory, calcLateralOffset) EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0)), -3.0, epsilon); } +TEST(trajectory, calcLateralOffset_without_segment_idx) +{ + using motion_utils::calcLateralOffset; + + const auto traj = generateTestTrajectory(10, 1.0); + const bool throw_exception = true; + + // Empty + EXPECT_THROW( + calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}, throw_exception), + std::invalid_argument); + + // Trajectory size is 1 + { + const auto one_point_traj = generateTestTrajectory(1, 1.0); + EXPECT_THROW( + calcLateralOffset( + one_point_traj.points, geometry_msgs::msg::Point{}, static_cast(0), + throw_exception), + std::runtime_error); + } + + // Same close points in trajectory + { + const auto invalid_traj = generateTestTrajectory(10, 0.0); + const auto p = createPoint(3.0, 0.0, 0.0); + EXPECT_THROW( + calcLateralOffset(invalid_traj.points, p, static_cast(2), throw_exception), + std::runtime_error); + EXPECT_THROW( + calcLateralOffset(invalid_traj.points, p, static_cast(3), throw_exception), + std::runtime_error); + } + + // Point on trajectory + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(3.1, 0.0, 0.0), static_cast(3)), 0.0, + epsilon); + + // Point before start point + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(-3.9, 3.0, 0.0), static_cast(0)), 3.0, + epsilon); + + // Point after start point + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(13.3, -10.0, 0.0), static_cast(8)), -10.0, + epsilon); + + // Random cases + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(4.3, 7.0, 0.0), static_cast(4)), 7.0, + epsilon); + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast(0)), -3.0, + epsilon); + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast(1)), -3.0, + epsilon); +} + TEST(trajectory, calcLateralOffset_CurveTrajectory) { using motion_utils::calcLateralOffset; 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 b399fa5197ba0..f44a0de79a056 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -119,7 +119,8 @@ FrenetCoordinate3d convertToFrenetCoordinate3d( motion_utils::calcLongitudinalOffsetToSegment(pose_array, seg_idx, search_point_geom); frenet_coordinate.length = motion_utils::calcSignedArcLength(pose_array, 0, seg_idx) + longitudinal_length; - frenet_coordinate.distance = motion_utils::calcLateralOffset(pose_array, search_point_geom); + frenet_coordinate.distance = + motion_utils::calcLateralOffset(pose_array, search_point_geom, seg_idx); return frenet_coordinate; } From 82bc75ad6ab0b98358bb141c8878166c1c498367 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 27 Sep 2022 17:45:14 +0900 Subject: [PATCH 12/38] refactor(tier4_planning_rviz_plugin): apply clang-tidy for path (#1637) --- .../include/path/display.hpp | 15 +++++++------ .../src/path/display.cpp | 22 +++++++++++-------- 2 files changed, 21 insertions(+), 16 deletions(-) diff --git a/common/tier4_planning_rviz_plugin/include/path/display.hpp b/common/tier4_planning_rviz_plugin/include/path/display.hpp index 2d7aeba5c7cfb..0029d13369a3e 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display.hpp @@ -45,7 +45,7 @@ class AutowarePathDisplay public: AutowarePathDisplay(); - virtual ~AutowarePathDisplay(); + ~AutowarePathDisplay() override; void onInitialize() override; void reset() override; @@ -56,12 +56,12 @@ private Q_SLOTS: protected: void processMessage( const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) override; - std::unique_ptr setColorDependsOnVelocity( + static std::unique_ptr setColorDependsOnVelocity( const double vel_max, const double cmd_vel); - std::unique_ptr gradation( + static std::unique_ptr gradation( const QColor & color_min, const QColor & color_max, const double ratio); - Ogre::ManualObject * path_manual_object_; - Ogre::ManualObject * velocity_manual_object_; + Ogre::ManualObject * path_manual_object_{nullptr}; + Ogre::ManualObject * velocity_manual_object_{nullptr}; rviz_common::properties::BoolProperty * property_path_view_; rviz_common::properties::BoolProperty * property_velocity_view_; rviz_common::properties::FloatProperty * property_path_width_; @@ -74,9 +74,10 @@ private Q_SLOTS: rviz_common::properties::BoolProperty * property_velocity_color_view_; rviz_common::properties::FloatProperty * property_vel_max_; -private: +private: // NOLINT for Qt autoware_auto_planning_msgs::msg::Path::ConstSharedPtr last_msg_ptr_; - bool validateFloats(const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr); + static bool validateFloats( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr); }; } // namespace rviz_plugins diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 41714d87588ff..02d6623cc5b6e 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -26,9 +26,10 @@ std::unique_ptr AutowarePathDisplay::gradation( const QColor & color_min, const QColor & color_max, const double ratio) { std::unique_ptr color_ptr(new Ogre::ColourValue); - color_ptr->g = color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio); - color_ptr->r = color_max.redF() * ratio + color_min.redF() * (1.0 - ratio); - color_ptr->b = color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio); + color_ptr->g = + static_cast(color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio)); + color_ptr->r = static_cast(color_max.redF() * ratio + color_min.redF() * (1.0 - ratio)); + color_ptr->b = static_cast(color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio)); return color_ptr; } @@ -183,7 +184,8 @@ void AutowarePathDisplay::processMessage( color = *dynamic_color_ptr; } color.a = property_path_alpha_->getFloat(); - Eigen::Vector3f vec_in, vec_out; + Eigen::Vector3f vec_in; + Eigen::Vector3f vec_out; Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); { vec_in << 0, (property_path_width_->getFloat() / 2.0), 0; @@ -195,8 +197,9 @@ void AutowarePathDisplay::processMessage( } vec_out = quat * vec_in; path_manual_object_->position( - path_point.pose.position.x + vec_out.x(), path_point.pose.position.y + vec_out.y(), - path_point.pose.position.z + vec_out.z()); + static_cast(path_point.pose.position.x) + vec_out.x(), + static_cast(path_point.pose.position.y) + vec_out.y(), + static_cast(path_point.pose.position.z) + vec_out.z()); path_manual_object_->colour(color); } { @@ -209,8 +212,9 @@ void AutowarePathDisplay::processMessage( } vec_out = quat * vec_in; path_manual_object_->position( - path_point.pose.position.x + vec_out.x(), path_point.pose.position.y + vec_out.y(), - path_point.pose.position.z + vec_out.z()); + static_cast(path_point.pose.position.x) + vec_out.x(), + static_cast(path_point.pose.position.y) + vec_out.y(), + static_cast(path_point.pose.position.z) + vec_out.z()); path_manual_object_->colour(color); } } @@ -231,7 +235,7 @@ void AutowarePathDisplay::processMessage( velocity_manual_object_->position( path_point.pose.position.x, path_point.pose.position.y, - path_point.pose.position.z + + static_cast(path_point.pose.position.z) + path_point.longitudinal_velocity_mps * property_velocity_scale_->getFloat()); velocity_manual_object_->colour(color); } From d5610fd342e585034e8aaba2f4547e6bd86098e4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 27 Sep 2022 17:47:26 +0900 Subject: [PATCH 13/38] feat(tier4_planning_rviz_plugin): add owner (#1953) --- common/tier4_planning_rviz_plugin/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index 233bc78b3cbfa..a140123035acf 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -5,6 +5,7 @@ 0.1.0 The tier4_planning_rviz_plugin package Yukihiro Saito + Takayuki Murooka Apache License 2.0 ament_cmake From e3292d2251df9c56a1b788c53acb55fd3b98f8dc Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 27 Sep 2022 18:07:23 +0900 Subject: [PATCH 14/38] refactor(run_out): add state machine class for state transition (#1884) * refactor(run_out): add state machine class for state transition Signed-off-by: Tomohito Ando * remove debug print Signed-off-by: Tomohito Ando * move parameters Signed-off-by: Tomohito Ando * add missing parameter Signed-off-by: Tomohito Ando * add documents Signed-off-by: Tomohito Ando * fix conflict Signed-off-by: Tomohito Ando * remove unused argument Signed-off-by: Tomohito Ando * fix parameter value Signed-off-by: Tomohito Ando Signed-off-by: Tomohito Ando --- .../run_out.param.yaml | 9 +- .../config/run_out.param.yaml | 10 +- .../run_out/insert_velocity_to_approach.svg | 4 + .../docs/run_out/state_transition.svg | 4 + .../include/scene_module/run_out/scene.hpp | 13 ++- .../scene_module/run_out/state_machine.hpp | 58 ++++++++++ .../include/scene_module/run_out/utils.hpp | 13 ++- .../run-out-design.md | 31 ++++-- .../src/scene_module/run_out/manager.cpp | 11 +- .../src/scene_module/run_out/scene.cpp | 69 ++++++------ .../scene_module/run_out/state_machine.cpp | 100 ++++++++++++++++++ 11 files changed, 255 insertions(+), 67 deletions(-) create mode 100644 planning/behavior_velocity_planner/docs/run_out/insert_velocity_to_approach.svg create mode 100644 planning/behavior_velocity_planner/docs/run_out/state_transition.svg create mode 100644 planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp create mode 100644 planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml index cb64617dca8e4..44f623b96bb69 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml @@ -30,9 +30,12 @@ enable: false margin: 0.0 # [m] distance on how close ego approaches the obstacle limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping - stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping - stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state - dist_thresh: 0.5 # [m] end the approaching state if distance to the obstacle is longer than stop_margin + dist_thresh + + # parameters for the change of state. used only when approaching is enabled + state: + stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping + stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state + disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value # parameter to avoid sudden stopping slow_down_limit: diff --git a/planning/behavior_velocity_planner/config/run_out.param.yaml b/planning/behavior_velocity_planner/config/run_out.param.yaml index 2782e4d14df56..44f623b96bb69 100644 --- a/planning/behavior_velocity_planner/config/run_out.param.yaml +++ b/planning/behavior_velocity_planner/config/run_out.param.yaml @@ -23,15 +23,19 @@ height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points max_prediction_time: 10.0 # [sec] create predicted path until this time time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path + points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method # approach if ego has stopped in the front of the obstacle for a certain amount of time approaching: enable: false margin: 0.0 # [m] distance on how close ego approaches the obstacle limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping - stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping - stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state - dist_thresh: 0.5 # [m] end the approaching state if distance to the obstacle is longer than stop_margin + dist_thresh + + # parameters for the change of state. used only when approaching is enabled + state: + stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping + stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state + disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value # parameter to avoid sudden stopping slow_down_limit: diff --git a/planning/behavior_velocity_planner/docs/run_out/insert_velocity_to_approach.svg b/planning/behavior_velocity_planner/docs/run_out/insert_velocity_to_approach.svg new file mode 100644 index 0000000000000..e76da5ce3193d --- /dev/null +++ b/planning/behavior_velocity_planner/docs/run_out/insert_velocity_to_approach.svg @@ -0,0 +1,4 @@ + + + +

Collision point

Collision point

v

v

x

x

Output velocity

Output velocity

Reference velocity

Reference velocity

Stop point

Stop point
Approach
margin
Approach...
Base link
to front
Base link...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/run_out/state_transition.svg b/planning/behavior_velocity_planner/docs/run_out/state_transition.svg new file mode 100644 index 0000000000000..aad4bdd34c4cd --- /dev/null +++ b/planning/behavior_velocity_planner/docs/run_out/state_transition.svg @@ -0,0 +1,4 @@ + + + +
GO
GO
STOP
STOP
APPROACH
APPROACH
Current velocity is
less than threshold
Current velocity is...
Stop time is
larger than threshold
Stop time is...
Current velocity is
larger than threshold
Current velocity is...
There are no obstacles or
distance to the obstacle is
larger than threshold
There are no obstacles or...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp index d9d6ae95c6e68..060e93df1d7b4 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp @@ -17,6 +17,7 @@ #include "scene_module/run_out/debug.hpp" #include "scene_module/run_out/dynamic_obstacle.hpp" +#include "scene_module/run_out/state_machine.hpp" #include "scene_module/run_out/utils.hpp" #include "scene_module/scene_module_interface.hpp" @@ -31,7 +32,6 @@ using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using run_out_utils::PlannerParam; -using run_out_utils::State; using tier4_debug_msgs::msg::Float32Stamped; using BasicPolygons2d = std::vector; @@ -58,11 +58,10 @@ class RunOutModule : public SceneModuleInterface PlannerParam planner_param_; // Variable - State state_{State::GO}; - rclcpp::Time stop_time_; BasicPolygons2d partition_lanelets_; std::unique_ptr dynamic_obstacle_creator_; std::shared_ptr debug_ptr_; + std::unique_ptr state_machine_; // Function Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const; @@ -116,10 +115,10 @@ class RunOutModule : public SceneModuleInterface const boost::optional stop_point, autoware_auto_planning_msgs::msg::PathWithLaneId & path); - void insertVelocity( - const boost::optional & dynamic_obstacle, - const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, - const PathWithLaneId & smoothed_path, PathWithLaneId & output_path); + void insertVelocityForState( + const boost::optional & dynamic_obstacle, const PlannerData planner_data, + const PlannerParam & planner_param, const PathWithLaneId & smoothed_path, + PathWithLaneId & output_path); void insertStoppingVelocity( const boost::optional & dynamic_obstacle, diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp new file mode 100644 index 0000000000000..e378e5b385428 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp @@ -0,0 +1,58 @@ +// 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__RUN_OUT__STATE_MACHINE_HPP_ +#define SCENE_MODULE__RUN_OUT__STATE_MACHINE_HPP_ + +#include "scene_module/run_out/utils.hpp" + +#include +#include + +namespace behavior_velocity_planner +{ +namespace run_out_utils +{ + +class StateMachine +{ +public: + enum class State { + GO = 0, + STOP, + APPROACH, + UNKNOWN, + }; + + struct StateInput + { + double current_velocity; + double dist_to_collision; + boost::optional current_obstacle; + }; + + explicit StateMachine(const StateParam & state_param) { state_param_ = state_param; } + State getCurrentState() const { return state_; } + std::string toString(const State & state) const; + void updateState(const StateInput & state_input, rclcpp::Clock & clock); + +private: + StateParam state_param_; + State state_{State::GO}; + rclcpp::Time stop_time_; +}; +} // namespace run_out_utils +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__RUN_OUT__STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp index 40f9a1213a889..357ec74c9d0a6 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp @@ -75,9 +75,13 @@ struct ApproachingParam bool enable; float margin; float limit_vel_kmph; +}; + +struct StateParam +{ float stop_thresh; float stop_time_thresh; - float dist_thresh; + float disable_approach_dist; }; struct SlowDownLimit @@ -99,17 +103,12 @@ struct PlannerParam VehicleParam vehicle_param; DetectionArea detection_area; ApproachingParam approaching; + StateParam state_param; DynamicObstacleParam dynamic_obstacle; SlowDownLimit slow_down_limit; Smoother smoother; }; -enum class State { - GO = 0, - APPROACH, - STOP, -}; - enum class DetectionMethod { Object = 0, ObjectWithoutPath, diff --git a/planning/behavior_velocity_planner/run-out-design.md b/planning/behavior_velocity_planner/run-out-design.md index 41bb71f0c4766..2772a39a363c3 100644 --- a/planning/behavior_velocity_planner/run-out-design.md +++ b/planning/behavior_velocity_planner/run-out-design.md @@ -137,6 +137,18 @@ If the collision is detected, stop point is inserted on distance of base link to ![brief](./docs/run_out/insert_velocity.svg) +#### Insert velocity to approach the obstacles + +If you select the method of `Points` or `ObjectWithoutPath`, sometimes ego keeps stopping in front of the obstacle. +To avoid this problem, This feature has option to approach the obstacle with slow velocity after stopping. +If the parameter of `approaching.enable` is set to true, ego will approach the obstacle after ego stopped for `state.stop_time_thresh` seconds. +The maximum velocity of approaching can be specified by the parameter of `approaching.limit_vel_kmph`. +The decision to approach the obstacle is determined by a simple state transition as following image. + +![brief](./docs/run_out/insert_velocity_to_approach.svg) + +![brief](./docs/run_out/state_transition.svg) + ##### Limit velocity with specified jerk and acc limit The maximum slowdown velocity is calculated in order not to slowdown too much. @@ -172,14 +184,17 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab | `time_step` | double | [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path | | `points_interval` | double | [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method | -| Parameter /approaching | Type | Description | -| ---------------------- | ------ | -------------------------------------------------------------------------------------------------- | -| `enable` | bool | [-] whether to enable approaching after stopping | -| `margin` | double | [m] distance on how close ego approaches the obstacle | -| `limit_vel_kmph` | double | [km/h] limit velocity for approaching after stopping | -| `stop_thresh` | double | [m/s] threshold to decide if ego is stopping | -| `stop_time_thresh` | double | [sec] threshold for stopping time to transit to approaching state | -| `dist_thresh` | double | [m] end the approaching state if distance to the obstacle is longer than stop_margin + dist_thresh | +| Parameter /approaching | Type | Description | +| ---------------------- | ------ | ----------------------------------------------------- | +| `enable` | bool | [-] whether to enable approaching after stopping | +| `margin` | double | [m] distance on how close ego approaches the obstacle | +| `limit_vel_kmph` | double | [km/h] limit velocity for approaching after stopping | + +| Parameter /state | Type | Description | +| ----------------------- | ------ | ----------------------------------------------------------------------------------- | +| `stop_thresh` | double | [m/s] threshold to decide if ego is stopping | +| `stop_time_thresh` | double | [sec] threshold for stopping time to transit to approaching state | +| `disable_approach_dist` | double | [m] end the approaching state if distance to the obstacle is longer than this value | | Parameter /slow_down_limit | Type | Description | | -------------------------- | ------ | ------------------------------------------------------------- | diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp index 5670d8c8be6ab..43846ba767e1a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp @@ -86,9 +86,14 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) p.enable = node.declare_parameter(ns_a + ".enable", false); p.margin = node.declare_parameter(ns_a + ".margin", 0.0); p.limit_vel_kmph = node.declare_parameter(ns_a + ".limit_vel_kmph", 5.0); - p.stop_thresh = node.declare_parameter(ns_a + ".stop_thresh", 0.01); - p.stop_time_thresh = node.declare_parameter(ns_a + ".stop_time_thresh", 3.0); - p.dist_thresh = node.declare_parameter(ns_a + ".dist_thresh", 0.5); + } + + { + auto & p = planner_param_.state_param; + const std::string ns_s = ns + ".state"; + p.stop_thresh = node.declare_parameter(ns_s + ".stop_thresh", 0.01); + p.stop_time_thresh = node.declare_parameter(ns_s + ".stop_time_thresh", 3.0); + p.disable_approach_dist = node.declare_parameter(ns_s + ".disable_approach_dist", 4.0); } { 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 0b57d1f20bad5..74bc9b8ac1bc9 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 @@ -30,7 +30,8 @@ RunOutModule::RunOutModule( : SceneModuleInterface(module_id, logger, clock), planner_param_(planner_param), dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), - debug_ptr_(debug_ptr) + debug_ptr_(debug_ptr), + state_machine_(std::make_unique(planner_param.state_param)) { if (planner_param.run_out.use_partition_lanelet) { const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); @@ -100,8 +101,8 @@ bool RunOutModule::modifyPathVelocity( if (planner_param_.approaching.enable) { // after a certain amount of time has elapsed since the ego stopped, // approach the obstacle with slow velocity - insertVelocity( - dynamic_obstacle, current_pose, current_vel, current_acc, trim_smoothed_path, *path); + insertVelocityForState( + dynamic_obstacle, *planner_data_, planner_param_, trim_smoothed_path, *path); } else { // just insert zero velocity for stopping insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, *path); @@ -648,57 +649,53 @@ void RunOutModule::insertStopPoint( planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); } -void RunOutModule::insertVelocity( - const boost::optional & dynamic_obstacle, - const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, - const PathWithLaneId & smoothed_path, PathWithLaneId & output_path) +void RunOutModule::insertVelocityForState( + const boost::optional & dynamic_obstacle, const PlannerData planner_data, + const PlannerParam & planner_param, const PathWithLaneId & smoothed_path, + PathWithLaneId & output_path) { - // no obstacles - if (!dynamic_obstacle) { - state_ = State::GO; - return; - } + using State = run_out_utils::StateMachine::State; + + const auto & current_pose = planner_data.current_pose.pose; + const auto & current_vel = planner_data.current_velocity->twist.linear.x; + const auto & current_acc = planner_data.current_acceleration->accel.accel.linear.x; - const auto longitudinal_offset_to_collision_point = + // set data to judge the state + run_out_utils::StateMachine::StateInput state_input; + state_input.current_velocity = current_vel; + state_input.current_obstacle = dynamic_obstacle; + state_input.dist_to_collision = motion_utils::calcSignedArcLength( smoothed_path.points, current_pose.position, dynamic_obstacle->nearest_collision_point) - - planner_param_.vehicle_param.base_to_front; - // enough distance to the obstacle - if ( - longitudinal_offset_to_collision_point > - planner_param_.run_out.stop_margin + planner_param_.approaching.dist_thresh) { - state_ = State::GO; + planner_param.vehicle_param.base_to_front; + + // update state + state_machine_->updateState(state_input, *clock_); + + // get updated state and target obstacle to decelerate + const auto state = state_machine_->getCurrentState(); + + // no obstacles to decelerate + if (!dynamic_obstacle) { + return; } - switch (state_) { + // insert velocity for each state + switch (state) { case State::GO: { - if ( - planner_data_->current_velocity->twist.linear.x < planner_param_.approaching.stop_thresh) { - RCLCPP_DEBUG_STREAM(logger_, "transit to STOP state."); - stop_time_ = clock_->now(); - state_ = State::STOP; - } - insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path); break; } case State::STOP: { - RCLCPP_DEBUG_STREAM(logger_, "STOP state"); - const auto elapsed_time = (clock_->now() - stop_time_).seconds(); - state_ = - elapsed_time > planner_param_.approaching.stop_time_thresh ? State::APPROACH : State::STOP; - RCLCPP_DEBUG_STREAM(logger_, "elapsed time: " << elapsed_time); - insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path); break; } case State::APPROACH: { - RCLCPP_DEBUG_STREAM(logger_, "APPROACH state"); insertApproachingVelocity( - *dynamic_obstacle, current_pose, planner_param_.approaching.limit_vel_kmph / 3.6, - planner_param_.approaching.margin, output_path); + *dynamic_obstacle, current_pose, planner_param.approaching.limit_vel_kmph / 3.6, + planner_param.approaching.margin, output_path); debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); break; } diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp new file mode 100644 index 0000000000000..f2a92ac7dcdac --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp @@ -0,0 +1,100 @@ +// 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 "scene_module/run_out/state_machine.hpp" + +namespace behavior_velocity_planner +{ +namespace run_out_utils +{ +using State = StateMachine::State; + +std::string StateMachine::toString(const State & state) const +{ + switch (state) { + case State::GO: + return "GO"; + + case State::STOP: + return "STOP"; + + case State::APPROACH: + return "APPROACH"; + + default: + return "UNKNOWN"; + } +} + +void StateMachine::updateState(const StateInput & state_input, rclcpp::Clock & clock) +{ + // no obstacles + if (!state_input.current_obstacle) { + state_ = State::GO; + return; + } + + switch (state_) { + case State::GO: { + // if current velocity is less than the threshold, transit to STOP state + if (state_input.current_velocity < state_param_.stop_thresh) { + stop_time_ = clock.now(); + state_ = State::STOP; + return; + } + + // continue GO state + return; + } + + case State::STOP: { + // if current velocity is larger than the threshold, transit to STOP state + if (state_input.current_velocity > state_param_.stop_thresh) { + state_ = State::GO; + return; + } + + // if STOP state continues for a certain time, transit to APPROACH state + const auto elapsed_time = (clock.now() - stop_time_).seconds(); + if (elapsed_time > state_param_.stop_time_thresh) { + state_ = State::APPROACH; + return; + } + + // continue STOP state + return; + } + + case State::APPROACH: { + // if the obstacle is far enough from ego, transit to GO state + const bool enough_dist_from_obstacle = + state_input.dist_to_collision > state_param_.disable_approach_dist; + if (enough_dist_from_obstacle) { + state_ = State::GO; + return; + } + + // continue APPROACH state + return; + } + + default: { + state_ = State::UNKNOWN; + return; + } + } +} + +} // namespace run_out_utils +} // namespace behavior_velocity_planner From 8f9d39b09b601f6a599fd42d485dfcce05cb009b Mon Sep 17 00:00:00 2001 From: Yusuke Muramatsu Date: Tue, 27 Sep 2022 18:53:20 +0900 Subject: [PATCH 15/38] feat(lidar_centerpoint): add IoU-based NMS (#1935) * feat(lidar_centerpoint): add IoU-based NMS Signed-off-by: yukke42 * feat: add magic number name Signed-off-by: yukke42 * feat: remove unnecessary headers Signed-off-by: yukke42 * fix: typo Signed-off-by: yukke42 * fix: typo Signed-off-by: yukke42 Signed-off-by: yukke42 --- perception/lidar_centerpoint/CMakeLists.txt | 5 +- perception/lidar_centerpoint/README.md | 23 ++-- .../config/centerpoint.param.yaml | 5 + .../config/centerpoint_tiny.param.yaml | 5 + .../include/lidar_centerpoint/node.hpp | 3 + .../postprocess/non_maximum_suppression.hpp | 81 ++++++++++++++ .../postprocess/non_maximum_suppression.cpp | 105 ++++++++++++++++++ perception/lidar_centerpoint/package.xml | 1 + perception/lidar_centerpoint/src/node.cpp | 25 ++++- 9 files changed, 237 insertions(+), 16 deletions(-) create mode 100644 perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp create mode 100644 perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 646267b0d7a61..c512e06b3bf0e 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -70,12 +70,12 @@ message(STATUS "start to download") if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # Download trained models set(DATA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/data) - set(DONWLOAD_BASE_URL https://awf.ml.dev.web.auto/perception/models/centerpoint) + set(DOWNLOAD_BASE_URL https://awf.ml.dev.web.auto/perception/models/centerpoint) execute_process(COMMAND mkdir -p ${DATA_PATH}) function(download VERSION FILE_NAME FILE_HASH) message(STATUS "Checking and downloading ${FILE_NAME} ") - set(DOWNLOAD_URL ${DONWLOAD_BASE_URL}/${VERSION}/${FILE_NAME}) + set(DOWNLOAD_URL ${DOWNLOAD_BASE_URL}/${VERSION}/${FILE_NAME}) set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) set(STATUS_CODE 0) message(STATUS "start ${FILE_NAME}") @@ -130,6 +130,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) lib/ros_utils.cpp lib/network/network_trt.cpp lib/network/tensorrt_wrapper.cpp + lib/postprocess/non_maximum_suppression.cpp lib/preprocess/pointcloud_densification.cpp lib/preprocess/voxel_generator.cpp ) diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index 0957749e16c83..72eb5498b71d4 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -30,16 +30,19 @@ We trained the models using . ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------------- | ------ | ------------- | ----------------------------------------------------------- | -| `score_threshold` | float | `0.4` | detected objects with score less than threshold are ignored | -| `densification_world_frame_id` | string | `map` | the world frame id to fuse multi-frame pointcloud | -| `densification_num_past_frames` | int | `1` | the number of past frames to fuse with the current frame | -| `trt_precision` | string | `fp16` | TensorRT inference precision: `fp32` or `fp16` | -| `encoder_onnx_path` | string | `""` | path to VoxelFeatureEncoder ONNX file | -| `encoder_engine_path` | string | `""` | path to VoxelFeatureEncoder TensorRT Engine file | -| `head_onnx_path` | string | `""` | path to DetectionHead ONNX file | -| `head_engine_path` | string | `""` | path to DetectionHead TensorRT Engine file | +| Name | Type | Default Value | Description | +| ------------------------------- | ------------ | ------------- | ------------------------------------------------------------- | +| `score_threshold` | float | `0.4` | detected objects with score less than threshold are ignored | +| `densification_world_frame_id` | string | `map` | the world frame id to fuse multi-frame pointcloud | +| `densification_num_past_frames` | int | `1` | the number of past frames to fuse with the current frame | +| `trt_precision` | string | `fp16` | TensorRT inference precision: `fp32` or `fp16` | +| `encoder_onnx_path` | string | `""` | path to VoxelFeatureEncoder ONNX file | +| `encoder_engine_path` | string | `""` | path to VoxelFeatureEncoder TensorRT Engine file | +| `head_onnx_path` | string | `""` | path to DetectionHead ONNX file | +| `head_engine_path` | string | `""` | path to DetectionHead TensorRT Engine file | +| `nms_iou_target_class_names` | list[string] | - | target classes for IoU-based Non Maximum Suppression | +| `nms_iou_search_distance_2d` | double | - | If two objects are farther than the value, NMS isn't applied. | +| `nms_iou_threshold` | double | - | IoU threshold for the IoU-based Non Maximum Suppression | ## Assumptions / Known limits diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml index 3810c864738af..565c769a08c72 100644 --- a/perception/lidar_centerpoint/config/centerpoint.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint.param.yaml @@ -7,3 +7,8 @@ voxel_size: [0.32, 0.32, 8.0] downsample_factor: 1 encoder_in_feature_size: 9 + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index c93b59ac0b8f9..e5ae31efc775d 100644 --- a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml @@ -7,3 +7,8 @@ voxel_size: [0.32, 0.32, 8.0] downsample_factor: 2 encoder_in_feature_size: 9 + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index 7477442a1060d..474c9884eb36f 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -15,6 +15,8 @@ #ifndef LIDAR_CENTERPOINT__NODE_HPP_ #define LIDAR_CENTERPOINT__NODE_HPP_ +#include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" + #include #include #include @@ -52,6 +54,7 @@ class LidarCenterPointNode : public rclcpp::Node std::vector class_names_; bool has_twist_{false}; + NonMaximumSuppression iou_bev_nms_; DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp new file mode 100644 index 0000000000000..78e362f77e7f1 --- /dev/null +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp @@ -0,0 +1,81 @@ +// 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 LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#define LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ + +#include "lidar_centerpoint/ros_utils.hpp" + +#include + +#include "autoware_auto_perception_msgs/msg/detected_object.hpp" + +#include +#include + +namespace centerpoint +{ +using autoware_auto_perception_msgs::msg::DetectedObject; + +// TODO(yukke42): now only support IoU_BEV +enum class NMS_TYPE { + IoU_BEV + // IoU_3D + // Distance_2D + // Distance_3D +}; + +struct NMSParams +{ + NMS_TYPE nms_type_{}; + std::vector target_class_names_{}; + double search_distance_2d_{}; + double iou_threshold_{}; + // double distance_threshold_{}; +}; + +std::vector classNamesToBooleanMask(const std::vector & class_names) +{ + std::vector mask; + constexpr std::size_t num_object_classification = 8; + mask.resize(num_object_classification); + for (const auto & class_name : class_names) { + const auto semantic_type = getSemanticType(class_name); + mask.at(semantic_type) = true; + } + + return mask; +} + +class NonMaximumSuppression +{ +public: + void setParameters(const NMSParams &); + + std::vector apply(const std::vector &); + +private: + bool isTargetLabel(const std::uint8_t); + + bool isTargetPairObject(const DetectedObject &, const DetectedObject &); + + Eigen::MatrixXd generateIoUMatrix(const std::vector &); + + NMSParams params_{}; + std::vector target_class_mask_{}; +}; + +} // namespace centerpoint + +#endif // LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp new file mode 100644 index 0000000000000..82b9ca673061f --- /dev/null +++ b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -0,0 +1,105 @@ +// 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 "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" + +#include "perception_utils/geometry.hpp" +#include "perception_utils/perception_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +namespace centerpoint +{ + +void NonMaximumSuppression::setParameters(const NMSParams & params) +{ + assert(params.target_class_names_.size() == 8); + assert(params.search_distance_2d_ >= 0.0); + assert(params.iou_threshold_ >= 0.0 && params.iou_threshold_ <= 1.0); + + params_ = params; + target_class_mask_ = classNamesToBooleanMask(params.target_class_names_); +} + +bool NonMaximumSuppression::isTargetLabel(const uint8_t label) +{ + if (label >= target_class_mask_.size()) { + return false; + } + return target_class_mask_.at(label); +} + +bool NonMaximumSuppression::isTargetPairObject( + const DetectedObject & object1, const DetectedObject & object2) +{ + const auto label1 = perception_utils::getHighestProbLabel(object1.classification); + const auto label2 = perception_utils::getHighestProbLabel(object2.classification); + + if (isTargetLabel(label1) && isTargetLabel(label2)) { + return true; + } + + const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; + const auto sqr_dist_2d = tier4_autoware_utils::calcSquaredDistance2d( + perception_utils::getPose(object1), perception_utils::getPose(object2)); + return sqr_dist_2d <= search_sqr_dist_2d; +} + +Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix( + const std::vector & input_objects) +{ + // NOTE(yukke42): row = target objects to be suppressed, col = source objects to be compared + Eigen::MatrixXd triangular_matrix = + Eigen::MatrixXd::Zero(input_objects.size(), input_objects.size()); + for (std::size_t target_i = 0; target_i < input_objects.size(); ++target_i) { + for (std::size_t source_i = 0; source_i < target_i; ++source_i) { + const auto & target_obj = input_objects.at(target_i); + const auto & source_obj = input_objects.at(source_i); + if (!isTargetPairObject(target_obj, source_obj)) { + continue; + } + + if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { + const double iou = perception_utils::get2dIoU(target_obj, source_obj); + triangular_matrix(target_i, source_i) = iou; + // NOTE(yukke42): If the target object has any objects with iou > iou_threshold, it + // will be suppressed regardless of later results. + if (iou > params_.iou_threshold_) { + break; + } + } + } + } + + return triangular_matrix; +} + +std::vector NonMaximumSuppression::apply( + const std::vector & input_objects) +{ + Eigen::MatrixXd iou_matrix = generateIoUMatrix(input_objects); + + std::vector output_objects; + output_objects.reserve(input_objects.size()); + for (std::size_t i = 0; i < input_objects.size(); ++i) { + const auto value = iou_matrix.row(i).maxCoeff(); + if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { + if (value <= params_.iou_threshold_) { + output_objects.emplace_back(input_objects.at(i)); + } + } + } + + return output_objects; +} +} // namespace centerpoint diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 71e8f71f6212f..4372b1848d08b 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -13,6 +13,7 @@ autoware_auto_perception_msgs pcl_ros + perception_utils rclcpp rclcpp_components tf2_eigen diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 9901f9a325953..f85b9006a7dee 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -26,6 +26,9 @@ #include #endif +#include +#include + #include #include #include @@ -38,7 +41,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti const float score_threshold = static_cast(this->declare_parameter("score_threshold", 0.35)); const float circle_nms_dist_threshold = - static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); + static_cast(this->declare_parameter("circle_nms_dist_threshold")); const float yaw_norm_threshold = static_cast(this->declare_parameter("yaw_norm_threshold", 0.0)); const std::string densification_world_frame_id = @@ -71,6 +74,16 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + { + NMSParams p; + p.nms_type_ = NMS_TYPE::IoU_BEV; + p.target_class_names_ = + this->declare_parameter>("iou_nms_target_class_names"); + p.search_distance_2d_ = this->declare_parameter("iou_nms_search_distance_2d"); + p.iou_threshold_ = this->declare_parameter("iou_nms_threshold"); + iou_bev_nms_.setParameters(p); + } + NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); DensificationParam densification_param( @@ -129,14 +142,18 @@ void LidarCenterPointNode::pointCloudCallback( return; } - autoware_auto_perception_msgs::msg::DetectedObjects output_msg; - output_msg.header = input_pointcloud_msg->header; + std::vector raw_objects; + raw_objects.reserve(det_boxes3d.size()); for (const auto & box3d : det_boxes3d) { autoware_auto_perception_msgs::msg::DetectedObject obj; box3DToDetectedObject(box3d, class_names_, has_twist_, obj); - output_msg.objects.emplace_back(obj); + raw_objects.emplace_back(obj); } + autoware_auto_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = input_pointcloud_msg->header; + output_msg.objects = iou_bev_nms_.apply(raw_objects); + detection_class_remapper_.mapClasses(output_msg); if (objects_sub_count > 0) { From 8e86f0e817ea0384b56b57ae9441312d5694a149 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 28 Sep 2022 00:57:59 +0900 Subject: [PATCH 16/38] fix(behavior_path_planner): pull over dies after curve (#1963) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../pull_over/shift_pull_over.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp index c83da0f0bd64f..0bba6b45bc473 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp @@ -24,7 +24,6 @@ #include namespace behavior_path_planner - { ShiftPullOver::ShiftPullOver( rclcpp::Node & node, const PullOverParameters & parameters, @@ -108,15 +107,21 @@ std::vector ShiftPullOver::generatePullOverPaths( const double jerk_resolution = std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_over_sampling_num; - const double distance_to_shoulder_lane_boundary = - util::getDistanceToShoulderBoundary(shoulder_lanes, current_pose); - const double offset_from_current_pose = - distance_to_shoulder_lane_boundary + common_parameters.vehicle_width / 2 + margin; + // calc lateral offset from road lane center line to shoulder target line. + lanelet::ConstLanelet goal_closest_road_lane; + lanelet::utils::query::getClosestLanelet(road_lanes, goal_pose, &goal_closest_road_lane); + const auto closest_center_pose = + lanelet::utils::getClosestCenterPose(goal_closest_road_lane, goal_pose.position); + const double distance_from_shoulder_left_bound = + util::getDistanceToShoulderBoundary(shoulder_lanes, closest_center_pose); + const double offset_from_road_line_center = + distance_from_shoulder_left_bound + common_parameters.vehicle_width / 2 + margin; // shift end point in shoulder lane const auto shift_end_point = std::invoke([&]() { const auto arc_position_goal = lanelet::utils::getArcCoordinates(shoulder_lanes, goal_pose); - const double s_start = arc_position_goal.length - after_pull_over_straight_distance; + const double s_start = + std::max(arc_position_goal.length - after_pull_over_straight_distance, 0.0); const double s_end = s_start + std::numeric_limits::epsilon(); const auto path = route_handler->getCenterLinePath(shoulder_lanes, s_start, s_end, true); return path.points.front(); @@ -130,7 +135,7 @@ std::vector ShiftPullOver::generatePullOverPaths( PullOverPath candidate_path; const double pull_over_distance = path_shifter.calcLongitudinalDistFromJerk( - abs(offset_from_current_pose), lateral_jerk, pull_over_velocity); + std::abs(offset_from_road_line_center), lateral_jerk, pull_over_velocity); // calculate straight distance before pull over const double straight_distance = std::invoke([&]() { From 4e739ca61b231f890946059dbce62d02b0e203d6 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 28 Sep 2022 12:15:54 +0900 Subject: [PATCH 17/38] chore(behavior_velocity): add maintainer for run out module (#1967) Signed-off-by: Tomohito Ando Signed-off-by: Tomohito Ando --- launch/tier4_planning_launch/package.xml | 1 + planning/behavior_velocity_planner/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 96b5d05c21d35..7033ab6aa87af 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -42,6 +42,7 @@ Kosuke Takeuchi Tomohito Ando + Makoto Kurihara Taiki Tanaka diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 035835d2e2ed0..c648e25a055d2 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -25,6 +25,7 @@ Kosuke Takeuchi Tomohito Ando + Makoto Kurihara Tomoya Kimura From 023fdda5705314e65e6afb4acc7838e3e7e7235a Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Wed, 28 Sep 2022 13:29:24 +0900 Subject: [PATCH 18/38] chore: update CODEOWNERS (#1966) Signed-off-by: GitHub Signed-off-by: GitHub Co-authored-by: TomohitoAndo --- .github/CODEOWNERS | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index dcc03b291d1d8..97b27a10e3010 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -34,7 +34,7 @@ common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp common/tier4_debug_tools/** kenji.miyake@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp -common/tier4_planning_rviz_plugin/** yukihiro.saito@tier4.jp +common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp @@ -63,7 +63,7 @@ launch/tier4_control_launch/** takamasa.horibe@tier4.jp launch/tier4_localization_launch/** yamato.ando@tier4.jp launch/tier4_map_launch/** ryohsuke.mitsudome@tier4.jp launch/tier4_perception_launch/** yukihiro.saito@tier4.jp -launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp +launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp launch/tier4_system_launch/** fumihito.ito@tier4.jp kenji.miyake@tier4.jp @@ -109,7 +109,7 @@ perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp planning/costmap_generator/** kenji.miyake@tier4.jp planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp planning/freespace_planner/** takamasa.horibe@tier4.jp From 30e968eaa3f4e92727543ccabae8befc39d048cd Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 28 Sep 2022 14:41:25 +0900 Subject: [PATCH 19/38] fix(behavior_path_planner): shift pull over path distortion with resampling (#1969) * fix(behavior_path_planner): shift pull over path distortion with resampling Signed-off-by: kosuke55 * fix from review Signed-off-by: kosuke55 * fix typo Signed-off-by: kosuke55 Signed-off-by: kosuke55 Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> --- .../src/scene_module/pull_over/shift_pull_over.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp index 0bba6b45bc473..46d0cef7bada2 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp @@ -167,6 +167,10 @@ std::vector ShiftPullOver::generatePullOverPaths( pull_over_velocity)); } } + // resample road straight path and shift source path respectively + constexpr double resample_interval{1.0}; + road_lane_reference_path = + util::resamplePathWithSpline(road_lane_reference_path, resample_interval); if (road_lane_reference_path.points.empty()) { RCLCPP_ERROR_STREAM( @@ -196,18 +200,14 @@ std::vector ShiftPullOver::generatePullOverPaths( const double offset = -distance_shoulder_to_target; for (size_t i = 0; i < target_lane_reference_path.points.size(); ++i) { { - if (fabs(offset) < 1.0e-8) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_over").get_child("util"), - "no offset from current lane center."); - } - auto & p = target_lane_reference_path.points.at(i).point.pose; p = tier4_autoware_utils::calcOffsetPose(p, 0, offset, 0); } - path_shifter.setPath(util::resamplePathWithSpline(target_lane_reference_path, 1.0)); } } + path_shifter.setPath( + util::resamplePathWithSpline(target_lane_reference_path, resample_interval)); + ShiftPoint shift_point; { shift_point.start = road_lane_reference_path.points.back().point.pose; From 87f8142efba0c3120f686792afc96932312ad8ad Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 28 Sep 2022 18:03:59 +0900 Subject: [PATCH 20/38] fix(kalman_filter): add more maintainers (#1977) Signed-off-by: kminoda Signed-off-by: kminoda --- common/kalman_filter/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/common/kalman_filter/package.xml b/common/kalman_filter/package.xml index 84887dc4e5804..950319c9ea282 100644 --- a/common/kalman_filter/package.xml +++ b/common/kalman_filter/package.xml @@ -5,6 +5,8 @@ 0.1.0 The kalman filter package Yukihiro Saito + Takeshi Ishita + Koji Minoda Apache License 2.0 Takamasa Horibe From 49d15a3794e04cb67946749eb6019e32154deb94 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Wed, 28 Sep 2022 20:52:04 +0900 Subject: [PATCH 21/38] fix(behavior_velocity_planner): do not consider stop_line_margin when a stop line is defined at map in crosswalk module (#1978) Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- .../scene_module/crosswalk/scene_crosswalk.hpp | 2 +- .../scene_module/crosswalk/scene_crosswalk.cpp | 15 ++++++++++----- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp index 7d011862b633c..40c6c58f24ec2 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp @@ -111,7 +111,7 @@ class CrosswalkModule : public SceneModuleInterface const PathWithLaneId & ego_path, StopFactor & stop_factor); boost::optional> getStopLine( - const PathWithLaneId & ego_path) const; + const PathWithLaneId & ego_path, bool & exist_stopline_in_map) const; std::vector getCollisionPoints( const PathWithLaneId & ego_path, const PredictedObject & object, 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 99ef42304ada4..12b657e347d7e 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 @@ -230,11 +230,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } boost::optional> CrosswalkModule::getStopLine( - const PathWithLaneId & ego_path) const + const PathWithLaneId & ego_path, bool & exist_stopline_in_map) const { const auto & ego_pos = planner_data_->current_pose.pose.position; const auto stop_line = getStopLineFromMap(module_id_, planner_data_, "crosswalk_id"); + exist_stopline_in_map = static_cast(stop_line); if (stop_line) { const auto intersects = getLinestringIntersects( ego_path, lanelet::utils::to2D(stop_line.get()).basicLineString(), ego_pos, 2); @@ -263,13 +264,15 @@ boost::optional CrosswalkModule::findRTCStopPoint( { const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto p_stop_line = getStopLine(ego_path); + bool exist_stopline_in_map; + const auto p_stop_line = getStopLine(ego_path, exist_stopline_in_map); if (!p_stop_line) { return {}; } const auto & p_stop = p_stop_line.get().second; - const auto margin = planner_param_.stop_line_distance + base_link2front; + const auto stop_line_distance = exist_stopline_in_map ? 0.0 : planner_param_.stop_line_distance; + const auto margin = stop_line_distance + base_link2front; const auto stop_pose = calcLongitudinalOffsetPose(ego_path.points, p_stop, -margin); if (!stop_pose) { @@ -292,7 +295,8 @@ boost::optional CrosswalkModule::findNearestStopPoint const auto & objects_ptr = planner_data_->predicted_objects; const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto p_stop_line = getStopLine(ego_path); + bool exist_stopline_in_map; + const auto p_stop_line = getStopLine(ego_path, exist_stopline_in_map); if (!p_stop_line) { return {}; } @@ -397,7 +401,8 @@ boost::optional CrosswalkModule::findNearestStopPoint const auto stop_at_stop_line = !found_pedestrians || within_stop_line_margin; const auto & p_stop = stop_at_stop_line ? p_stop_line.get().second : first_stop_point; - const auto margin = stop_at_stop_line ? planner_param_.stop_line_distance + base_link2front + const auto stop_line_distance = exist_stopline_in_map ? 0.0 : planner_param_.stop_line_distance; + const auto margin = stop_at_stop_line ? stop_line_distance + base_link2front : planner_param_.stop_margin + base_link2front; const auto stop_pose = calcLongitudinalOffsetPose(ego_path.points, p_stop, -margin); From 342a959f9a323dac8b31898d53f0a9d0bd33c6b2 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 28 Sep 2022 21:35:05 +0900 Subject: [PATCH 22/38] feat(behavior_path_planner): add new turn signal algorithm (#1964) * clean code Signed-off-by: yutaka * clean format Signed-off-by: yutaka * udpate Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * add test Signed-off-by: yutaka * add test * add test Signed-off-by: yutaka * fix(avoidance): use new turn signal algorithm Signed-off-by: satoshi-ota * fix(avoidance): fix desired_start_point position Signed-off-by: satoshi-ota * update Signed-off-by: yutaka * change policy Signed-off-by: yutaka * feat(behavior_path_planner): update pull_over for new blinker logic Signed-off-by: kosuke55 * feat(behavior_path_planner): update pull_out for new blinker logic * tmp: install flask via pip Signed-off-by: Takayuki Murooka * feat(lane_change): added lane change point * fix start_point and non backward driving turn signal Signed-off-by: kosuke55 * get 3 second during constructing lane change path Signed-off-by: Muhammad Zulfaqar Azmi * fix pull over desired start point Signed-off-by: kosuke55 * update Signed-off-by: yutaka * delete Signed-off-by: yutaka * Update Readme * Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe * fix format Signed-off-by: yutaka Signed-off-by: yutaka Signed-off-by: satoshi-ota Signed-off-by: kosuke55 Signed-off-by: Takayuki Murooka Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: satoshi-ota Co-authored-by: kosuke55 Co-authored-by: Takayuki Murooka Co-authored-by: Muhammad Zulfaqar Azmi Co-authored-by: Fumiya Watanabe --- planning/behavior_path_planner/CMakeLists.txt | 8 + planning/behavior_path_planner/README.md | 4 + .../behavior_path_planner_node.hpp | 2 +- .../lane_change/lane_change_module.hpp | 1 + .../lane_change/lane_change_path.hpp | 3 + .../scene_module/lane_change/util.hpp | 13 +- .../scene_module/pull_out/pull_out_module.hpp | 5 +- .../pull_over/pull_over_module.hpp | 5 +- .../scene_module/scene_module_interface.hpp | 18 +- .../turn_signal_decider.hpp | 53 ++- .../src/behavior_path_planner_node.cpp | 7 +- .../avoidance/avoidance_module.cpp | 45 +- .../lane_change/lane_change_module.cpp | 5 +- .../src/scene_module/lane_change/util.cpp | 50 ++- .../scene_module/pull_out/pull_out_module.cpp | 52 ++- .../pull_over/pull_over_module.cpp | 91 ++-- .../src/turn_signal_decider.cpp | 422 ++++++++++++++---- .../test/test_turn_signal.cpp | 400 +++++++++++++++++ 18 files changed, 967 insertions(+), 217 deletions(-) create mode 100644 planning/behavior_path_planner/test/test_turn_signal.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 05b4f1f9436c0..73a4bc7652408 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -67,6 +67,14 @@ if(BUILD_TESTING) behavior_path_planner_node ) + ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_turn_signal + test/test_turn_signal.cpp + ) + + target_link_libraries(test_${CMAKE_PROJECT_NAME}_turn_signal + behavior_path_planner_node + ) + endif() ament_auto_package( diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 35ae0b892befe..ba8b01f27e1fb 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -517,6 +517,10 @@ Turn on signal when the planned path crosses lanes or when a right or left turn When multiple turn signal conditions are met, the turn signal with the smaller distance is selected. +#### Limitation + +Currently, the algorithm of turn signal has been chagned, and the document of the turn signal will be available soon. + ## References / External links This module depends on the external [BehaviorTreeCpp](https://github.com/BehaviorTree/BehaviorTree.CPP) library. 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 8aabe413c0c5b..e926e44cd53b8 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 @@ -175,7 +175,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node bool isForcedCandidatePath() const; /** - * @brief publish the steering factor from intersection + * @brief publish steering factor from intersection */ void publish_steering_factor(const TurnIndicatorsCommand & turn_signal); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index a331269871215..096b9d4cec3c0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#include "behavior_path_planner/turn_signal_decider.hpp" #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp index 9d24060e60eb7..d603fc36f1d28 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" +#include "behavior_path_planner/turn_signal_decider.hpp" #include @@ -24,6 +25,7 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::TurnSignalInfo; struct LaneChangePath { PathWithLaneId path; @@ -32,6 +34,7 @@ struct LaneChangePath double acceleration{0.0}; double preparation_length{0.0}; double lane_change_length{0.0}; + TurnSignalInfo turn_signal_info; }; using LaneChangePaths = std::vector; 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 7d1664a48eec5..3928dca353b4a 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 @@ -61,8 +61,10 @@ std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const PathWithLaneId & target_lane_reference_path, const ShiftPoint & shift_point, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const double & acceleration, const double & prepare_distance, const double & lane_change_distance, - const double & lane_changing_duration, const double & minimum_lane_change_velocity); + const double & acceleration, const double & prepare_distance, const double & prepare_duration, + const double & prepare_speed, const double & minimum_prepare_distance, + const double & lane_change_distance, const double & lane_changing_duration, + const double & minimum_lane_change_velocity); LaneChangePaths getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, @@ -126,6 +128,13 @@ PathWithLaneId getLaneChangePathLaneChangingSegment( const double & lane_change_distance_buffer, const double & lane_changing_duration, const double & minimum_lane_change_velocity); +TurnSignalInfo calc_turn_signal_info( + const PathWithLaneId & prepare_path, const double prepare_velocity, + const double min_prepare_distance, const double prepare_duration, const ShiftPoint & shift_points, + const ShiftedPath & lane_changing_path); + +void get_turn_signal_info( + const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info); } // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index cfbb63c02e073..41f49cf9dd43d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -87,12 +87,11 @@ class PullOutModule : public SceneModuleInterface std::vector> pull_out_planners_; PullOutStatus status_; - std::vector backed_pose_candidates_; - PoseStamped backed_pose_; std::deque odometry_buffer_; std::unique_ptr last_route_received_time_; std::unique_ptr last_pull_out_start_update_time_; + std::unique_ptr last_approved_pose_; std::shared_ptr getCurrentPlanner() const; lanelet::ConstLanelets getCurrentLanes() const; @@ -103,7 +102,7 @@ class PullOutModule : public SceneModuleInterface std::shared_ptr lane_departure_checker_; // turn signal - TurnSignalInfo calcTurnSignalInfo(const Pose start_pose, const Pose end_pose) const; + TurnSignalInfo calcTurnSignalInfo() const; void incrementPathIndex(); PathWithLaneId getCurrentPath() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 36c10fde1e0e9..f85aa225d647c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -125,6 +125,7 @@ class PullOverModule : public SceneModuleInterface tier4_autoware_utils::LinearRing2d vehicle_footprint_; std::unique_ptr last_received_time_; std::unique_ptr last_approved_time_; + std::unique_ptr last_approved_pose_; void incrementPathIndex(); PathWithLaneId getCurrentPath() const; @@ -150,9 +151,7 @@ class PullOverModule : public SceneModuleInterface const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; bool checkCollisionWithPose(const Pose & pose) const; - // turn signal - std::pair getHazardInfo() const; - std::pair getTurnInfo() const; + TurnSignalInfo calcTurnSignalInfo() const; // debug void setDebugData(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index d7f47f5391d15..b8fdd5a01d736 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/utilities.hpp" #include +#include #include #include #include @@ -53,23 +54,6 @@ using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; using PlanResult = PathWithLaneId::SharedPtr; -struct TurnSignalInfo -{ - TurnSignalInfo() - { - turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; - hazard_signal.command = HazardLightsCommand::NO_COMMAND; - } - - // desired turn signal - TurnIndicatorsCommand turn_signal; - HazardLightsCommand hazard_signal; - - // TODO(Horibe) replace with point. Distance should be calculates in turn_signal_decider. - // distance to the turn signal trigger point (to choose nearest signal for multiple requests) - double signal_distance{std::numeric_limits::max()}; -}; - struct BehaviorModuleOutput { BehaviorModuleOutput() = default; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp index ff6f3aed118c2..e67c50a13587e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp @@ -18,27 +18,60 @@ #include #include +#include #include #include #include +#include #include +#include #include namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Pose; using route_handler::RouteHandler; + +struct TurnSignalInfo +{ + TurnSignalInfo() + { + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + hazard_signal.command = HazardLightsCommand::NO_COMMAND; + } + + // desired turn signal + TurnIndicatorsCommand turn_signal; + HazardLightsCommand hazard_signal; + + geometry_msgs::msg::Point desired_start_point; + geometry_msgs::msg::Point desired_end_point; + geometry_msgs::msg::Point required_start_point; + geometry_msgs::msg::Point required_end_point; +}; + +const std::map signal_map = { + {"left", TurnIndicatorsCommand::ENABLE_LEFT}, + {"right", TurnIndicatorsCommand::ENABLE_RIGHT}, + {"straight", TurnIndicatorsCommand::DISABLE}, + {"none", TurnIndicatorsCommand::DISABLE}}; + class TurnSignalDecider { public: TurnIndicatorsCommand getTurnSignal( + const PathWithLaneId & path, const Pose & current_pose, const double current_vel, + const size_t current_seg_idx, const RouteHandler & route_handler, + const TurnSignalInfo & turn_signal_info); + + TurnIndicatorsCommand resolve_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, - const RouteHandler & route_handler, const TurnIndicatorsCommand & turn_signal_plan, - const double plan_distance) const; + const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info); void setParameters(const double base_link2front, const double intersection_search_distance) { @@ -50,9 +83,20 @@ class TurnSignalDecider std::pair getIntersectionPoseAndDistance(); private: - std::pair getIntersectionTurnSignal( + boost::optional getIntersectionTurnSignalInfo( + const PathWithLaneId & path, const Pose & current_pose, const double current_vel, + const size_t current_seg_idx, const RouteHandler & route_handler); + + geometry_msgs::msg::Point get_required_end_point(const lanelet::ConstLineString3d & centerline); + + bool use_prior_turn_signal( + const double dist_to_prior_required_start, const double dist_to_prior_required_end, + const double dist_to_subsequent_required_start, const double dist_to_subsequent_required_end); + + void set_intersection_info( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, - const RouteHandler & route_handler) const; + const TurnSignalInfo & intersection_turn_signal_info); + void initialize_intersection_info(); rclcpp::Logger logger_{ rclcpp::get_logger("behavior_path_planner").get_child("turn_signal_decider")}; @@ -60,6 +104,7 @@ class TurnSignalDecider // data double intersection_search_distance_{0.0}; double base_link2front_{0.0}; + std::map desired_start_point_map_; mutable bool intersection_turn_signal_ = false; mutable bool approaching_intersection_turn_signal_ = false; mutable double intersection_distance_ = std::numeric_limits::max(); 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 1eb43822654c2..173521f051f1a 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -606,10 +606,13 @@ void BehaviorPathPlannerNode::run() turn_signal.command = TurnIndicatorsCommand::DISABLE; hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { + const auto & current_pose = planner_data->self_pose->pose; + const double & current_vel = planner_data->self_odometry->twist.twist.linear.x; const size_t ego_seg_idx = findEgoSegmentIndex(path->points); + turn_signal = turn_signal_decider_.getTurnSignal( - *path, planner_data->self_pose->pose, ego_seg_idx, *(planner_data->route_handler), - output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance); + *path, current_pose, current_vel, ego_seg_idx, *(planner_data->route_handler), + output.turn_signal_info); hazard_signal.command = HazardLightsCommand::DISABLE; } turn_signal.stamp = get_clock()->now(); 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 92db1e8e42792..11d5661ccabc4 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 @@ -2546,25 +2546,44 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con return {}; } - const auto latest_shift_point = shift_points.front(); // assuming it is sorted. + const auto getRelativeLength = [this](const ShiftPoint & sp) { + const auto current_shift = getCurrentShift(); + return sp.length - current_shift; + }; - const auto turn_info = util::getPathTurnSignal( - avoidance_data_.current_lanelets, path, latest_shift_point, planner_data_->self_pose->pose, - planner_data_->self_odometry->twist.twist.linear.x, planner_data_->parameters); + const auto front_shift_point = shift_points.front(); - // Set turn signal if the vehicle across the lane. - if (!path.shift_length.empty()) { - if (isAvoidancePlanRunning()) { - turn_signal.turn_signal.command = turn_info.first.command; - } + TurnSignalInfo turn_signal_info{}; + + if (std::abs(getRelativeLength(front_shift_point)) < 0.1) { + return turn_signal_info; } - // calc distance from ego to latest_shift_point end point. - if (turn_info.second >= 0.0) { - turn_signal.signal_distance = turn_info.second; + const auto signal_prepare_distance = std::max(getEgoSpeed() * 3.0, 10.0); + const auto ego_to_shift_start = + calcSignedArcLength(path.path.points, getEgoPosition(), front_shift_point.start.position); + + if (signal_prepare_distance < ego_to_shift_start) { + return turn_signal_info; } - return turn_signal; + if (getRelativeLength(front_shift_point) > 0.0) { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + + if (ego_to_shift_start > 0.0) { + turn_signal_info.desired_start_point = getEgoPosition(); + } else { + turn_signal_info.desired_start_point = front_shift_point.start.position; + } + + turn_signal_info.desired_end_point = front_shift_point.end.position; + turn_signal_info.required_start_point = front_shift_point.start.position; + turn_signal_info.required_end_point = front_shift_point.end.position; + + return turn_signal_info; } double AvoidanceModule::getCurrentShift() const 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 fc1b99b05fa4d..1a31e6db4dd37 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 @@ -17,6 +17,7 @@ #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/lane_change/util.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utilities.hpp" #include @@ -190,7 +191,9 @@ BehaviorModuleOutput LaneChangeModule::plan() status_.lane_change_path.shift_point, planner_data_->self_pose->pose, planner_data_->self_odometry->twist.twist.linear.x, planner_data_->parameters); output.turn_signal_info.turn_signal.command = turn_signal_info.first.command; - output.turn_signal_info.signal_distance = turn_signal_info.second; + + lane_change_utils::get_turn_signal_info(status_.lane_change_path, &output.turn_signal_info); + // output.turn_signal_info.signal_distance = turn_signal_info.second; return 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 44a521734934b..056204ba5fdf6 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 @@ -87,8 +87,10 @@ std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const PathWithLaneId & target_lane_reference_path, const ShiftPoint & shift_point, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const double & acceleration, const double & prepare_distance, const double & lane_change_distance, - const double & lane_changing_duration, const double & minimum_lane_change_velocity) + const double & acceleration, const double & prepare_distance, const double & prepare_duration, + const double & prepare_speed, const double & minimum_prepare_distance, + const double & lane_change_distance, const double & lane_changing_duration, + const double & minimum_lane_change_velocity) { PathShifter path_shifter; path_shifter.setPath(target_lane_reference_path); @@ -147,6 +149,9 @@ std::optional constructCandidatePath( return std::nullopt; } + candidate_path.turn_signal_info = lane_change_utils::calc_turn_signal_info( + prepare_segment, prepare_speed, minimum_prepare_distance, prepare_duration, shift_point, + shifted_path); // check candidate path is in lanelet if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { return std::nullopt; @@ -241,6 +246,7 @@ LaneChangePaths getLaneChangePaths( const auto candidate_path = constructCandidatePath( prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, shift_point, original_lanelets, target_lanelets, acceleration, prepare_distance, + lane_change_prepare_duration, prepare_speed, minimum_lane_change_prepare_distance, lane_changing_distance, lane_changing_duration, minimum_lane_change_velocity); if (!candidate_path) { @@ -581,4 +587,44 @@ PathWithLaneId getLaneChangePathLaneChangingSegment( return lane_changing_segment; } +TurnSignalInfo calc_turn_signal_info( + const PathWithLaneId & prepare_path, const double prepare_velocity, + const double min_prepare_distance, const double prepare_duration, const ShiftPoint & shift_points, + const ShiftedPath & lane_changing_path) +{ + TurnSignalInfo turn_signal_info{}; + constexpr double turn_signal_start_duration{3.0}; + turn_signal_info.desired_start_point = + std::invoke([&prepare_path, &prepare_velocity, &min_prepare_distance, &prepare_duration]() { + if (prepare_velocity * turn_signal_start_duration > min_prepare_distance) { + const auto duration = static_cast(prepare_path.points.size()) / prepare_duration; + double time{-duration}; + for (auto itr = prepare_path.points.crbegin(); itr != prepare_path.points.crend(); ++itr) { + time += duration; + if (time >= turn_signal_start_duration) { + return itr->point.pose.position; + } + } + } + return prepare_path.points.front().point.pose.position; + }); + + turn_signal_info.required_start_point = shift_points.start.position; + turn_signal_info.required_end_point = std::invoke([&lane_changing_path]() { + const auto mid_path_idx = lane_changing_path.path.points.size() / 2; + return lane_changing_path.path.points.at(mid_path_idx).point.pose.position; + }); + turn_signal_info.desired_end_point = shift_points.end.position; + return turn_signal_info; +} + +void get_turn_signal_info( + const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info) +{ + turn_signal_info->desired_start_point = lane_change_path.turn_signal_info.desired_start_point; + turn_signal_info->required_start_point = lane_change_path.turn_signal_info.required_start_point; + turn_signal_info->required_end_point = lane_change_path.turn_signal_info.required_end_point; + turn_signal_info->desired_end_point = lane_change_path.turn_signal_info.desired_end_point; +} + } // namespace behavior_path_planner::lane_change_utils 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 901378416493c..05d0ffe532833 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 @@ -60,7 +60,6 @@ PullOutModule::PullOutModule( BehaviorModuleOutput PullOutModule::run() { - clearWaitingApproval(); current_state_ = BT::NodeStatus::RUNNING; return plan(); } @@ -155,6 +154,12 @@ BT::NodeStatus PullOutModule::updateState() BehaviorModuleOutput PullOutModule::plan() { + if (isWaitingApproval()) { + clearWaitingApproval(); + // save current_pose when approved for start_point of turn_signal for backward driving + last_approved_pose_ = std::make_unique(planner_data_->self_pose->pose); + } + BehaviorModuleOutput output; if (!status_.is_safe) { return output; @@ -175,8 +180,7 @@ BehaviorModuleOutput PullOutModule::plan() planner_data_->parameters.vehicle_length, planner_data_); output.path = std::make_shared(path); - output.turn_signal_info = - calcTurnSignalInfo(status_.pull_out_path.start_pose, status_.pull_out_path.end_pose); + output.turn_signal_info = calcTurnSignalInfo(); const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -256,6 +260,8 @@ PathWithLaneId PullOutModule::getFullPath() const BehaviorModuleOutput PullOutModule::planWaitingApproval() { + waitApproval(); + BehaviorModuleOutput output; const auto current_lanes = getCurrentLanes(); const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); @@ -272,12 +278,9 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() } output.path = std::make_shared(stop_path); - output.turn_signal_info = - calcTurnSignalInfo(status_.pull_out_path.start_pose, status_.pull_out_path.end_pose); + output.turn_signal_info = calcTurnSignalInfo(); output.path_candidate = std::make_shared(candidate_path); - waitApproval(); - const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -603,35 +606,38 @@ bool PullOutModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -TurnSignalInfo PullOutModule::calcTurnSignalInfo(const Pose start_pose, const Pose end_pose) const +TurnSignalInfo PullOutModule::calcTurnSignalInfo() const { - TurnSignalInfo turn_signal; + TurnSignalInfo turn_signal{}; // output + const auto & current_pose = planner_data_->self_pose->pose; - // turn hazard light when backward driving + // turn on hazard light when backward driving if (!status_.back_finished) { turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; - turn_signal.signal_distance = - tier4_autoware_utils::calcDistance2d(start_pose, planner_data_->self_pose->pose); + const auto back_start_pose = isWaitingApproval() ? current_pose : *last_approved_pose_; + turn_signal.desired_start_point = back_start_pose.position; + turn_signal.required_start_point = back_start_pose.position; + // pull_out start_pose is same to backward driving end_pose + turn_signal.required_end_point = status_.pull_out_path.start_pose.position; + turn_signal.desired_end_point = status_.pull_out_path.start_pose.position; return turn_signal; } - // calculate distance to pull_out end on target lanes - const auto current_lanes = getCurrentLanes(); - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(current_lanes, planner_data_->self_pose->pose); - const auto arc_position_pull_out_end = lanelet::utils::getArcCoordinates(current_lanes, end_pose); - const double distance_from_pull_out_end = - arc_position_current_pose.length - arc_position_pull_out_end.length; - // turn on right signal until passing pull_out end point - const double turn_signal_off_buffer = std::min(parameters_.pull_out_finish_judge_buffer, 3.0); - if (distance_from_pull_out_end < turn_signal_off_buffer) { + const auto path = getFullPath(); + // pull out path does not overlap + const double distance_from_end = motion_utils::calcSignedArcLength( + path.points, status_.pull_out_path.end_pose.position, current_pose.position); + if (distance_from_end < parameters_.pull_out_finish_judge_buffer) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } else { turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; } - turn_signal.signal_distance = -distance_from_pull_out_end + turn_signal_off_buffer; + turn_signal.desired_start_point = status_.pull_out_path.start_pose.position; + turn_signal.required_start_point = status_.pull_out_path.start_pose.position; + turn_signal.required_end_point = status_.pull_out_path.end_pose.position; + turn_signal.desired_end_point = status_.pull_out_path.end_pose.position; return turn_signal; } 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 8d88a523c06f4..ea28e6c80a43c 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 @@ -427,6 +427,8 @@ bool PullOverModule::planWithCloseGoal() BehaviorModuleOutput PullOverModule::plan() { + const auto & current_pose = planner_data_->self_pose->pose; + status_.current_lanes = util::getExtendedCurrentLanes(planner_data_); status_.pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); status_.lanes = lanelet::ConstLanelets{}; @@ -438,8 +440,8 @@ BehaviorModuleOutput PullOverModule::plan() // Check if it needs to decide path if (status_.is_safe) { const auto dist_to_parking_start_pose = calcSignedArcLength( - getCurrentPath().points, planner_data_->self_pose->pose, - status_.pull_over_path.start_pose.position, std::numeric_limits::max(), M_PI_2); + getCurrentPath().points, current_pose, status_.pull_over_path.start_pose.position, + std::numeric_limits::max(), M_PI_2); if (*dist_to_parking_start_pose < parameters_.decide_path_distance) { status_.has_decided_path = true; @@ -460,6 +462,7 @@ BehaviorModuleOutput PullOverModule::plan() // When it is approved again after path is decided clearWaitingApproval(); last_approved_time_ = std::make_unique(clock_->now()); + last_approved_pose_ = std::make_unique(current_pose); // decide velocity to guarantee turn signal lighting time if (!status_.has_decided_velocity) { @@ -542,16 +545,7 @@ BehaviorModuleOutput PullOverModule::plan() // set hazard and turn signal if (status_.has_decided_path) { - const auto hazard_info = getHazardInfo(); - const auto turn_info = getTurnInfo(); - - if (hazard_info.first.command == HazardLightsCommand::ENABLE) { - output.turn_signal_info.hazard_signal.command = hazard_info.first.command; - output.turn_signal_info.signal_distance = hazard_info.second; - } else { - output.turn_signal_info.turn_signal.command = turn_info.first.command; - output.turn_signal_info.signal_distance = turn_info.second; - } + output.turn_signal_info = calcTurnSignalInfo(); } const auto distance_to_path_change = calcDistanceToPathChange(); @@ -568,9 +562,10 @@ BehaviorModuleOutput PullOverModule::plan() } const uint16_t steering_factor_direction = std::invoke([this]() { - if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_LEFT) { + const auto turn_signal = calcTurnSignalInfo(); + if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; - } else if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { return SteeringFactor::RIGHT; } return SteeringFactor::STRAIGHT; @@ -608,9 +603,10 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); const uint16_t steering_factor_direction = std::invoke([this]() { - if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_LEFT) { + const auto turn_signal = calcTurnSignalInfo(); + if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; - } else if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { return SteeringFactor::RIGHT; } return SteeringFactor::STRAIGHT; @@ -840,50 +836,37 @@ bool PullOverModule::hasFinishedPullOver() return car_is_on_goal && isStopped(); } -std::pair PullOverModule::getHazardInfo() const +TurnSignalInfo PullOverModule::calcTurnSignalInfo() const { - HazardLightsCommand hazard_signal{}; + TurnSignalInfo turn_signal{}; // output - const auto arc_position_goal_pose = - lanelet::utils::getArcCoordinates(status_.pull_over_lanes, modified_goal_pose_); - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(status_.pull_over_lanes, planner_data_->self_pose->pose); - const double distance_to_goal = arc_position_goal_pose.length - arc_position_current_pose.length; + const auto & current_pose = planner_data_->self_pose->pose; + const auto & start_pose = status_.pull_over_path.start_pose; + const auto & end_pose = status_.pull_over_path.end_pose; + const auto & full_path = getFullPath(); - const double velocity = std::abs(planner_data_->self_odometry->twist.twist.linear.x); - if ( - (distance_to_goal < parameters_.hazard_on_threshold_distance && - velocity < parameters_.hazard_on_threshold_velocity) || - status_.planner->getPlannerType() == PullOverPlannerType::ARC_BACKWARD) { - hazard_signal.command = HazardLightsCommand::ENABLE; - const double distance_from_front_to_goal = - distance_to_goal - planner_data_->parameters.base_link2front; - return std::make_pair(hazard_signal, distance_from_front_to_goal); + // calc TurnIndicatorsCommand + { + const double distance_to_end = + calcSignedArcLength(full_path.points, current_pose.position, end_pose.position); + const bool is_before_end_pose = distance_to_end >= 0.0; + turn_signal.turn_signal.command = + is_before_end_pose ? TurnIndicatorsCommand::ENABLE_LEFT : TurnIndicatorsCommand::NO_COMMAND; } - return std::make_pair(hazard_signal, std::numeric_limits::max()); -} - -std::pair PullOverModule::getTurnInfo() const -{ - std::pair turn_info{}; - - const double distance_from_vehicle_front = std::invoke([&]() { - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(status_.current_lanes, planner_data_->self_pose->pose); - const auto arc_position_end_pose = - lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_over_path.end_pose); - return arc_position_end_pose.length - arc_position_current_pose.length - - planner_data_->parameters.base_link2front; - }); + // calc desired/required start/end point + { + // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds + // before starting pull_over + turn_signal.desired_start_point = last_approved_pose_ && status_.has_decided_path + ? last_approved_pose_->position + : current_pose.position; + turn_signal.desired_end_point = end_pose.position; + turn_signal.required_start_point = start_pose.position; + turn_signal.required_end_point = end_pose.position; + } - TurnIndicatorsCommand turn_signal{}; - const bool is_before_parking_end = distance_from_vehicle_front >= 0.0; - turn_signal.command = - is_before_parking_end ? TurnIndicatorsCommand::ENABLE_LEFT : TurnIndicatorsCommand::NO_COMMAND; - turn_info.first = turn_signal; - turn_info.second = distance_from_vehicle_front; - return turn_info; + return turn_signal; } void PullOverModule::setDebugData() diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 9fbe0d5a5f997..f2e295cffbc52 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -16,6 +16,9 @@ #include "behavior_path_planner/utilities.hpp" +#include +#include +#include #include #include @@ -25,120 +28,355 @@ namespace behavior_path_planner { TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( - const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, - const RouteHandler & route_handler, const TurnIndicatorsCommand & turn_signal_plan, - const double plan_distance) const + const PathWithLaneId & path, const Pose & current_pose, const double current_vel, + const size_t current_seg_idx, const RouteHandler & route_handler, + const TurnSignalInfo & turn_signal_info) { - auto turn_signal = turn_signal_plan; + // Guard + if (path.points.empty()) { + return turn_signal_info.turn_signal; + } - // If the distance to intersection is nearer than path change point, - // use turn signal for turning at the intersection - const auto intersection_result = - getIntersectionTurnSignal(path, current_pose, current_seg_idx, route_handler); - const auto intersection_turn_signal = intersection_result.first; - const auto intersection_distance = intersection_result.second; + // Get closest intersection turn signal if exists + const auto intersection_turn_signal_info = + getIntersectionTurnSignalInfo(path, current_pose, current_vel, current_seg_idx, route_handler); - if ( - intersection_distance < plan_distance || - turn_signal_plan.command == TurnIndicatorsCommand::NO_COMMAND || - turn_signal_plan.command == TurnIndicatorsCommand::DISABLE) { - turn_signal.command = intersection_turn_signal.command; + if (!intersection_turn_signal_info) { + initialize_intersection_info(); + return turn_signal_info.turn_signal; + } else if ( + turn_signal_info.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND || + turn_signal_info.turn_signal.command == TurnIndicatorsCommand::DISABLE) { + set_intersection_info(path, current_pose, current_seg_idx, *intersection_turn_signal_info); + return intersection_turn_signal_info->turn_signal; } - return turn_signal; + return resolve_turn_signal( + path, current_pose, current_seg_idx, *intersection_turn_signal_info, turn_signal_info); } -std::pair TurnSignalDecider::getIntersectionTurnSignal( - const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, - const RouteHandler & route_handler) const +std::pair TurnSignalDecider::getIntersectionTurnSignalFlag() { - TurnIndicatorsCommand turn_signal{}; - turn_signal.command = TurnIndicatorsCommand::DISABLE; - double distance = std::numeric_limits::max(); - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + return std::make_pair(intersection_turn_signal_, approaching_intersection_turn_signal_); +} - if (path.points.empty()) { - return std::make_pair(turn_signal, distance); - } - - // Get frenet coordinate of current_pose on path - const auto vehicle_pose_frenet = - util::convertToFrenetCoordinate3d(path, current_pose.position, current_seg_idx); - - // Get nearest intersection and decide turn signal - double accumulated_distance = 0; - - auto prev_point = path.points.front(); - auto lane_attribute = std::string("none"); - for (const auto & path_point : path.points) { - const double path_point_distance = - tier4_autoware_utils::calcDistance3d(prev_point.point, path_point.point); - accumulated_distance += path_point_distance; - prev_point = path_point; - const double distance_from_vehicle_front = - accumulated_distance - vehicle_pose_frenet.length - base_link2front_; - if (distance_from_vehicle_front > intersection_search_distance_) { - if (turn_signal.command == TurnIndicatorsCommand::DISABLE) { - distance = std::numeric_limits::max(); - approaching_intersection_turn_signal_ = false; - } else { - intersection_distance_ = distance; - approaching_intersection_turn_signal_ = true; +std::pair TurnSignalDecider::getIntersectionPoseAndDistance() +{ + return std::make_pair(intersection_pose_point_, intersection_distance_); +} + +boost::optional TurnSignalDecider::getIntersectionTurnSignalInfo( + const PathWithLaneId & path, const Pose & current_pose, const double current_vel, + const size_t current_seg_idx, const RouteHandler & route_handler) +{ + // search distance + const double search_distance = 3.0 * current_vel + intersection_search_distance_; + + // unique lane ids + std::vector unique_lane_ids; + for (size_t i = 0; i < path.points.size(); ++i) { + for (const auto & lane_id : path.points.at(i).lane_ids) { + if ( + std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) == + unique_lane_ids.end()) { + unique_lane_ids.push_back(lane_id); } - return std::make_pair(turn_signal, distance); } - // TODO(Horibe): Route Handler should be a library. - for (const auto & lane : route_handler.getLaneletsFromIds(path_point.lane_ids)) { - // judgement of lighting of turn_signal - bool lighting_turn_signal = false; - if (lane.attributeOr("turn_direction", std::string("none")) != lane_attribute) { - if ( - distance_from_vehicle_front >= 0.0 && - distance_from_vehicle_front < - lane.attributeOr("turn_signal_distance", intersection_search_distance_) && - path_point_distance > 0.0) { - lighting_turn_signal = true; - } - } else { - if ( - lane.hasAttribute("turn_direction") && - distance_from_vehicle_front < path_point_distance && distance_from_vehicle_front > 0) { - lighting_turn_signal = true; - } + } + + std::queue signal_queue; + for (const auto & lane_id : unique_lane_ids) { + const auto lane = route_handler.getLaneletsFromId(lane_id); + + // lane front and back point + const geometry_msgs::msg::Point lane_front_point = + lanelet::utils::conversion::toGeomMsgPt(lane.centerline3d().front()); + const geometry_msgs::msg::Point lane_terminal_point = + lanelet::utils::conversion::toGeomMsgPt(lane.centerline3d().back()); + + const size_t front_nearest_seg_idx = + motion_utils::findNearestSegmentIndex(path.points, lane_front_point); + const size_t terminal_nearest_seg_idx = + motion_utils::findNearestSegmentIndex(path.points, lane_terminal_point); + + // Distance from ego vehicle front pose to front point of the lane + const double dist_to_front_point = motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, + lane_front_point, front_nearest_seg_idx) - + base_link2front_; + + // Distance from ego vehicle base link to the terminal point of the lane + const double dist_to_terminal_point = motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, lane_terminal_point, + terminal_nearest_seg_idx); + + if (dist_to_terminal_point < 0.0) { + // Vehicle is already passed this lane + if (desired_start_point_map_.find(lane_id) != desired_start_point_map_.end()) { + desired_start_point_map_.erase(lane_id); } - lane_attribute = lane.attributeOr("turn_direction", std::string("none")); - - if (lighting_turn_signal) { - if (lane_attribute == std::string("left")) { - turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - distance = distance_from_vehicle_front; - } else if (lane_attribute == std::string("right")) { - turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - distance = distance_from_vehicle_front; - } - intersection_pose_point_ = path_point.point.pose; + continue; + } else if (search_distance < dist_to_front_point) { + break; + } + const std::string lane_attribute = lane.attributeOr("turn_direction", std::string("none")); + if ( + (lane_attribute == "right" || lane_attribute == "left") && + dist_to_front_point < lane.attributeOr("turn_signal_distance", search_distance)) { + // update map if necessary + if (desired_start_point_map_.find(lane_id) == desired_start_point_map_.end()) { + desired_start_point_map_.emplace(lane_id, current_pose.position); } + + TurnSignalInfo turn_signal_info{}; + turn_signal_info.desired_start_point = desired_start_point_map_.at(lane_id); + turn_signal_info.required_start_point = lane_front_point; + turn_signal_info.required_end_point = get_required_end_point(lane.centerline3d()); + turn_signal_info.desired_end_point = lane_terminal_point; + turn_signal_info.turn_signal.command = signal_map.at(lane_attribute); + signal_queue.push(turn_signal_info); } } - if (turn_signal.command == TurnIndicatorsCommand::DISABLE) { - distance = std::numeric_limits::max(); - intersection_turn_signal_ = false; - approaching_intersection_turn_signal_ = false; - } else { - intersection_turn_signal_ = true; + + // Resolve the conflict between several turn signal requirements + while (!signal_queue.empty()) { + if (signal_queue.size() == 1) { + return signal_queue.front(); + } + + const auto & turn_signal_info = signal_queue.front(); + const auto & required_end_point = turn_signal_info.required_end_point; + const size_t nearest_seg_idx = + motion_utils::findNearestSegmentIndex(path.points, required_end_point); + const double dist_to_end_point = motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, required_end_point, nearest_seg_idx); + + if (dist_to_end_point >= 0.0) { + // we haven't finished the current mandatory turn signal + return turn_signal_info; + } + + signal_queue.pop(); } - intersection_distance_ = distance; - return std::make_pair(turn_signal, distance); + + return {}; } -std::pair TurnSignalDecider::getIntersectionTurnSignalFlag() +TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info) { - return std::make_pair(intersection_turn_signal_, approaching_intersection_turn_signal_); + const auto get_distance = [&](const auto & input_point) { + const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex(path.points, input_point); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point, nearest_seg_idx); + }; + + const auto & inter_desired_start_point = intersection_signal_info.desired_start_point; + const auto & inter_desired_end_point = intersection_signal_info.desired_end_point; + const auto & inter_required_start_point = intersection_signal_info.required_start_point; + const auto & inter_required_end_point = intersection_signal_info.required_end_point; + const auto & behavior_desired_start_point = behavior_signal_info.desired_start_point; + const auto & behavior_desired_end_point = behavior_signal_info.desired_end_point; + const auto & behavior_required_start_point = behavior_signal_info.required_start_point; + const auto & behavior_required_end_point = behavior_signal_info.required_end_point; + + const double dist_to_intersection_desired_start = + get_distance(inter_desired_start_point) - base_link2front_; + const double dist_to_intersection_desired_end = get_distance(inter_desired_end_point); + const double dist_to_intersection_required_start = + get_distance(inter_required_start_point) - base_link2front_; + const double dist_to_intersection_required_end = get_distance(inter_required_end_point); + const double dist_to_behavior_desired_start = + get_distance(behavior_desired_start_point) - base_link2front_; + const double dist_to_behavior_desired_end = get_distance(behavior_desired_end_point); + const double dist_to_behavior_required_start = + get_distance(behavior_required_start_point) - base_link2front_; + const double dist_to_behavior_required_end = get_distance(behavior_required_end_point); + + // If we still do not reach the desired front point we ignore it + if (dist_to_intersection_desired_start > 0.0 && dist_to_behavior_desired_start > 0.0) { + TurnIndicatorsCommand empty_signal_command; + empty_signal_command.command = TurnIndicatorsCommand::DISABLE; + initialize_intersection_info(); + return empty_signal_command; + } else if (dist_to_intersection_desired_start > 0.0) { + initialize_intersection_info(); + return behavior_signal_info.turn_signal; + } else if (dist_to_behavior_desired_start > 0.0) { + set_intersection_info(path, current_pose, current_seg_idx, intersection_signal_info); + return intersection_signal_info.turn_signal; + } + + // If we already passed the desired end point, return the other signal + if (dist_to_intersection_desired_end < 0.0 && dist_to_behavior_desired_end < 0.0) { + TurnIndicatorsCommand empty_signal_command; + empty_signal_command.command = TurnIndicatorsCommand::DISABLE; + initialize_intersection_info(); + return empty_signal_command; + } else if (dist_to_intersection_desired_end < 0.0) { + initialize_intersection_info(); + return behavior_signal_info.turn_signal; + } else if (dist_to_behavior_desired_end < 0.0) { + set_intersection_info(path, current_pose, current_seg_idx, intersection_signal_info); + return intersection_signal_info.turn_signal; + } + + if (dist_to_intersection_desired_start <= dist_to_behavior_desired_start) { + // intersection signal is prior than behavior signal + const auto enable_prior = use_prior_turn_signal( + dist_to_intersection_required_start, dist_to_intersection_required_end, + dist_to_behavior_required_start, dist_to_behavior_required_end); + + if (enable_prior) { + set_intersection_info(path, current_pose, current_seg_idx, intersection_signal_info); + return intersection_signal_info.turn_signal; + } + initialize_intersection_info(); + return behavior_signal_info.turn_signal; + } + + // behavior signal is prior than intersection signal + const auto enable_prior = use_prior_turn_signal( + dist_to_behavior_required_start, dist_to_behavior_required_end, + dist_to_intersection_required_start, dist_to_intersection_required_end); + if (enable_prior) { + initialize_intersection_info(); + return behavior_signal_info.turn_signal; + } + set_intersection_info(path, current_pose, current_seg_idx, intersection_signal_info); + return intersection_signal_info.turn_signal; } -std::pair TurnSignalDecider::getIntersectionPoseAndDistance() +bool TurnSignalDecider::use_prior_turn_signal( + const double dist_to_prior_required_start, const double dist_to_prior_required_end, + const double dist_to_subsequent_required_start, const double dist_to_subsequent_required_end) { - return std::make_pair(intersection_pose_point_, intersection_distance_); + const bool before_prior_required = dist_to_prior_required_start > 0.0; + const bool before_subsequent_required = dist_to_subsequent_required_start > 0.0; + const bool inside_prior_required = + dist_to_prior_required_start < 0.0 && 0.0 <= dist_to_prior_required_end; + + if (dist_to_prior_required_start < dist_to_subsequent_required_start) { + // subsequent signal required section is completely overlapped the prior signal required section + if (dist_to_subsequent_required_end < dist_to_prior_required_end) { + return true; + } + + // Vehicle is inside or in front of the prior required section + if (before_prior_required || inside_prior_required) { + return true; + } + + // passed prior required section but in front of the subsequent required section + if (before_subsequent_required) { + return true; + } + + // within or passed subsequent required section and completely passed prior required section + return false; + } + + // Subsequent required section starts faster than prior required starts section + + // If the prior section is inside of the subsequent required section + if (dist_to_prior_required_end < dist_to_subsequent_required_end) { + if (before_prior_required || inside_prior_required) { + return true; + } + return false; + } + + // inside or passed the intersection required + if (before_prior_required) { + return false; + } + + return true; } +geometry_msgs::msg::Point TurnSignalDecider::get_required_end_point( + const lanelet::ConstLineString3d & centerline) +{ + std::vector converted_centerline(centerline.size()); + for (size_t i = 0; i < centerline.size(); ++i) { + converted_centerline.at(i).position = lanelet::utils::conversion::toGeomMsgPt(centerline[i]); + } + motion_utils::insertOrientation(converted_centerline, true); + + const double length = motion_utils::calcArcLength(converted_centerline); + + // Create resampling intervals + const double resampling_interval = 1.0; + std::vector resampling_arclength; + for (double s = 0.0; s < length; s += resampling_interval) { + resampling_arclength.push_back(s); + } + + // Insert terminal point + if (length - resampling_arclength.back() < motion_utils::overlap_threshold) { + resampling_arclength.back() = length; + } else { + resampling_arclength.push_back(length); + } + + const auto resampled_centerline = + motion_utils::resamplePath(converted_centerline, resampling_arclength); + + const double terminal_yaw = tf2::getYaw(resampled_centerline.back().orientation); + for (size_t i = 0; i < resampled_centerline.size(); ++i) { + const double yaw = tf2::getYaw(resampled_centerline.at(i).orientation); + const double yaw_diff = tier4_autoware_utils::normalizeRadian(yaw - terminal_yaw); + if (std::fabs(yaw_diff) < tier4_autoware_utils::deg2rad(15)) { + return resampled_centerline.at(i).position; + } + } + + return resampled_centerline.back().position; +} + +void TurnSignalDecider::set_intersection_info( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & intersection_turn_signal_info) +{ + const auto get_distance = [&](const auto & input_point) { + const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex(path.points, input_point); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point, nearest_seg_idx); + }; + + const auto & inter_desired_start_point = intersection_turn_signal_info.desired_start_point; + const auto & inter_desired_end_point = intersection_turn_signal_info.desired_end_point; + const auto & inter_required_start_point = intersection_turn_signal_info.required_start_point; + + const double dist_to_intersection_desired_start = + get_distance(inter_desired_start_point) - base_link2front_; + const double dist_to_intersection_desired_end = get_distance(inter_desired_end_point); + const double dist_to_intersection_required_start = + get_distance(inter_required_start_point) - base_link2front_; + + if (dist_to_intersection_desired_start < 0.0 && dist_to_intersection_desired_end > 0.0) { + if (dist_to_intersection_required_start > 0.0) { + intersection_turn_signal_ = false; + approaching_intersection_turn_signal_ = true; + } else { + intersection_turn_signal_ = true; + approaching_intersection_turn_signal_ = false; + } + intersection_distance_ = dist_to_intersection_required_start; + const size_t required_start_nearest_seg_idx = + motion_utils::findNearestSegmentIndex(path.points, inter_required_start_point); + intersection_pose_point_ = path.points.at(required_start_nearest_seg_idx).point.pose; + } + + initialize_intersection_info(); +} + +void TurnSignalDecider::initialize_intersection_info() +{ + intersection_turn_signal_ = false; + approaching_intersection_turn_signal_ = false; + intersection_pose_point_ = Pose(); + intersection_distance_ = std::numeric_limits::max(); +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/test/test_turn_signal.cpp b/planning/behavior_path_planner/test/test_turn_signal.cpp new file mode 100644 index 0000000000000..7d9630bfe0358 --- /dev/null +++ b/planning/behavior_path_planner/test/test_turn_signal.cpp @@ -0,0 +1,400 @@ +// 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 "behavior_path_planner/turn_signal_decider.hpp" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" + +#include +#include + +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using behavior_path_planner::PathWithLaneId; +using behavior_path_planner::Pose; +using behavior_path_planner::TurnSignalDecider; +using behavior_path_planner::TurnSignalInfo; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Twist; +using tier4_autoware_utils::createPoint; + +namespace +{ +PathWithLaneId generateStraightSamplePathWithLaneId( + float initial_pose_value, float pose_increment, size_t point_sample) +{ + PathWithLaneId path; + for (size_t idx = 0; idx < point_sample; ++idx) { + PathPoint point; + point.pose.position.x = std::exchange(initial_pose_value, initial_pose_value + pose_increment); + point.pose.position.y = 0.0; + point.pose.position.z = 0.0; + point.longitudinal_velocity_mps = 0.1; // [m/s] + point.heading_rate_rps = 0.0; // [rad/s] + point.is_final = (idx == point_sample - 1); + + PathPointWithLaneId path_point_with_lane_id; + path_point_with_lane_id.point = point; + path_point_with_lane_id.lane_ids = {}; + + path.header.frame_id = "map"; + path.points.push_back(path_point_with_lane_id); + } + + return path; +} + +Pose generateEgoSamplePose(float && p_x, float && p_y, float && p_z) +{ + Pose pose; + pose.position.x = p_x; + pose.position.y = p_y; + pose.position.z = p_z; + return pose; +} +} // namespace + +TEST(BehaviorPathPlanningTurnSignal, Condition1) +{ + PathWithLaneId path = generateStraightSamplePathWithLaneId(0.0f, 1.0f, 70u); + TurnSignalDecider turn_signal_decider; + turn_signal_decider.setParameters(1.0, 30.0); + + TurnSignalInfo intersection_signal_info; + intersection_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + intersection_signal_info.desired_start_point = createPoint(0.0, 0.0, 0.0); + intersection_signal_info.desired_end_point = createPoint(65.0, 0.0, 0.0); + intersection_signal_info.required_start_point = createPoint(35.0, 0.0, 0.0); + intersection_signal_info.required_end_point = createPoint(48.0, 0.0, 0.0); + + TurnSignalInfo behavior_signal_info; + behavior_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + behavior_signal_info.desired_start_point = createPoint(5.0, 0.0, 0.0); + behavior_signal_info.desired_end_point = createPoint(70.0, 0.0, 0.0); + behavior_signal_info.required_start_point = createPoint(45.0, 0.0, 0.0); + behavior_signal_info.required_end_point = createPoint(50.0, 0.0, 0.0); + + // current pose on the behavior desired start + { + Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right before the intersection required start + { + Pose current_pose = generateEgoSamplePose(34.99f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is within the intersection required section + { + Pose current_pose = generateEgoSamplePose(40.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is within the intersection and behavior required section + { + Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is on the intersection required end + { + Pose current_pose = generateEgoSamplePose(48.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right after the intersection required end + { + Pose current_pose = generateEgoSamplePose(48.1f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is on the behavior required end + { + Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right after the behavior required end + { + Pose current_pose = generateEgoSamplePose(50.1f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right on the intersection desired end + { + Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right right after the intersection desired end + { + Pose current_pose = generateEgoSamplePose(65.1f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } +} + +TEST(BehaviorPathPlanningTurnSignal, Condition2) +{ + PathWithLaneId path = generateStraightSamplePathWithLaneId(0.0f, 1.0f, 70u); + TurnSignalDecider turn_signal_decider; + turn_signal_decider.setParameters(1.0, 30.0); + + TurnSignalInfo intersection_signal_info; + intersection_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + intersection_signal_info.desired_start_point = createPoint(0.0, 0.0, 0.0); + intersection_signal_info.desired_end_point = createPoint(65.0, 0.0, 0.0); + intersection_signal_info.required_start_point = createPoint(35.0, 0.0, 0.0); + intersection_signal_info.required_end_point = createPoint(50.0, 0.0, 0.0); + + TurnSignalInfo behavior_signal_info; + behavior_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + behavior_signal_info.desired_start_point = createPoint(5.0, 0.0, 0.0); + behavior_signal_info.desired_end_point = createPoint(70.0, 0.0, 0.0); + behavior_signal_info.required_start_point = createPoint(40.0, 0.0, 0.0); + behavior_signal_info.required_end_point = createPoint(45.0, 0.0, 0.0); + + // current pose on the behavior desired start + { + Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right before the intersection required start + { + Pose current_pose = generateEgoSamplePose(34.99f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is on the behavior required start + { + Pose current_pose = generateEgoSamplePose(40.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is on the behavior required end + { + Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is on the intersection required end + { + Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right after the intersection required end + { + Pose current_pose = generateEgoSamplePose(50.1f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is on the intersection desired end + { + Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } +} + +TEST(BehaviorPathPlanningTurnSignal, Condition3) +{ + PathWithLaneId path = generateStraightSamplePathWithLaneId(0.0f, 1.0f, 70u); + TurnSignalDecider turn_signal_decider; + turn_signal_decider.setParameters(1.0, 30.0); + + TurnSignalInfo intersection_signal_info; + intersection_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + intersection_signal_info.desired_start_point = createPoint(0.0, 0.0, 0.0); + intersection_signal_info.desired_end_point = createPoint(65.0, 0.0, 0.0); + intersection_signal_info.required_start_point = createPoint(35.0, 0.0, 0.0); + intersection_signal_info.required_end_point = createPoint(50.0, 0.0, 0.0); + + TurnSignalInfo behavior_signal_info; + behavior_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + behavior_signal_info.desired_start_point = createPoint(5.0, 0.0, 0.0); + behavior_signal_info.desired_end_point = createPoint(70.0, 0.0, 0.0); + behavior_signal_info.required_start_point = createPoint(30.0, 0.0, 0.0); + behavior_signal_info.required_end_point = createPoint(45.0, 0.0, 0.0); + + // current pose on the behavior desired start + { + Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right before the behavior required start + { + Pose current_pose = generateEgoSamplePose(29.9f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right on the behavior required start + { + Pose current_pose = generateEgoSamplePose(30.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right before the intersection required start + { + Pose current_pose = generateEgoSamplePose(33.9f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right on the intersection required start + { + Pose current_pose = generateEgoSamplePose(35.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right before the behavior required end + { + Pose current_pose = generateEgoSamplePose(44.9f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right on the behavior required end + { + Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right before the intersection required end + { + Pose current_pose = generateEgoSamplePose(49.9f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right on the intersection required end + { + Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right on the intersection desired end + { + Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } +} From 248295ef0ba0cbfa6b92425b02eb11f2ec07d9f2 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 29 Sep 2022 09:56:55 +0900 Subject: [PATCH 23/38] test(signal_processing): fix unit test bug (#1960) fix(signal_processing): unit test bug Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../signal_processing/lowpass_filter.hpp | 2 +- .../test/src/lowpass_filter_1d_test.cpp | 4 ++-- .../test/src/lowpass_filter_test.cpp | 17 ++--------------- 3 files changed, 5 insertions(+), 18 deletions(-) diff --git a/common/signal_processing/include/signal_processing/lowpass_filter.hpp b/common/signal_processing/include/signal_processing/lowpass_filter.hpp index 695f687dd20ee..89b7a6da263f7 100644 --- a/common/signal_processing/include/signal_processing/lowpass_filter.hpp +++ b/common/signal_processing/include/signal_processing/lowpass_filter.hpp @@ -33,7 +33,7 @@ class LowpassFilterInterface public: explicit LowpassFilterInterface(const double gain) : gain_(gain) {} - void reset() { x_ = T{}; } + void reset() { x_ = {}; } void reset(const T & x) { x_ = x; } boost::optional getValue() const { return x_; } diff --git a/common/signal_processing/test/src/lowpass_filter_1d_test.cpp b/common/signal_processing/test/src/lowpass_filter_1d_test.cpp index d768cd0b9e2ba..5cce36884588e 100644 --- a/common/signal_processing/test/src/lowpass_filter_1d_test.cpp +++ b/common/signal_processing/test/src/lowpass_filter_1d_test.cpp @@ -23,7 +23,7 @@ TEST(lowpass_filter_1d, filter) LowpassFilter1d lowpass_filter_1d(0.1); // initial state - EXPECT_EQ(*lowpass_filter_1d.getValue(), {}); + EXPECT_EQ(lowpass_filter_1d.getValue(), boost::none); // random filter EXPECT_NEAR(lowpass_filter_1d.filter(0.0), 0.0, epsilon); @@ -33,7 +33,7 @@ TEST(lowpass_filter_1d, filter) // reset without value lowpass_filter_1d.reset(); - EXPECT_EQ(*lowpass_filter_1d.getValue(), {}); + EXPECT_EQ(lowpass_filter_1d.getValue(), boost::none); // reset with value lowpass_filter_1d.reset(-1.1); diff --git a/common/signal_processing/test/src/lowpass_filter_test.cpp b/common/signal_processing/test/src/lowpass_filter_test.cpp index 8fdcd5149d296..8dfea4dcae02e 100644 --- a/common/signal_processing/test/src/lowpass_filter_test.cpp +++ b/common/signal_processing/test/src/lowpass_filter_test.cpp @@ -39,13 +39,7 @@ TEST(lowpass_filter_twist, filter) LowpassFilterTwist lowpass_filter_(0.1); { // initial state - const auto filtered_twist = lowpass_filter_.getValue(); - EXPECT_NEAR(filtered_twist->linear.x, 0.0, epsilon); - EXPECT_NEAR(filtered_twist->linear.y, 0.0, epsilon); - EXPECT_NEAR(filtered_twist->linear.z, 0.0, epsilon); - EXPECT_NEAR(filtered_twist->angular.x, 0.0, epsilon); - EXPECT_NEAR(filtered_twist->angular.y, 0.0, epsilon); - EXPECT_NEAR(filtered_twist->angular.z, 0.0, epsilon); + EXPECT_EQ(lowpass_filter_.getValue(), boost::none); } { // random filter @@ -62,14 +56,7 @@ TEST(lowpass_filter_twist, filter) { // reset without value lowpass_filter_.reset(); - - const auto filtered_twist = lowpass_filter_.getValue(); - EXPECT_NEAR(filtered_twist->linear.x, 0.0, epsilon); - EXPECT_NEAR(filtered_twist->linear.y, 0.0, epsilon); - EXPECT_NEAR(filtered_twist->linear.z, 0.0, epsilon); - EXPECT_NEAR(filtered_twist->angular.x, 0.0, epsilon); - EXPECT_NEAR(filtered_twist->angular.y, 0.0, epsilon); - EXPECT_NEAR(filtered_twist->angular.z, 0.0, epsilon); + EXPECT_EQ(lowpass_filter_.getValue(), boost::none); } { // reset with value From 87f23223d130a1e8a05479d450009917a781997a Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 29 Sep 2022 11:16:40 +0900 Subject: [PATCH 24/38] refactor(ndt_scan_matcher): refactor callbackSensorPoints (#1757) * now works Signed-off-by: kminoda * ci(pre-commit): autofix * refactored alignUsingMonteCarlo Signed-off-by: kminoda * ci(pre-commit): autofix * output from screen to log Signed-off-by: kminoda * lowerCamelCase to snake_case Signed-off-by: kminoda * ci(pre-commit): autofix * remove iteration_num_max from NdtResult Signed-off-by: kminoda * minor fix Signed-off-by: kminoda * remove publish_scalars Signed-off-by: kminoda * ci(pre-commit): autofix * move publish_tf out from publish_pose Signed-off-by: kminoda * ci(pre-commit): autofix * simplified the function transform_sensor_points Signed-off-by: kminoda * ci(pre-commit): autofix * WIP pose_array_interpolator Signed-off-by: kminoda * added pose_array_interpolator class Signed-off-by: kminoda * ci(pre-commit): autofix * remove unnecessary code Signed-off-by: kminoda * removed std::pow(*, 2) Signed-off-by: kminoda * reflected comments Signed-off-by: kminoda * ci(pre-commit): autofix * minor fix in publish_point_cloud (seperated pcd transform and publish) Signed-off-by: kminoda * removed unnecessary commentout Signed-off-by: kminoda * ci(pre-commit): autofix * changed Eigen::Vector3f to Eigen::Vector3d Signed-off-by: kminoda * reverted No pose warning for initial pose Signed-off-by: kminoda * from_HOGE_to_FUGA to HOGE_to_FUGA Signed-off-by: kminoda * validate_* functions should not change member variables implicitely Signed-off-by: kminoda * ci(pre-commit): autofix * removed member function that implicitely changes the member variables Signed-off-by: kminoda * ci(pre-commit): autofix * remote 'ros' and 'eigen' prefix from utiility function name Signed-off-by: kminoda * ci(pre-commit): autofix * debugged Signed-off-by: kminoda * ci(pre-commit): autofix * added a name for each validation booleans Signed-off-by: kminoda * ci(pre-commit): autofix Signed-off-by: kminoda Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ndt_scan_matcher/CMakeLists.txt | 1 + .../include/ndt_scan_matcher/debug.hpp | 2 +- .../ndt_scan_matcher_core.hpp | 70 ++- .../pose_array_interpolator.hpp | 62 ++ .../include/ndt_scan_matcher/util_func.hpp | 29 +- localization/ndt_scan_matcher/src/debug.cpp | 14 +- .../src/ndt_scan_matcher_core.cpp | 577 +++++++++--------- .../src/pose_array_interpolator.cpp | 105 ++++ .../ndt_scan_matcher/src/util_func.cpp | 78 ++- 9 files changed, 572 insertions(+), 366 deletions(-) create mode 100644 localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp create mode 100644 localization/ndt_scan_matcher/src/pose_array_interpolator.cpp diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 953cc4d7f6664..df79a2cfc0dfd 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -27,6 +27,7 @@ ament_auto_add_executable(ndt_scan_matcher src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp src/util_func.cpp + src/pose_array_interpolator.cpp ) ament_auto_package( diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp index 4e63f7fb7f6fb..ccc3b59e63328 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp @@ -21,7 +21,7 @@ #include -visualization_msgs::msg::MarkerArray makeDebugMarkers( +visualization_msgs::msg::MarkerArray make_debug_markers( const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, const geometry_msgs::msg::Vector3 & scale, const Particle & particle, const size_t i); diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 3d00587e326cc..2be9c8084be81 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -68,8 +68,17 @@ enum class ConvergedParamType { NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 }; +struct NdtResult +{ + geometry_msgs::msg::Pose pose; + float transform_probability; + float nearest_voxel_transformation_likelihood; + int iteration_num; + std::vector transformation_array; +}; + template -std::shared_ptr> getNDT( +std::shared_ptr> get_ndt( const NDTImplementType & ndt_mode) { std::shared_ptr> ndt_ptr; @@ -107,40 +116,59 @@ class NDTScanMatcher : public rclcpp::Node NDTScanMatcher(); private: - void serviceNDTAlign( + void service_ndt_align( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res); - void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); - void callbackSensorPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); - void callbackInitialPose( + void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); + void callback_sensor_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); + void callback_initial_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr); - void callbackRegularizationPose( + void callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr); - geometry_msgs::msg::PoseWithCovarianceStamped alignUsingMonteCarlo( + NdtResult align(const geometry_msgs::msg::Pose & initial_pose_msg); + geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo( const std::shared_ptr> & ndt_ptr, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); - void updateTransforms(); - - void publishTF( - const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg); - bool getTransform( + void transform_sensor_measurement( + const std::string source_frame, const std::string target_frame, + const pcl::shared_ptr> sensor_points_input_ptr, + pcl::shared_ptr> sensor_points_output_ptr); + void update_transforms(); + + void publish_tf( + const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg); + void publish_pose( + const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, + const bool is_converged); + void publish_point_cloud( + const rclcpp::Time & sensor_ros_time, const std::string & frame_id, + const std::shared_ptr> & sensor_points_mapTF_ptr); + void publish_marker( + const rclcpp::Time & sensor_ros_time, const std::vector & pose_array); + void publish_initial_to_result_distances( + const rclcpp::Time & sensor_ros_msg, const geometry_msgs::msg::Pose & result_pose_msg, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_cov_msg, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_new_msg); + + bool get_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr); - bool validateTimeStampDifference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec); - bool validatePositionDifference( - const geometry_msgs::msg::Point & target_point, - const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_); + bool validate_num_iteration(const int iter_num, const int max_iter_num); + bool validate_score( + const double score, const double score_threshold, const std::string score_name); + bool validate_converged_param( + const double & transform_probability, const double & nearest_voxel_transformation_likelihood); - std::optional interpolateRegularizationPose( + std::optional interpolate_regularization_pose( const rclcpp::Time & sensor_ros_time); + void add_regularization_pose(const rclcpp::Time & sensor_ros_time); - void timerDiagnostic(); + void timer_diagnostic(); rclcpp::Subscription::SharedPtr initial_pose_sub_; rclcpp::Subscription::SharedPtr map_points_sub_; @@ -197,7 +225,7 @@ class NDTScanMatcher : public rclcpp::Node std::deque initial_pose_msg_ptr_array_; - std::mutex ndt_map_mtx_; + std::mutex ndt_ptr_mtx_; std::mutex initial_pose_array_mtx_; OMPParams omp_params_; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp new file mode 100644 index 0000000000000..e88862781225d --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp @@ -0,0 +1,62 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ +#define NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ + +#include "ndt_scan_matcher/util_func.hpp" + +#include + +#include + +#include + +class PoseArrayInterpolator +{ +private: + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + +public: + PoseArrayInterpolator( + rclcpp::Node * node, const rclcpp::Time target_ros_time, + const std::deque & pose_msg_ptr_array, + const double & pose_timeout_sec, const double & pose_distance_tolerance_meters); + + PoseArrayInterpolator( + rclcpp::Node * node, const rclcpp::Time target_ros_time, + const std::deque & pose_msg_ptr_array); + + PoseWithCovarianceStamped get_current_pose(); + PoseWithCovarianceStamped get_old_pose(); + PoseWithCovarianceStamped get_new_pose(); + bool is_success(); + +private: + rclcpp::Logger logger_; + rclcpp::Clock clock_; + const PoseWithCovarianceStamped::SharedPtr current_pose_ptr_; + PoseWithCovarianceStamped::SharedPtr old_pose_ptr_; + PoseWithCovarianceStamped::SharedPtr new_pose_ptr_; + bool success_; + + bool validate_time_stamp_difference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec) const; + bool validate_position_difference( + const geometry_msgs::msg::Point & target_point, + const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; +}; + +#endif // NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp index 0c40e820c3ffd..933375d5796f2 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp @@ -36,41 +36,44 @@ #include // ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA ExchangeColorCrc(double x); +std_msgs::msg::ColorRGBA exchange_color_crc(double x); -double calcDiffForRadian(const double lhs_rad, const double rhs_rad); +double calc_diff_for_radian(const double lhs_rad, const double rhs_rad); // x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose); -geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose); -geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose); +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose); +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); -geometry_msgs::msg::Twist calcTwist( +geometry_msgs::msg::Twist calc_twist( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b); -void getNearestTimeStampPose( +void get_nearest_timestamp_pose( const std::deque & pose_cov_msg_ptr_array, const rclcpp::Time & time_stamp, geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_old_pose_cov_msg_ptr, geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_new_pose_cov_msg_ptr); -geometry_msgs::msg::PoseStamped interpolatePose( +geometry_msgs::msg::PoseStamped interpolate_pose( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, const rclcpp::Time & time_stamp); -geometry_msgs::msg::PoseStamped interpolatePose( +geometry_msgs::msg::PoseStamped interpolate_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp); -void popOldPose( +void pop_old_pose( std::deque & pose_cov_msg_ptr_array, const rclcpp::Time & time_stamp); -Eigen::Affine3d fromRosPoseToEigen(const geometry_msgs::msg::Pose & ros_pose); +Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose); +Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose); +geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix); +Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos); -std::vector createRandomPoseArray( +std::vector create_random_pose_array( const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num); template @@ -81,4 +84,6 @@ T transform(const T & input, const geometry_msgs::msg::TransformStamped & transf return output; } +double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); + #endif // NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ diff --git a/localization/ndt_scan_matcher/src/debug.cpp b/localization/ndt_scan_matcher/src/debug.cpp index 74dd89e76b4ea..2a9fdf1e1eafd 100644 --- a/localization/ndt_scan_matcher/src/debug.cpp +++ b/localization/ndt_scan_matcher/src/debug.cpp @@ -16,7 +16,7 @@ #include "ndt_scan_matcher/util_func.hpp" -visualization_msgs::msg::MarkerArray makeDebugMarkers( +visualization_msgs::msg::MarkerArray make_debug_markers( const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, const geometry_msgs::msg::Vector3 & scale, const Particle & particle, const size_t i) { @@ -34,32 +34,32 @@ visualization_msgs::msg::MarkerArray makeDebugMarkers( marker.ns = "initial_pose_transform_probability_color_marker"; marker.pose = particle.initial_pose; - marker.color = ExchangeColorCrc(particle.score / 4.5); + marker.color = exchange_color_crc(particle.score / 4.5); marker_array.markers.push_back(marker); marker.ns = "initial_pose_iteration_color_marker"; marker.pose = particle.initial_pose; - marker.color = ExchangeColorCrc((1.0 * particle.iteration) / 30.0); + marker.color = exchange_color_crc((1.0 * particle.iteration) / 30.0); marker_array.markers.push_back(marker); marker.ns = "initial_pose_index_color_marker"; marker.pose = particle.initial_pose; - marker.color = ExchangeColorCrc((1.0 * i) / 100); + marker.color = exchange_color_crc((1.0 * i) / 100); marker_array.markers.push_back(marker); marker.ns = "result_pose_transform_probability_color_marker"; marker.pose = particle.result_pose; - marker.color = ExchangeColorCrc(particle.score / 4.5); + marker.color = exchange_color_crc(particle.score / 4.5); marker_array.markers.push_back(marker); marker.ns = "result_pose_iteration_color_marker"; marker.pose = particle.result_pose; - marker.color = ExchangeColorCrc((1.0 * particle.iteration) / 30.0); + marker.color = exchange_color_crc((1.0 * particle.iteration) / 30.0); marker_array.markers.push_back(marker); marker.ns = "result_pose_index_color_marker"; marker.pose = particle.result_pose; - marker.color = ExchangeColorCrc((1.0 * i) / 100); + marker.color = exchange_color_crc((1.0 * i) / 100); marker_array.markers.push_back(marker); return marker_array; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 38e3f8c617327..684a723754a08 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -17,6 +17,7 @@ #include "ndt_scan_matcher/debug.hpp" #include "ndt_scan_matcher/matrix_type.hpp" #include "ndt_scan_matcher/particle.hpp" +#include "ndt_scan_matcher/pose_array_interpolator.hpp" #include "ndt_scan_matcher/util_func.hpp" #include @@ -36,21 +37,21 @@ #include #include -tier4_debug_msgs::msg::Float32Stamped makeFloat32Stamped( +tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) { using T = tier4_debug_msgs::msg::Float32Stamped; return tier4_debug_msgs::build().stamp(stamp).data(data); } -tier4_debug_msgs::msg::Int32Stamped makeInt32Stamped( +tier4_debug_msgs::msg::Int32Stamped make_int32_stamped( const builtin_interfaces::msg::Time & stamp, const int32_t data) { using T = tier4_debug_msgs::msg::Int32Stamped; return tier4_debug_msgs::build().stamp(stamp).data(data); } -geometry_msgs::msg::TransformStamped identityTransformStamped( +geometry_msgs::msg::TransformStamped identity_transform_stamped( const builtin_interfaces::msg::Time & timestamp, const std::string & header_frame_id, const std::string & child_frame_id) { @@ -63,24 +64,19 @@ geometry_msgs::msg::TransformStamped identityTransformStamped( return transform; } -double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) -{ - return std::sqrt( - std::pow(p1.x - p2.x, 2.0) + std::pow(p1.y - p2.y, 2.0) + std::pow(p1.z - p2.z, 2.0)); -} - -bool isLocalOptimalSolutionOscillation( - const std::vector> & - result_pose_matrix_array, +bool validate_local_optimal_solution_oscillation( + const std::vector & result_pose_msg_array, const float oscillation_threshold, const float inversion_vector_threshold) { bool prev_oscillation = false; int oscillation_cnt = 0; - for (size_t i = 2; i < result_pose_matrix_array.size(); ++i) { - const Eigen::Vector3f current_pose = result_pose_matrix_array.at(i).block(0, 3, 3, 1); - const Eigen::Vector3f prev_pose = result_pose_matrix_array.at(i - 1).block(0, 3, 3, 1); - const Eigen::Vector3f prev_prev_pose = result_pose_matrix_array.at(i - 2).block(0, 3, 3, 1); - const auto current_vec = (current_pose - prev_pose).normalized(); + + for (size_t i = 2; i < result_pose_msg_array.size(); ++i) { + const Eigen::Vector3d current_pose = point_to_vector3d(result_pose_msg_array.at(i).position); + const Eigen::Vector3d prev_pose = point_to_vector3d(result_pose_msg_array.at(i - 1).position); + const Eigen::Vector3d prev_prev_pose = + point_to_vector3d(result_pose_msg_array.at(i - 2).position); + const auto current_vec = current_pose - prev_pose; const auto prev_vec = (prev_pose - prev_prev_pose).normalized(); const bool oscillation = prev_vec.dot(current_vec) < inversion_vector_threshold; if (prev_oscillation && oscillation) { @@ -123,7 +119,7 @@ NDTScanMatcher::NDTScanMatcher() RCLCPP_INFO(get_logger(), "NDT Implement Type is %d", ndt_implement_type_tmp); try { - ndt_ptr_ = getNDT(ndt_implement_type_); + ndt_ptr_ = get_ndt(ndt_implement_type_); } catch (std::exception & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); return; @@ -222,18 +218,18 @@ NDTScanMatcher::NDTScanMatcher() initial_pose_sub_ = this->create_subscription( "ekf_pose_with_covariance", 100, - std::bind(&NDTScanMatcher::callbackInitialPose, this, std::placeholders::_1), + std::bind(&NDTScanMatcher::callback_initial_pose, this, std::placeholders::_1), initial_pose_sub_opt); map_points_sub_ = this->create_subscription( "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&NDTScanMatcher::callbackMapPoints, this, std::placeholders::_1), main_sub_opt); + std::bind(&NDTScanMatcher::callback_map_points, this, std::placeholders::_1), main_sub_opt); sensor_points_sub_ = this->create_subscription( "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size), - std::bind(&NDTScanMatcher::callbackSensorPoints, this, std::placeholders::_1), main_sub_opt); + std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), main_sub_opt); regularization_pose_sub_ = this->create_subscription( "regularization_pose_with_covariance", 100, - std::bind(&NDTScanMatcher::callbackRegularizationPose, this, std::placeholders::_1)); + std::bind(&NDTScanMatcher::callback_regularization_pose, this, std::placeholders::_1)); sensor_aligned_pose_pub_ = this->create_publisher("points_aligned", 10); @@ -270,14 +266,15 @@ NDTScanMatcher::NDTScanMatcher() service_ = this->create_service( "ndt_align_srv", - std::bind(&NDTScanMatcher::serviceNDTAlign, this, std::placeholders::_1, std::placeholders::_2), + std::bind( + &NDTScanMatcher::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), main_callback_group); - diagnostic_thread_ = std::thread(&NDTScanMatcher::timerDiagnostic, this); + diagnostic_thread_ = std::thread(&NDTScanMatcher::timer_diagnostic, this); diagnostic_thread_.detach(); } -void NDTScanMatcher::timerDiagnostic() +void NDTScanMatcher::timer_diagnostic() { rclcpp::Rate rate(100); while (rclcpp::ok()) { @@ -328,13 +325,13 @@ void NDTScanMatcher::timerDiagnostic() } } -void NDTScanMatcher::serviceNDTAlign( +void NDTScanMatcher::service_ndt_align( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { // get TF from pose_frame to map_frame auto TF_pose_to_map_ptr = std::make_shared(); - getTransform(map_frame_, req->pose_with_covariance.header.frame_id, TF_pose_to_map_ptr); + get_transform(map_frame_, req->pose_with_covariance.header.frame_id, TF_pose_to_map_ptr); // transform pose_frame to map_frame const auto mapTF_initial_pose_msg = transform(req->pose_with_covariance, *TF_pose_to_map_ptr); @@ -352,16 +349,16 @@ void NDTScanMatcher::serviceNDTAlign( } // mutex Map - std::lock_guard lock(ndt_map_mtx_); + std::lock_guard lock(ndt_ptr_mtx_); key_value_stdmap_["state"] = "Aligning"; - res->pose_with_covariance = alignUsingMonteCarlo(ndt_ptr_, mapTF_initial_pose_msg); + res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, mapTF_initial_pose_msg); key_value_stdmap_["state"] = "Sleeping"; res->success = true; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; } -void NDTScanMatcher::callbackInitialPose( +void NDTScanMatcher::callback_initial_pose( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { // lock mutex for initial pose @@ -381,7 +378,7 @@ void NDTScanMatcher::callbackInitialPose( } else { // get TF from pose_frame to map_frame auto TF_pose_to_map_ptr = std::make_shared(); - getTransform(map_frame_, initial_pose_msg_ptr->header.frame_id, TF_pose_to_map_ptr); + get_transform(map_frame_, initial_pose_msg_ptr->header.frame_id, TF_pose_to_map_ptr); // transform pose_frame to map_frame auto mapTF_initial_pose_msg_ptr = @@ -392,13 +389,13 @@ void NDTScanMatcher::callbackInitialPose( } } -void NDTScanMatcher::callbackRegularizationPose( +void NDTScanMatcher::callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr) { regularization_pose_msg_ptr_array_.push_back(pose_conv_msg_ptr); } -void NDTScanMatcher::callbackMapPoints( +void NDTScanMatcher::callback_map_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) { const auto trans_epsilon = ndt_ptr_->getTransformationEpsilon(); @@ -407,7 +404,7 @@ void NDTScanMatcher::callbackMapPoints( const auto max_iterations = ndt_ptr_->getMaximumIterations(); using NDTBase = NormalDistributionsTransformBase; - std::shared_ptr new_ndt_ptr = getNDT(ndt_implement_type_); + std::shared_ptr new_ndt_ptr = get_ndt(ndt_implement_type_); if (ndt_implement_type_ == NDTImplementType::OMP) { using T = NormalDistributionsTransformOMP; @@ -434,126 +431,65 @@ void NDTScanMatcher::callbackMapPoints( new_ndt_ptr->align(*output_cloud, Eigen::Matrix4f::Identity()); // swap - ndt_map_mtx_.lock(); + ndt_ptr_mtx_.lock(); ndt_ptr_ = new_ndt_ptr; - ndt_map_mtx_.unlock(); + ndt_ptr_mtx_.unlock(); } -void NDTScanMatcher::callbackSensorPoints( +void NDTScanMatcher::callback_sensor_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_sensorTF_msg_ptr) { - const auto exe_start_time = std::chrono::system_clock::now(); - // mutex Map - std::lock_guard lock(ndt_map_mtx_); + // mutex ndt_ptr_ + std::lock_guard lock(ndt_ptr_mtx_); - const std::string & sensor_frame = sensor_points_sensorTF_msg_ptr->header.frame_id; + const auto exe_start_time = std::chrono::system_clock::now(); const rclcpp::Time sensor_ros_time = sensor_points_sensorTF_msg_ptr->header.stamp; - boost::shared_ptr> sensor_points_sensorTF_ptr( + // preprocess input pointcloud + pcl::shared_ptr> sensor_points_sensorTF_ptr( new pcl::PointCloud); - pcl::fromROSMsg(*sensor_points_sensorTF_msg_ptr, *sensor_points_sensorTF_ptr); - // get TF base to sensor - auto TF_base_to_sensor_ptr = std::make_shared(); - getTransform(base_frame_, sensor_frame, TF_base_to_sensor_ptr); - const Eigen::Affine3d base_to_sensor_affine = tf2::transformToEigen(*TF_base_to_sensor_ptr); - const Eigen::Matrix4f base_to_sensor_matrix = base_to_sensor_affine.matrix().cast(); pcl::shared_ptr> sensor_points_baselinkTF_ptr( new pcl::PointCloud); - pcl::transformPointCloud( - *sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix); + const std::string & sensor_frame = sensor_points_sensorTF_msg_ptr->header.frame_id; + + pcl::fromROSMsg(*sensor_points_sensorTF_msg_ptr, *sensor_points_sensorTF_ptr); + transform_sensor_measurement( + sensor_frame, base_frame_, sensor_points_sensorTF_ptr, sensor_points_baselinkTF_ptr); ndt_ptr_->setInputSource(sensor_points_baselinkTF_ptr); - // start of critical section for initial_pose_msg_ptr_array_ + // calculate initial pose std::unique_lock initial_pose_array_lock(initial_pose_array_mtx_); - // check if (initial_pose_msg_ptr_array_.size() <= 1) { RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No Pose!"); return; } - // searchNNPose using timestamp - auto initial_pose_old_msg_ptr = std::make_shared(); - auto initial_pose_new_msg_ptr = std::make_shared(); - getNearestTimeStampPose( - initial_pose_msg_ptr_array_, sensor_ros_time, initial_pose_old_msg_ptr, - initial_pose_new_msg_ptr); - popOldPose(initial_pose_msg_ptr_array_, sensor_ros_time); - - // check the time stamp - bool valid_old_timestamp = validateTimeStampDifference( - initial_pose_old_msg_ptr->header.stamp, sensor_ros_time, initial_pose_timeout_sec_); - bool valid_new_timestamp = validateTimeStampDifference( - initial_pose_new_msg_ptr->header.stamp, sensor_ros_time, initial_pose_timeout_sec_); - - // check the position jumping (ex. immediately after the initial pose estimation) - bool valid_new_to_old_distance = validatePositionDifference( - initial_pose_old_msg_ptr->pose.pose.position, initial_pose_new_msg_ptr->pose.pose.position, + PoseArrayInterpolator interpolator( + this, sensor_ros_time, initial_pose_msg_ptr_array_, initial_pose_timeout_sec_, initial_pose_distance_tolerance_m_); - - // must all validations are true - if (!(valid_old_timestamp && valid_new_timestamp && valid_new_to_old_distance)) { - RCLCPP_WARN(get_logger(), "Validation error."); - return; - } - - // If regularization is enabled and available, set pose to NDT for regularization - if (regularization_enabled_ && (ndt_implement_type_ == NDTImplementType::OMP)) { - ndt_ptr_->unsetRegularizationPose(); - std::optional pose_opt = interpolateRegularizationPose(sensor_ros_time); - if (pose_opt.has_value()) { - ndt_ptr_->setRegularizationPose(pose_opt.value()); - RCLCPP_DEBUG_STREAM(get_logger(), "Regularization pose is set to NDT"); - } - } - - const auto initial_pose_msg = - interpolatePose(*initial_pose_old_msg_ptr, *initial_pose_new_msg_ptr, sensor_ros_time); - - // enf of critical section for initial_pose_msg_ptr_array_ + if (!interpolator.is_success()) return; + pop_old_pose(initial_pose_msg_ptr_array_, sensor_ros_time); initial_pose_array_lock.unlock(); - geometry_msgs::msg::PoseWithCovarianceStamped initial_pose_cov_msg; - initial_pose_cov_msg.header = initial_pose_msg.header; - initial_pose_cov_msg.pose.pose = initial_pose_msg.pose; + // if regularization is enabled and available, set pose to NDT for regularization + if (regularization_enabled_ && (ndt_implement_type_ == NDTImplementType::OMP)) + add_regularization_pose(sensor_ros_time); if (ndt_ptr_->getInputTarget() == nullptr) { RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No MAP!"); return; } - // align - const Eigen::Affine3d initial_pose_affine = fromRosPoseToEigen(initial_pose_cov_msg.pose.pose); - const Eigen::Matrix4f initial_pose_matrix = initial_pose_affine.matrix().cast(); - auto output_cloud = std::make_shared>(); + // perform ndt scan matching key_value_stdmap_["state"] = "Aligning"; - ndt_ptr_->align(*output_cloud, initial_pose_matrix); + const NdtResult ndt_result = align(interpolator.get_current_pose().pose.pose); key_value_stdmap_["state"] = "Sleeping"; - const Eigen::Matrix4f result_pose_matrix = ndt_ptr_->getFinalTransformation(); - Eigen::Affine3d result_pose_affine; - result_pose_affine.matrix() = result_pose_matrix.cast(); - const geometry_msgs::msg::Pose result_pose_msg = tf2::toMsg(result_pose_affine); - - const std::vector> - result_pose_matrix_array = ndt_ptr_->getFinalTransformationArray(); - std::vector result_pose_msg_array; - for (const auto & pose_matrix : result_pose_matrix_array) { - Eigen::Affine3d pose_affine; - pose_affine.matrix() = pose_matrix.cast(); - const geometry_msgs::msg::Pose pose_msg = tf2::toMsg(pose_affine); - result_pose_msg_array.push_back(pose_msg); - } - const auto exe_end_time = std::chrono::system_clock::now(); const double exe_time = std::chrono::duration_cast(exe_end_time - exe_start_time).count() / 1000.0; - const float transform_probability = ndt_ptr_->getTransformationProbability(); - const float nearest_voxel_transformation_likelihood = - ndt_ptr_->getNearestVoxelTransformationLikelihood(); - - const int iteration_num = ndt_ptr_->getFinalNumIteration(); - + // perform several validations /***************************************************************************** The reason the add 2 to the ndt_ptr_->getMaximumIterations() is that there are bugs in implementation of ndt. @@ -566,138 +502,48 @@ void NDTScanMatcher::callbackSensorPoints( These bugs are now resolved in original pcl implementation. https://github.com/PointCloudLibrary/pcl/blob/424c1c6a0ca97d94ca63e5daff4b183a4db8aae4/registration/include/pcl/registration/impl/ndt.hpp#L73-L180 *****************************************************************************/ - bool is_ok_iteration_num = iteration_num < ndt_ptr_->getMaximumIterations() + 2; - if (!is_ok_iteration_num) { - RCLCPP_WARN( - get_logger(), - "The number of iterations has reached its upper limit. The number of iterations: %d, Limit: " - "%d", - iteration_num, ndt_ptr_->getMaximumIterations() + 2); - } - + bool is_ok_iteration_num = + validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations() + 2); bool is_local_optimal_solution_oscillation = false; if (!is_ok_iteration_num) { - is_local_optimal_solution_oscillation = isLocalOptimalSolutionOscillation( - result_pose_matrix_array, oscillation_threshold_, inversion_vector_threshold_); + is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation( + ndt_result.transformation_array, oscillation_threshold_, inversion_vector_threshold_); } - - bool is_ok_converged_param = false; - if (converged_param_type_ == ConvergedParamType::TRANSFORM_PROBABILITY) { - is_ok_converged_param = transform_probability > converged_param_transform_probability_; - if (!is_ok_converged_param) { - RCLCPP_WARN( - get_logger(), "Transform Probability is below the threshold. Score: %lf, Threshold: %lf", - transform_probability, converged_param_transform_probability_); - } - } else if (converged_param_type_ == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { - is_ok_converged_param = nearest_voxel_transformation_likelihood > - converged_param_nearest_voxel_transformation_likelihood_; - if (!is_ok_converged_param) { - RCLCPP_WARN( - get_logger(), - "Nearest Voxel Transform Probability is below the threshold. Score: %lf, Threshold: %lf", - nearest_voxel_transformation_likelihood, - converged_param_nearest_voxel_transformation_likelihood_); - } - } else { - is_ok_converged_param = false; - RCLCPP_ERROR_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 1, "Unknown converged param type."); - } - - bool is_converged = false; + bool is_ok_converged_param = validate_converged_param( + ndt_result.transform_probability, ndt_result.nearest_voxel_transformation_likelihood); + bool is_converged = is_ok_iteration_num && is_ok_converged_param; static size_t skipping_publish_num = 0; - if (is_ok_iteration_num && is_ok_converged_param) { - is_converged = true; + if (is_converged) { skipping_publish_num = 0; } else { - is_converged = false; ++skipping_publish_num; RCLCPP_WARN(get_logger(), "Not Converged"); } // publish - geometry_msgs::msg::PoseStamped result_pose_stamped_msg; - result_pose_stamped_msg.header.stamp = sensor_ros_time; - result_pose_stamped_msg.header.frame_id = map_frame_; - result_pose_stamped_msg.pose = result_pose_msg; - - geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; - result_pose_with_cov_msg.header.stamp = sensor_ros_time; - result_pose_with_cov_msg.header.frame_id = map_frame_; - result_pose_with_cov_msg.pose.pose = result_pose_msg; - result_pose_with_cov_msg.pose.covariance = output_pose_covariance_; - - if (is_converged) { - ndt_pose_pub_->publish(result_pose_stamped_msg); - ndt_pose_with_covariance_pub_->publish(result_pose_with_cov_msg); - } - - publishTF(ndt_base_frame_, result_pose_stamped_msg); + initial_pose_with_covariance_pub_->publish(interpolator.get_current_pose()); + exe_time_pub_->publish(make_float32_stamped(sensor_ros_time, exe_time)); + transform_probability_pub_->publish( + make_float32_stamped(sensor_ros_time, ndt_result.transform_probability)); + nearest_voxel_transformation_likelihood_pub_->publish( + make_float32_stamped(sensor_ros_time, ndt_result.nearest_voxel_transformation_likelihood)); + iteration_num_pub_->publish(make_int32_stamped(sensor_ros_time, ndt_result.iteration_num)); + publish_tf(sensor_ros_time, ndt_result.pose); + publish_pose(sensor_ros_time, ndt_result.pose, is_converged); + publish_marker(sensor_ros_time, ndt_result.transformation_array); + publish_initial_to_result_distances( + sensor_ros_time, ndt_result.pose, interpolator.get_current_pose(), interpolator.get_old_pose(), + interpolator.get_new_pose()); auto sensor_points_mapTF_ptr = std::make_shared>(); pcl::transformPointCloud( - *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, result_pose_matrix); - sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg; - pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg); - sensor_points_mapTF_msg.header.stamp = sensor_ros_time; - sensor_points_mapTF_msg.header.frame_id = map_frame_; - sensor_aligned_pose_pub_->publish(sensor_points_mapTF_msg); - - initial_pose_with_covariance_pub_->publish(initial_pose_cov_msg); - - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker marker; - marker.header.stamp = sensor_ros_time; - marker.header.frame_id = map_frame_; - marker.type = visualization_msgs::msg::Marker::ARROW; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1); - int i = 0; - marker.ns = "result_pose_matrix_array"; - marker.action = visualization_msgs::msg::Marker::ADD; - for (const auto & pose_msg : result_pose_msg_array) { - marker.id = i++; - marker.pose = pose_msg; - marker.color = ExchangeColorCrc((1.0 * i) / 15.0); - marker_array.markers.push_back(marker); - } - // TODO(Tier IV): delete old marker - for (; i < ndt_ptr_->getMaximumIterations() + 2;) { - marker.id = i++; - marker.pose = geometry_msgs::msg::Pose(); - marker.color = ExchangeColorCrc(0); - marker_array.markers.push_back(marker); - } - ndt_marker_pub_->publish(marker_array); - - exe_time_pub_->publish(makeFloat32Stamped(sensor_ros_time, exe_time)); - - transform_probability_pub_->publish(makeFloat32Stamped(sensor_ros_time, transform_probability)); - nearest_voxel_transformation_likelihood_pub_->publish( - makeFloat32Stamped(sensor_ros_time, nearest_voxel_transformation_likelihood)); - - iteration_num_pub_->publish(makeInt32Stamped(sensor_ros_time, iteration_num)); - - const float initial_to_result_distance = - norm(initial_pose_cov_msg.pose.pose.position, result_pose_with_cov_msg.pose.pose.position); - initial_to_result_distance_pub_->publish( - makeFloat32Stamped(sensor_ros_time, initial_to_result_distance)); - - const float initial_to_result_distance_old = - norm(initial_pose_old_msg_ptr->pose.pose.position, result_pose_with_cov_msg.pose.pose.position); - initial_to_result_distance_old_pub_->publish( - makeFloat32Stamped(sensor_ros_time, initial_to_result_distance_old)); - - const float initial_to_result_distance_new = - norm(initial_pose_new_msg_ptr->pose.pose.position, result_pose_with_cov_msg.pose.pose.position); - initial_to_result_distance_new_pub_->publish( - makeFloat32Stamped(sensor_ros_time, initial_to_result_distance_new)); + *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, pose_to_matrix4f(ndt_result.pose)); + publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_mapTF_ptr); - key_value_stdmap_["transform_probability"] = std::to_string(transform_probability); + key_value_stdmap_["transform_probability"] = std::to_string(ndt_result.transform_probability); key_value_stdmap_["nearest_voxel_transformation_likelihood"] = - std::to_string(nearest_voxel_transformation_likelihood); - key_value_stdmap_["iteration_num"] = std::to_string(iteration_num); + std::to_string(ndt_result.nearest_voxel_transformation_likelihood); + key_value_stdmap_["iteration_num"] = std::to_string(ndt_result.iteration_num); key_value_stdmap_["skipping_publish_num"] = std::to_string(skipping_publish_num); if (is_local_optimal_solution_oscillation) { key_value_stdmap_["is_local_optimal_solution_oscillation"] = "1"; @@ -706,7 +552,47 @@ void NDTScanMatcher::callbackSensorPoints( } } -geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCarlo( +void NDTScanMatcher::transform_sensor_measurement( + const std::string source_frame, const std::string target_frame, + const pcl::shared_ptr> sensor_points_input_ptr, + pcl::shared_ptr> sensor_points_output_ptr) +{ + auto TF_target_to_source_ptr = std::make_shared(); + get_transform(target_frame, source_frame, TF_target_to_source_ptr); + const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = + tier4_autoware_utils::transform2pose(*TF_target_to_source_ptr); + const Eigen::Matrix4f base_to_sensor_matrix = + pose_to_matrix4f(target_to_source_pose_stamped.pose); + pcl::transformPointCloud( + *sensor_points_input_ptr, *sensor_points_output_ptr, base_to_sensor_matrix); +} + +NdtResult NDTScanMatcher::align(const geometry_msgs::msg::Pose & initial_pose_msg) +{ + const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose_msg); + + auto output_cloud = std::make_shared>(); + ndt_ptr_->align(*output_cloud, initial_pose_matrix); + + const std::vector> + transformation_array_matrix = ndt_ptr_->getFinalTransformationArray(); + std::vector transformation_array_msg; + for (auto pose_matrix : transformation_array_matrix) { + geometry_msgs::msg::Pose pose_ros = matrix4f_to_pose(pose_matrix); + transformation_array_msg.push_back(pose_ros); + } + + NdtResult ndt_result; + ndt_result.pose = matrix4f_to_pose(ndt_ptr_->getFinalTransformation()); + ndt_result.transformation_array = transformation_array_msg; + ndt_result.transform_probability = ndt_ptr_->getTransformationProbability(); + ndt_result.nearest_voxel_transformation_likelihood = + ndt_ptr_->getNearestVoxelTransformationLikelihood(); + ndt_result.iteration_num = ndt_ptr_->getFinalNumIteration(); + return ndt_result; +} + +geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_carlo( const std::shared_ptr> & ndt_ptr, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) { @@ -717,42 +603,26 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCar // generateParticle const auto initial_poses = - createRandomPoseArray(initial_pose_with_cov, initial_estimate_particles_num_); + create_random_pose_array(initial_pose_with_cov, initial_estimate_particles_num_); std::vector particle_array; auto output_cloud = std::make_shared>(); for (unsigned int i = 0; i < initial_poses.size(); i++) { const auto & initial_pose = initial_poses[i]; + const NdtResult ndt_result = align(initial_pose); - const Eigen::Affine3d initial_pose_affine = fromRosPoseToEigen(initial_pose); - const Eigen::Matrix4f initial_pose_matrix = initial_pose_affine.matrix().cast(); - - ndt_ptr->align(*output_cloud, initial_pose_matrix); - - const Eigen::Matrix4f result_pose_matrix = ndt_ptr->getFinalTransformation(); - Eigen::Affine3d result_pose_affine; - result_pose_affine.matrix() = result_pose_matrix.cast(); - const geometry_msgs::msg::Pose result_pose = tf2::toMsg(result_pose_affine); - - const auto transform_probability = ndt_ptr->getTransformationProbability(); - const auto num_iteration = ndt_ptr->getFinalNumIteration(); - - Particle particle(initial_pose, result_pose, transform_probability, num_iteration); + Particle particle( + initial_pose, ndt_result.pose, ndt_result.transform_probability, ndt_result.iteration_num); particle_array.push_back(particle); - const auto marker_array = makeDebugMarkers( + const auto marker_array = make_debug_markers( this->now(), map_frame_, tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1), particle, i); ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); auto sensor_points_mapTF_ptr = std::make_shared>(); - const auto sensor_points_baselinkTF_ptr = ndt_ptr->getInputSource(); pcl::transformPointCloud( - *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, result_pose_matrix); - sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg; - pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg); - sensor_points_mapTF_msg.header.stamp = initial_pose_with_cov.header.stamp; - sensor_points_mapTF_msg.header.frame_id = map_frame_; - sensor_aligned_pose_pub_->publish(sensor_points_mapTF_msg); + *ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, pose_to_matrix4f(ndt_result.pose)); + publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr); } auto best_particle_ptr = std::max_element( @@ -768,18 +638,107 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCar return result_pose_with_cov_msg; } -void NDTScanMatcher::publishTF( - const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg) +void NDTScanMatcher::publish_tf( + const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg) +{ + geometry_msgs::msg::PoseStamped result_pose_stamped_msg; + result_pose_stamped_msg.header.stamp = sensor_ros_time; + result_pose_stamped_msg.header.frame_id = map_frame_; + result_pose_stamped_msg.pose = result_pose_msg; + tf2_broadcaster_.sendTransform( + tier4_autoware_utils::pose2transform(result_pose_stamped_msg, ndt_base_frame_)); +} + +void NDTScanMatcher::publish_pose( + const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, + const bool is_converged) +{ + geometry_msgs::msg::PoseStamped result_pose_stamped_msg; + result_pose_stamped_msg.header.stamp = sensor_ros_time; + result_pose_stamped_msg.header.frame_id = map_frame_; + result_pose_stamped_msg.pose = result_pose_msg; + + geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; + result_pose_with_cov_msg.header.stamp = sensor_ros_time; + result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.pose.pose = result_pose_msg; + result_pose_with_cov_msg.pose.covariance = output_pose_covariance_; + + if (is_converged) { + ndt_pose_pub_->publish(result_pose_stamped_msg); + ndt_pose_with_covariance_pub_->publish(result_pose_with_cov_msg); + } +} + +void NDTScanMatcher::publish_point_cloud( + const rclcpp::Time & sensor_ros_time, const std::string & frame_id, + const std::shared_ptr> & sensor_points_mapTF_ptr) +{ + sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg; + pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg); + sensor_points_mapTF_msg.header.stamp = sensor_ros_time; + sensor_points_mapTF_msg.header.frame_id = frame_id; + sensor_aligned_pose_pub_->publish(sensor_points_mapTF_msg); +} + +void NDTScanMatcher::publish_marker( + const rclcpp::Time & sensor_ros_time, const std::vector & pose_array) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + marker.header.stamp = sensor_ros_time; + marker.header.frame_id = map_frame_; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1); + int i = 0; + marker.ns = "result_pose_matrix_array"; + marker.action = visualization_msgs::msg::Marker::ADD; + for (const auto & pose_msg : pose_array) { + marker.id = i++; + marker.pose = pose_msg; + marker.color = exchange_color_crc((1.0 * i) / 15.0); + marker_array.markers.push_back(marker); + } + + // TODO(Tier IV): delete old marker + for (; i < ndt_ptr_->getMaximumIterations() + 2;) { + marker.id = i++; + marker.pose = geometry_msgs::msg::Pose(); + marker.color = exchange_color_crc(0); + marker_array.markers.push_back(marker); + } + ndt_marker_pub_->publish(marker_array); +} + +void NDTScanMatcher::publish_initial_to_result_distances( + const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_cov_msg, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_new_msg) { - tf2_broadcaster_.sendTransform(tier4_autoware_utils::pose2transform(pose_msg, child_frame_id)); + const float initial_to_result_distance = + norm(initial_pose_cov_msg.pose.pose.position, result_pose_msg.position); + initial_to_result_distance_pub_->publish( + make_float32_stamped(sensor_ros_time, initial_to_result_distance)); + + const float initial_to_result_distance_old = + norm(initial_pose_old_msg.pose.pose.position, result_pose_msg.position); + initial_to_result_distance_old_pub_->publish( + make_float32_stamped(sensor_ros_time, initial_to_result_distance_old)); + + const float initial_to_result_distance_new = + norm(initial_pose_new_msg.pose.pose.position, result_pose_msg.position); + initial_to_result_distance_new_pub_->publish( + make_float32_stamped(sensor_ros_time, initial_to_result_distance_new)); } -bool NDTScanMatcher::getTransform( +bool NDTScanMatcher::get_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr) { const geometry_msgs::msg::TransformStamped identity = - identityTransformStamped(this->now(), target_frame, source_frame); + identity_transform_stamped(this->now(), target_frame, source_frame); if (target_frame == source_frame) { *transform_stamped_ptr = identity; @@ -800,39 +759,52 @@ bool NDTScanMatcher::getTransform( return true; } -bool NDTScanMatcher::validateTimeStampDifference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) +bool NDTScanMatcher::validate_num_iteration(const int iter_num, const int max_iter_num) { - const double dt = std::abs((target_time - reference_time).seconds()); - if (dt > time_tolerance_sec) { + bool is_ok_iter_num = iter_num < max_iter_num; + if (!is_ok_iter_num) { RCLCPP_WARN( get_logger(), - "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " - "difference is %lf[sec] (the tolerance is %lf[sec]).", - reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); - return false; + "The number of iterations has reached its upper limit. The number of iterations: %d, Limit: " + "%d", + iter_num, max_iter_num); } - return true; + return is_ok_iter_num; } -bool NDTScanMatcher::validatePositionDifference( - const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, - const double distance_tolerance_m_) +bool NDTScanMatcher::validate_score( + const double score, const double score_threshold, const std::string score_name) { - double distance = norm(target_point, reference_point); - if (distance > distance_tolerance_m_) { + bool is_ok_score = score > score_threshold; + if (!is_ok_score) { RCLCPP_WARN( - get_logger(), - "Validation error. The distance from reference position to target position is %lf[m] (the " - "tolerance is %lf[m]).", - distance, distance_tolerance_m_); - return false; + get_logger(), "%s is below the threshold. Score: %lf, Threshold: %lf", score_name.c_str(), + score, score_threshold); } - return true; + return is_ok_score; +} + +bool NDTScanMatcher::validate_converged_param( + const double & transform_probability, const double & nearest_voxel_transformation_likelihood) +{ + bool is_ok_converged_param = false; + if (converged_param_type_ == ConvergedParamType::TRANSFORM_PROBABILITY) { + is_ok_converged_param = validate_score( + transform_probability, converged_param_transform_probability_, "Transform Probability"); + } else if (converged_param_type_ == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { + is_ok_converged_param = validate_score( + nearest_voxel_transformation_likelihood, + converged_param_nearest_voxel_transformation_likelihood_, + "Nearest Voxel Transformation Likelihood"); + } else { + is_ok_converged_param = false; + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1, "Unknown converged param type."); + } + return is_ok_converged_param; } -std::optional NDTScanMatcher::interpolateRegularizationPose( +std::optional NDTScanMatcher::interpolate_regularization_pose( const rclcpp::Time & sensor_ros_time) { if (regularization_pose_msg_ptr_array_.empty()) { @@ -840,23 +812,24 @@ std::optional NDTScanMatcher::interpolateRegularizationPose( } // synchronization - auto regularization_old_msg_ptr = - std::make_shared(); - auto regularization_new_msg_ptr = - std::make_shared(); - getNearestTimeStampPose( - regularization_pose_msg_ptr_array_, sensor_ros_time, regularization_old_msg_ptr, - regularization_new_msg_ptr); - popOldPose(regularization_pose_msg_ptr_array_, sensor_ros_time); - - const geometry_msgs::msg::PoseStamped regularization_pose_msg = - interpolatePose(*regularization_old_msg_ptr, *regularization_new_msg_ptr, sensor_ros_time); - // if the interpolatePose fails, 0.0 is stored in the stamp - if (rclcpp::Time(regularization_pose_msg.header.stamp).seconds() == 0.0) { + PoseArrayInterpolator interpolator(this, sensor_ros_time, regularization_pose_msg_ptr_array_); + + pop_old_pose(regularization_pose_msg_ptr_array_, sensor_ros_time); + + // if the interpolate_pose fails, 0.0 is stored in the stamp + if (rclcpp::Time(interpolator.get_current_pose().header.stamp).seconds() == 0.0) { return std::nullopt; } - Eigen::Affine3d regularization_pose_affine; - tf2::fromMsg(regularization_pose_msg.pose, regularization_pose_affine); - return regularization_pose_affine.matrix().cast(); + return pose_to_matrix4f(interpolator.get_current_pose().pose.pose); +} + +void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_time) +{ + ndt_ptr_->unsetRegularizationPose(); + std::optional pose_opt = interpolate_regularization_pose(sensor_ros_time); + if (pose_opt.has_value()) { + ndt_ptr_->setRegularizationPose(pose_opt.value()); + RCLCPP_DEBUG_STREAM(get_logger(), "Regularization pose is set to NDT"); + } } diff --git a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp new file mode 100644 index 0000000000000..b770295277346 --- /dev/null +++ b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp @@ -0,0 +1,105 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "ndt_scan_matcher/pose_array_interpolator.hpp" + +PoseArrayInterpolator::PoseArrayInterpolator( + rclcpp::Node * node, const rclcpp::Time target_ros_time, + const std::deque & pose_msg_ptr_array) +: logger_(node->get_logger()), + clock_(*node->get_clock()), + current_pose_ptr_(new PoseWithCovarianceStamped), + old_pose_ptr_(new PoseWithCovarianceStamped), + new_pose_ptr_(new PoseWithCovarianceStamped), + success_(true) +{ + get_nearest_timestamp_pose(pose_msg_ptr_array, target_ros_time, old_pose_ptr_, new_pose_ptr_); + const geometry_msgs::msg::PoseStamped interpolated_pose_msg = + interpolate_pose(*old_pose_ptr_, *new_pose_ptr_, target_ros_time); + current_pose_ptr_->header = interpolated_pose_msg.header; + current_pose_ptr_->pose.pose = interpolated_pose_msg.pose; + current_pose_ptr_->pose.covariance = old_pose_ptr_->pose.covariance; +} + +PoseArrayInterpolator::PoseArrayInterpolator( + rclcpp::Node * node, const rclcpp::Time target_ros_time, + const std::deque & pose_msg_ptr_array, + const double & pose_timeout_sec, const double & pose_distance_tolerance_meters) +: PoseArrayInterpolator(node, target_ros_time, pose_msg_ptr_array) +{ + // check the time stamp + bool is_old_pose_valid = + validate_time_stamp_difference(old_pose_ptr_->header.stamp, target_ros_time, pose_timeout_sec); + bool is_new_pose_valid = + validate_time_stamp_difference(new_pose_ptr_->header.stamp, target_ros_time, pose_timeout_sec); + + // check the position jumping (ex. immediately after the initial pose estimation) + bool is_pose_diff_valid = validate_position_difference( + old_pose_ptr_->pose.pose.position, new_pose_ptr_->pose.pose.position, + pose_distance_tolerance_meters); + + // all validations must be true + if (!(is_old_pose_valid & is_new_pose_valid & is_pose_diff_valid)) { + RCLCPP_WARN(logger_, "Validation error."); + } +} + +geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_current_pose() +{ + return *current_pose_ptr_; +} + +geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_old_pose() +{ + return *old_pose_ptr_; +} + +geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_new_pose() +{ + return *new_pose_ptr_; +} + +bool PoseArrayInterpolator::is_success() { return success_; } + +bool PoseArrayInterpolator::validate_time_stamp_difference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec) const +{ + const double dt = std::abs((target_time - reference_time).seconds()); + bool success = dt < time_tolerance_sec; + if (!success) { + RCLCPP_WARN( + logger_, + "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " + "difference is %lf[sec] (the tolerance is %lf[sec]).", + reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); + } + return success; +} + +bool PoseArrayInterpolator::validate_position_difference( + const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, + const double distance_tolerance_m_) const +{ + double distance = norm(target_point, reference_point); + bool success = distance < distance_tolerance_m_; + if (!success) { + RCLCPP_WARN( + logger_, + "Validation error. The distance from reference position to target position is %lf[m] (the " + "tolerance is %lf[m]).", + distance, distance_tolerance_m_); + } + return success; +} diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/ndt_scan_matcher/src/util_func.cpp index 86d994b7c2361..237b85ad75e10 100644 --- a/localization/ndt_scan_matcher/src/util_func.cpp +++ b/localization/ndt_scan_matcher/src/util_func.cpp @@ -19,7 +19,7 @@ static std::random_device seed_gen; // ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA ExchangeColorCrc(double x) +std_msgs::msg::ColorRGBA exchange_color_crc(double x) { std_msgs::msg::ColorRGBA color; @@ -47,7 +47,7 @@ std_msgs::msg::ColorRGBA ExchangeColorCrc(double x) return color; } -double calcDiffForRadian(const double lhs_rad, const double rhs_rad) +double calc_diff_for_radian(const double lhs_rad, const double rhs_rad) { double diff_rad = lhs_rad - rhs_rad; if (diff_rad > M_PI) { @@ -64,7 +64,7 @@ Eigen::Map makeEigenCovariance(const std::array & } // x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose) { geometry_msgs::msg::Vector3 rpy; tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); @@ -72,17 +72,17 @@ geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) return rpy; } -geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose) +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose) { - return getRPY(pose.pose); + return get_rpy(pose.pose); } -geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { - return getRPY(pose.pose.pose); + return get_rpy(pose.pose.pose); } -geometry_msgs::msg::Twist calcTwist( +geometry_msgs::msg::Twist calc_twist( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) { const rclcpp::Duration dt = rclcpp::Time(pose_b.header.stamp) - rclcpp::Time(pose_a.header.stamp); @@ -92,8 +92,8 @@ geometry_msgs::msg::Twist calcTwist( return geometry_msgs::msg::Twist(); } - const auto pose_a_rpy = getRPY(pose_a); - const auto pose_b_rpy = getRPY(pose_b); + const auto pose_a_rpy = get_rpy(pose_a); + const auto pose_b_rpy = get_rpy(pose_b); geometry_msgs::msg::Vector3 diff_xyz; geometry_msgs::msg::Vector3 diff_rpy; @@ -101,9 +101,9 @@ geometry_msgs::msg::Twist calcTwist( diff_xyz.x = pose_b.pose.position.x - pose_a.pose.position.x; diff_xyz.y = pose_b.pose.position.y - pose_a.pose.position.y; diff_xyz.z = pose_b.pose.position.z - pose_a.pose.position.z; - diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x); - diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y); - diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z); + diff_rpy.x = calc_diff_for_radian(pose_b_rpy.x, pose_a_rpy.x); + diff_rpy.y = calc_diff_for_radian(pose_b_rpy.y, pose_a_rpy.y); + diff_rpy.z = calc_diff_for_radian(pose_b_rpy.z, pose_a_rpy.z); geometry_msgs::msg::Twist twist; twist.linear.x = diff_xyz.x / dt_s; @@ -116,7 +116,7 @@ geometry_msgs::msg::Twist calcTwist( return twist; } -void getNearestTimeStampPose( +void get_nearest_timestamp_pose( const std::deque & pose_cov_msg_ptr_array, const rclcpp::Time & time_stamp, @@ -138,7 +138,7 @@ void getNearestTimeStampPose( } } -geometry_msgs::msg::PoseStamped interpolatePose( +geometry_msgs::msg::PoseStamped interpolate_pose( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, const rclcpp::Time & time_stamp) { @@ -150,10 +150,10 @@ geometry_msgs::msg::PoseStamped interpolatePose( return geometry_msgs::msg::PoseStamped(); } - const auto twist = calcTwist(pose_a, pose_b); + const auto twist = calc_twist(pose_a, pose_b); const double dt = (time_stamp - pose_a_time_stamp).seconds(); - const auto pose_a_rpy = getRPY(pose_a); + const auto pose_a_rpy = get_rpy(pose_a); geometry_msgs::msg::Vector3 xyz; geometry_msgs::msg::Vector3 rpy; @@ -177,7 +177,7 @@ geometry_msgs::msg::PoseStamped interpolatePose( return pose; } -geometry_msgs::msg::PoseStamped interpolatePose( +geometry_msgs::msg::PoseStamped interpolate_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp) { @@ -189,10 +189,10 @@ geometry_msgs::msg::PoseStamped interpolatePose( tmp_pose_b.header = pose_b.header; tmp_pose_b.pose = pose_b.pose.pose; - return interpolatePose(tmp_pose_a, tmp_pose_b, time_stamp); + return interpolate_pose(tmp_pose_a, tmp_pose_b, time_stamp); } -void popOldPose( +void pop_old_pose( std::deque & pose_cov_msg_ptr_array, const rclcpp::Time & time_stamp) @@ -205,14 +205,38 @@ void popOldPose( } } -Eigen::Affine3d fromRosPoseToEigen(const geometry_msgs::msg::Pose & ros_pose) +Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose) { Eigen::Affine3d eigen_pose; tf2::fromMsg(ros_pose, eigen_pose); return eigen_pose; } -std::vector createRandomPoseArray( +Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose) +{ + Eigen::Affine3d eigen_pose_affine = pose_to_affine3d(ros_pose); + Eigen::Matrix4f eigen_pose_matrix = eigen_pose_affine.matrix().cast(); + return eigen_pose_matrix; +} + +Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos) +{ + Eigen::Vector3d eigen_pos; + eigen_pos.x() = ros_pos.x; + eigen_pos.y() = ros_pos.y; + eigen_pos.z() = ros_pos.z; + return eigen_pos; +} + +geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix) +{ + Eigen::Affine3d eigen_pose_affine; + eigen_pose_affine.matrix() = eigen_pose_matrix.cast(); + geometry_msgs::msg::Pose ros_pose = tf2::toMsg(eigen_pose_affine); + return ros_pose; +} + +std::vector create_random_pose_array( const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num) { std::default_random_engine engine(seed_gen()); @@ -226,7 +250,7 @@ std::vector createRandomPoseArray( std::normal_distribution<> pitch_distribution(0.0, std::sqrt(covariance(4, 4))); std::normal_distribution<> yaw_distribution(0.0, std::sqrt(covariance(5, 5))); - const auto base_rpy = getRPY(base_pose_with_cov); + const auto base_rpy = get_rpy(base_pose_with_cov); std::vector poses; for (int i = 0; i < particle_num; ++i) { @@ -254,3 +278,11 @@ std::vector createRandomPoseArray( return poses; } + +double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + const double dx = p1.x - p2.x; + const double dy = p1.y - p2.y; + const double dz = p1.z - p2.z; + return std::sqrt(dx * dx + dy * dy + dz * dz); +} From c6e055450f5117777ee64f47103baf59f2e21c08 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Thu, 29 Sep 2022 11:55:36 +0900 Subject: [PATCH 25/38] feat: add rtc manager rviz plugin (#1936) * feat: add rtc manager rviz plugin Signed-off-by: tanaka3 * chore: cosmetic change Signed-off-by: tanaka3 * chore: remove indent Signed-off-by: tanaka3 * feat: add rtc safe unsafe color Signed-off-by: tanaka3 * fix: typo Signed-off-by: tanaka3 * chore: simplify layout Signed-off-by: tanaka3 * feat: update rtc panel Signed-off-by: tanaka3 Signed-off-by: tanaka3 --- common/rtc_manager_rviz_plugin/CMakeLists.txt | 28 ++ common/rtc_manager_rviz_plugin/README.md | 20 ++ .../icons/classes/RTCManagerPanel.png | Bin 0 -> 18815 bytes .../images/select_panels.png | Bin 0 -> 67237 bytes common/rtc_manager_rviz_plugin/package.xml | 32 ++ .../plugins/plugin_description.xml | 9 + .../src/rtc_manager_panel.cpp | 332 ++++++++++++++++++ .../src/rtc_manager_panel.hpp | 100 ++++++ 8 files changed, 521 insertions(+) create mode 100644 common/rtc_manager_rviz_plugin/CMakeLists.txt create mode 100644 common/rtc_manager_rviz_plugin/README.md create mode 100644 common/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png create mode 100644 common/rtc_manager_rviz_plugin/images/select_panels.png create mode 100644 common/rtc_manager_rviz_plugin/package.xml create mode 100644 common/rtc_manager_rviz_plugin/plugins/plugin_description.xml create mode 100644 common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp create mode 100644 common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp diff --git a/common/rtc_manager_rviz_plugin/CMakeLists.txt b/common/rtc_manager_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..f2fad9e24665f --- /dev/null +++ b/common/rtc_manager_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.14) +project(rtc_manager_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/rtc_manager_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/rtc_manager_rviz_plugin/README.md b/common/rtc_manager_rviz_plugin/README.md new file mode 100644 index 0000000000000..a4542849e4ce5 --- /dev/null +++ b/common/rtc_manager_rviz_plugin/README.md @@ -0,0 +1,20 @@ +# rtc_manager_rviz_plugin + +## Purpose + +This plugin displays each content of RTC status and switches each module of RTC auto mode. + +## Inputs / Outputs + +### Input + +tbd. + +### Output + +tbd. + +## HowToUse + +1. Start rviz and select panels/Add new panel. + ![select_panel](./images/select_panels.png) diff --git a/common/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png b/common/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png new file mode 100644 index 0000000000000000000000000000000000000000..6a67573717ae18b6008e00077defb27256415663 GIT binary patch literal 18815 zcmc#)ig}7!VL~0BI3P>4u>jX{5WQyQGE==@gXiZcsv`yJ3)ShVHnpzyIKV zx%UIl!@PCQ*=Oy&*IwtGaAid)9L!gkAP@*gMjEOL0wKly`$K;YTnXsp8Ulf2BxRtY zY95&f%V-`irs2ZJD-E_>W|QJ#$f(lDGUzn9FhfdJ6ls+f7g13tl1h)8=nGVpD&-eU zEXJbkUMvxiN-bt%sdy?0Ws#;2U}Tww$D9k?tJrv3b90N4w6q7mY>Vu>j)cq`Gq>&g ztxVotd7x=_%0QrfCTU6#h!Tqc9R&LG1r-?ts`)>?Y`h5pRS_lWag8OXawfXJHk9@r zwSCxzbPRm-g9n1hq$!Q)8a9)#!i(M_cz&h|6vnpsyKVD7twGSL0-4KL6Q=BD1ZeP- zzfzYzWVM-q+?d)>KwJl;5;rOeaVP-p8?mb$R~c z!r4STVAB1bJ?W`Q1N^wk6a>;QVI%JxOu}Vf{^G|)Co3MGH{(9RMV<;nHU4Za)%KSC z`;I=`QN*K~)?CjlT%q3CJ0d$G%J_7^;$JI4Ms3f{6oyJDbjK_N!4ju2 z&$?3P$rm$WTureE-aq`r%H%qf&xx}1+^E65XE7*U*@ASHv#;8ex#{Xeuo?I&+x*og zCp;~E10-qE582|U!y4K~`SF#B31Gjpj3?LDSx6hf<#=NRr5x>o;P$^l9osYrEfL{WYjQydU9;g?K5#3k=W*FphTCuCHewl5&`Uu9F)(f>MMOxB>pE z88huBN|GrNq|eH~M(ELpl-tdZkMhs%;OV+^2?6{7^3=m>h4(Sn1AF4SLTeDL;O9g! zBWaM6c%X0DechlP8B_BUQ^1_B5ImG0q$&AGba@Q1!Sx#uv;b)FNlKJ%FF!gu4goAi z+jugMiX+lt3%Db%$x}1fa)yh%A&xxGW-Lle9BaRZcr(Jl8YJvmXVoM8b*d?nDn zr>xqjT|4gjOmgjyb!-I_L=1WSO(};jF5)F9(KK9k$6juv0YKyrQRHz_DK~8jI2)+w zm-Yt$QfvYLhOutzxp@d+SUG4qO=vJ%C2^D=7atD;N#3;^c;DQzmR&&!oPfkcBJT}x-4FY#kT02m@DxoqS-{xT|$ z3PAj4Y06h5S$bR{FjZhmf2ImPCOB3Cm{OLqda0jhqvfLSxmZ+?A2<^h+z64sYjMk*OaM^512bTNz1yJX#3oL;p>F~$69vB)J^K&J0kyCuHxo2qc9?N7 zpx^)Vh!`XhEMW3-8hQ#=jbwx3L_Yk3cspR5f1h~;US}c=&En=n$AFqJRa-Trw&}AF z#AgA`>wx2@JWAg4l9l5}1GY|0Z9CNsXNdgw(z**Vj@S4BOp0I}AA5Z|Zl~>Z3kVbj zqQ+>~?`j3P7S~|HAAK#cX8I@4)zn@Yx%DiDRJ@~c4bTC4ySPXJL&So)v8RA@6#{J%M_y$DCRzdd0*FPR zEH--gPG5+|{w5Yegr~;hMSkzb)vYj91mGV5xcRrf*}fIu`8(Gk(oJHQTMWSTlpe3W z7U|f@97Ozui1Pqn>xtK!34&Gp1$<#-lg>4RGs7K!6%!+v&|>f%Vhw^400u&sGY~q5 zJ^R2hK)+x%+?N_ujGMppd5=D@{V_pS_0HC2*7x*eApoKv>S`$MhIW>f zMXBEn^5P16FfqDGOxdOTX$;a&7%;G-28$P)$1&8Il!2^?K;(-9A|F432ZYVYM^|}I z3>OV1q`?mb9wc7}dJzwJ)_0FSe8o7Cm(><8hzS0{vBcWu7VZ9!xVd$!*%0qZqPEJb=%hJ$Z_L zz~Miu9bQOq9)39Df;+w>C4k8SnA^IR``!NH4|SUf61t|;@Y}Cs7L7w2kT8`ufki#> z(|*WcvwwZbRQK{xx%^U^9it@ui(i`PZ(a!{U!?$lf%er!QwQi?Jq@BTnXsQ0^KiZy z)@H)?s!l`=|Lg4jvo;>IFs7CP;9l8V4UpAo>sjIq#`%v=kuRv~Dh zsiW)pz26t!@MhZgf>?_yj35jsQu16~;-8_l{?rj9%$d(V-9L6Mv^t{|GGZ5-C`W&WqNh29e6d|hsvmvzE$*g zQmyMXe|TD_9?2}u%kdXf_x#(4xd=B4sAmzB{R*=+ZDMv*{0=ZZw`U z1zHnRLzGA%_kVvk=&UnLH`*nK6OLz)IZ+1RaP z$_a6)pzGO}t0_(S2EeN@?u0pWGf^QOT;Fy&fIf+061Vv&k6#eQ9E?CJbN7 zR~KK=eS#kf?q9C)BZqF)@T7l`iF(-E2|R4|?91b}4Ez)Kk2FyYsGq$iY-#*_u@~1} zergZr={EWSme9lc1Lvw@G~X6iq*U zJ>6TJJ1l)Wu8Wk+%I_jO@X>ttg#2Gtvr6H&5?wENhU3wfOD|1HK$EE0JWPIo?o%@w7QX>1z=lRIcDYmm^RdaH!J$U0qdKjR{v?Nj@k8=mLg z!QaffX+{G^hFLx>yzv{!{4I7+*?C}Mw;W;Akw&+^oGNB zKbo1D;RxOCe8*HY+Wq2Z{a&$bq>kL0E*5uqV%bZU97(Rgg}axR>R3#V{3&SGo$cMU zo>il+6}(CD_7>Sn+RG>LD*ZH>q}A^P_|jLN;2-ZSZ)e^-CR7g&UCtF)C)#ml2sjLp zna0G_?3W<_14PCTp7u%y`1jJLdPCb;WW==}ZA1fH$8?tCsR>bJF?vJ$I|J{? zlTYZ~d#P!IC7nc5MI zRsbBkP4l%D7PaBB{&6DKt4yR^NwBqPM*lr^I~U=AH?+LJ*&C$ZAXr^~8P$6rvc|OQ z@5*T^JLt$TP5T0t(=wiG^sXg zEd~AVuZVrio-s!T&LrqYTve>D*P81_=>7;qF3oJb%9-_$ zl7tF5{GK`$Y)Po@y_Df6U|SUSg7!xbv_*4yVe!Q#Ebc;ZcwoSK)rHUB2ho1zr!2(h z%dVk7oadz}SQ849EKz@)p|3b8py_+bZq+KNZ|#0KdDLC_ZhWXy#9XXyyOh3&D_KBd$d5r~dZ&@?gHCJhG5mniAQ@R>y{O zFvmlJ?JS~a-%K~9k;T?h@u&=-btl<6W(Mr?#p_#Nvx_0v`ilrR<$^+%&Dej}o4vNPn* z;owRb6rB0y*XFvkd6OhCQqGB6I?r)a!X@j6i&CJJ1K0a?>M~fR$7l@HLqkBtR{DNi z#>_e~=e6CHkiV3$L3^TB!NYiWD6Xr+?LjRJ218fc9?44c9VrfHEASeRf4u5=;4Pz2 zbmAarlXGC+<}+MNXuEB2%{j_j84W=YGt*J6aB$pdMP%f^yA?e@n6EWm{~fLLbbmH# zB#6dX=5Pv<(zsg%*jkAKL5r0knZM%tivqc1CatP(x82cXPdwWwcmjs-W@q|ImYY99 zYwlE~Y-hze~lk`x3mjPB!27mzk`xg^y;8^_L_ahVv6~)|t~mfqeU``w(TZ zxrTjOPssfK`oySR$6>Am1>m)tK+)!_P=0!6FfqlwTf!xubMJn*u$lh(uwJ8c<*Bgn zDIx;XIk9`S^Zs-&ZN{2BW%h21L^x#Hy{t&3#$u%2EIrNNs9uMGjS4;G4WkVIc79S?9<_)}5sGh^o2GGu01pfp6?z7wr_`sf6M}tS@hT zNCXk5MDOIEc)b#>BL5Byp4r;f?uV$qGqs%c8FpR1U>>e-v`u`iQ65*US#Q)pH|_z0GX?kiG%%(LOU!%Ckq@v`f@JK|pfX2{xiK)~<5soB5N zZ^n6hgiX)Z0q$xxBO+$zshAz%JT;G?R^ui!yaQF=CYolEefA_D418x5EBtuv6S_T# zt>8C5ZK^P2{Tk3jP{74C9i5Z*XB90Em|L^1Lp$Ty?ZVFx%zQ38_!1Hlo_wx*Jxm>5 z;cSTnM62^vnW0! zv(<%*u%dQ#ZEdDm`^UPysNb89-plAwdG@K0)y_jBD-qUQUN%)_5(TsN78%{K)f*=5FXVTI-5jC-M0r)_e2{Uu>Io;LFZ)WTSayCh zH$y%7yq?2_WZ>t-lJJ(ErM5;8)CMsVjU&indTZFQ!V%ji9)yN-+O%Xp_zMN?x2c&~ z$h26bQIZIOY$0|Hl-A{A>HW>D%Vl99Us^*}%6q+6(o_lL&Dq|Mo_&23{zG+CyVGoa zZdPuB-3?-rE;hgTQB^{1j%hnpE81vUZxe{q7+r62)9R zQjuMBfm(#Ms?@4Z$;Ik^iw9oWqRW`6isW|}$3~+7uuRp_uSRo5+uoV%o;%fYOqe&( z|2-;yrIE5RJ-g8wo6hVoIj1wlF#nl)eXp0d4XYB-b^eG|Cv?<)1s7iS_r5%WWTvIf z2b#SW2vI-|e@Dud15=%5ScGn5xI-4JpMXeprsukuG_&3pJ9vw|;{D{iSX$ZsyN-QS zIkazCCe z5tjTIqBD|W;A`f4 z(o2%2vBi}U($ZixbedSg2KMZ`WSTOQSc*AmMXpEHQN>d_Ts3UpJGs9*!n#G=T_Pyr zRXRI_^rJ$49%InZRr>IOdrQ?H&qWVYSv0t<)_MjA=@l6!>}m{uAv-nCbu4CkNL1I0 zszx2HgbeMhEPPFWT{;-iq%i3PmyXoFdjD&`g5k23{NZw;w9<>q|Iu^oZgl-bE6dSw z^mlKsUR{#$i%`XX%3&%n7Tqbvr-=iv3Vn!kYG;%8(aknQHmT914|hK(l9+ATj)ON8U+A7k}qtQoh z(o1n!2r2de-qqvX8LPa^T2)TVwotK{{5PUlaF}h}t$?CyJIDXX_j2AsLij`a@Rsi6 zT9`WmSJ@(5~J3(YDA@J|o3An#c=nA#EWWa`laHX*T$SjobapA&u?(UWP+Q{C%@7Q}QA^ zJNv{`cm#_$-9Pc`0QTcIA;7WaQ;?NPKQ)wA2wg2XAF*`a%1+DL{Ei|8qxs#0>_+qE zIygdQ6ZJGXrnY0=Ug)~OeO>jx(RvJib8E2<`5+hRj<=UQ`XSK&;bJyBjKUK-vv>=H zGCKa(@!Sj)ME<{YC-0A1b(Bqx63NF8LaQ+pwr7++?Y;83W-@6>tv_S?kk#VM|MVs2LY-DySvyPfa--K5g8|I^)y zxR)i5=c%cxpXe{y&Af@{;>*~{b02i&{aB_$54p;>&#@87qjme%iw$;g@1y3hdBV!k zZH`JGW7Tng^Ak~%}#;Q-2Kv@J$5 zPoiIQ%EmkU-JkTeJJ;0SF{mVKykranq(ebDsHb&e)<5FL`@L~x6=ryQdf$-7vCQ0< zVKw1(9v>zMv~Hr4MzS?k%k=W&z-eNFmOLHeG{ZZM>d@B7MqQoA_xwa^NGPaDE9_=G zqs6AdhWYsONzd@+OaTr-8XOBq(wvBuPJ?Raqs7jrj-Q(vB?55luOh~nXxNd`{VH~I zM;8yWZJr7r(}dDk^xNhxwsalXR!sA1N(T6xx8%3 z9i?;He4?%(U&RqXs^zm7y_}z_XZ`rKJWdqi+!lDPbqjFCc03mCyq$|H4*?12d18Xu zsc9-IL6-9k#5yU9z|37?M|bFc*A_tKQa1XE*M2$dVt-b%RtQm5BcL!+`EtBIz{=SB z>EVhR7s6-iy&6K2n(c0w;sbP}fDO(N@z^>W*1LXDq9TP$lSj^&IGFi{KpGIP-++mV zR2tI$TwO=$)6FcJ+Rs%5-rd7wzS5;BWefCwh}pkOiJM7&g&1R6o^Ft!tBsfjK$~LV zchl!;(BZ>dy(Cy;~wVJ*0yFzq6!EcXR5$rY?4gI54mUi2JbS2$QJGSQ5Xyxt;M>AF4Sv@28`bN@} zlqwK4bFtGi`_*QHwN9+3#N)ub$|i36r9T2zdP63$?>Q-i9luzG2$gB(w!3~+Q&2P3 ztg~7hwvPLAu**HR*1BKO`siW+cbQb!0_qL(aGzg2%;p*MKf-C??dsxN<>Sk(?%vrz zzc_PV+EeGDFeZ;i84*ZtW=H3=#2j|W^|ND<(N6Ys^vYL`H`p!um`KLX43QJ|#XvZ& z5VNDI>gv4*nZ&heTY8$rMpSG1;mfQ=IFry0%W*2X;r!dHgLxC(0(fUSoX7 zwNjEXD0t!s_`m5$=5BFW$;8yBAcYLBacVr_p>vrk2KwkD((ynWWJf^m6M0>Rs^j&K zcuG{Bb;k~FsXL?edF#}7S zHZDLmq@^Y_Y4fI*ecn|MwalQipJKC!`&+AEi=Oxumg^tYvS0s6v}&dED>#4Eap zAJNq9yf;-G>9>swxt+fKLmkVyKVy>_Z%8^@94F5{sE|(%yu=RF} zU|f+?>1X;0!Jp9#;p0r9dszlE)NBFz%Xb>g4B!|F;iIe%Rte_9u|W9MNnz3b-6obM zRP{c^sD1ZD1@8DThqDat@NK>7r_aG&(BpD|3!JTXho#kHn#b%IMn={-%208He(I{j z8s5K{sLB<8*d9?YNvdlAkE1Xq4M&^5V`}zED!eajyno=M;-hVhny2RT;+R}Z9r4hs zHg0|z%N0-eaMaHz%=Q`>bdt9ENtBg7zrZD9*0FRwt#|ss^{%z@XNh|0y-*Iz&V$F+ z5b1tlj^6+i1CSkW24b4a6t9GRb-61JetIWG3TakcE8lU{iJ1*WfY zhT3M?F_AEMwx8^2o>J9?B73d2M-Ao0yblIV=w|pGk7+ zChG0Dy~rHOK1Z}(>0iZDOF^?7HV2aYQZklSUv5SgTzl!7E#o4p(rOC24ScRx|^VD=~3JdRynp4w?)W(5A0 z)_C3;5w{#ut;AeK9af+D3$6?#Btje>?{BQL*L-vD2I8Ik7Vg!@QnLe zppa1pgsr!=`wq1_mTHZ}C3xl@K9Vf_gQQRAD_36GdaZ87H=x1+REu&}7i5gjPv`MP z?=NN3^VvFySA4Is?wUDmnv~`eOyF%d=CNUipe$ZOJ}FZgXz3+QW*Yt>xpMko-JL05 zks6_=q<8Zb!SRc%GSdhvd#^TbuH?h6Nef0sA3Wjvs`Q+zGw6AI#HO>wHLBBX;iyJI zFZ5A$b~Ji_KEX=v=l0q7@oKkklDfM5JEnZw<(BE1>)c*>-8TRsJgfKBDNYu#C!HGU zbLvbP6<#?=;4iU0T&Pb*LBnMo{A)%V&E)8oVNDoVfNMGIGqDC~Tscy8s*HW5h(C^+ zG%$nRO&>=ccGarn_c)}SF|TH0#$7Cx8+pN%20+veAhSU?RF1MOcSo&5t!{_L=^j!S zwpKQodunYpRvLi#(OCA4LtJoKXSCHK;j8k9qyr5?e~KQ|t>1+SXe) z>)&;E*Mlsb=gfL7YJkKP1nqI#dXEOw6i8@(OKrTvK3jR+J9~Xn-iY+EwA~bLWlsRR z23NE2O*dU)=1+e<1VW`GQj_6h3MbAnyDHL5QY*f|gl9W++|d2#xN2PS<3Ii9G|AA= zOQyQ5g5I}YLVzr($N9aHm+H&mo*)(pv+|j@cP5Nnz1nAZ%$JsKX2hLXNhUIS$$QnG z1AL<~%s^invTXLbb=gI`FD6^i`_j_7{ay3FG_nui=Feq_SET$RM11j`*GRQ*M5-@2 zd2E<{+liz^%K)8H_K-`#W19`%J018J8$M5t{XUL%67!Kv?T5Y6%HoSPfj9xo@SRNA zO&i%cnz)EbdPRxH)73v%VP9f)rk1ApgoAeZVYB+))9p8Bqd4&bBR~*`oNY>09O~N2 z!Rp>7KnHX=U8>;-697~ZJ_sN<+Irs|Hl_`;G1z694OixT`*V|Cz_YBGLc(tM@Kohm z>JD&MYd3Xf+B|B;lk$xwEPD9_~%=V(vx(C@a6Y-H$#=bfVOn57$&>rkwrVs4#bi zIXh7e(IM6w&y9tw+~I(;y6HV5=yM_(oXxC~r9=|;e>##2d}?n{dgmZtm$kdn2_X2$ zZn1&1c*eQwvMIIRLX^A-fxxJ(8WBPe?||LSuy z{t{iR`A!>-41uqe*4_8|FWHQLq|ffyeq*aJzDYNo!z||t!@KwR;k$ICGOlv)`wp!Q zFe7=&V)-C1g>f{5f5E>X@5^pc*;|x_)H3t0Wj(Hdf}t6f7dj;Lm3y|b61$Fgis3HI zZZTE=iadp@o=Y|={({-`cL3Mp!SitW{cQ0t++xA?d#mq<7jHsy-_FH#^te&BG6e9=TM4K?yE4$gLtJB{QzeetB zryUa6*x1vKi=Ur?^*FjI-@8jse%C$IlbLGM)=y!06b)*c>gqrMkCt)IoHxxwf;h>K zI$ZxUI0Qd&(@HM3+nD1SE<}@A-dyZ1FM9v|iBvCi@E-ah@k7-!4x7-5u`g+?MqO*l zm)P-Zzu$iR6iAlY+w9>D6kT>tx3it8Zf2XNw7OwQn=-dqz_;&l(OUI~)JrvPrtJfn zwVGJjkAIYJ@iHeSyf)eOw~G0ZJld2UGPIIZu3h{4455%Qbz6lZW74FexmYeYM;8aI z$K-h3Y^GFFCuw>-YX)_9?Ig^9A4wx1xQ2#eYVG!-LK}|bv$R8@eqlJgC_fD*}q9KhIE1f_i#O7_ zT)%x`)T#K2OJP6!v6Vb?t~cc+MQq6?;as!glC6mA<(e z<%lFRX`h0@1C67z0NHWh|ZjuVZ|V8t@FP@kIUYdtPp)2I9s3B_BXeBp}-ZP z5|l~AJ-|-~e7adHXKm6rZHQ~nBqb!qRwUhuE_?tsh^Kl=Hlz{jJv5 z%a`iA?S8PdtMf0~>M^6t2;7z3^z?rn(s|R}N+Jo!JO1-*rxQyqqw0F6X>RM!ACLOT zz2%2LmeJ%LT4#%}#EGCHI_{2UWy@EDyhONJs|(RvT?Z?St5 zJe~uJKn9oH8ehO#Qs>h$z{xbsOPOK-DR@vG_GmD%r#DqpIINVFeAg~=yeiV@S@ycUgHP|m(|5vgek*ZEH4RdwK{d$6jL>+4X z`4ChMuWjS9nJF{!ij95ygn<6)v21l;z0gZ@UjZab`9s9KK++(ZU}hS#jlPp@YHsdX+N+j<%}bAm_k9)T8@LAgyH4JzSyw>)>XO5eYZ-f(KZq4CZ8KAE}&p33a#nAw#ZL!zhkYZa0_fy&Jh12UfTAj4W?< z)O2(X-CC!M1+7n}8-n0j-m&cbyUhc#29lDYkyrH{#~wfcNz*@%?SyYHR3jK#qYkg` zfOxt;Xp>!NetI3?-j2$*c6E0peFJ1k0qtRyN~0J}eb0QIcI)6yPMaCaqc?e?=5%L0 z!wWTLgHwtNdLFYf->C5f&&TSGJKKO9egTz|q7H=dL_|bSuP9CLa~>76 znCtPqJkWLkR->ldp7iOcx({hE83#s3x$hnxA+ElF{8rgnRi*qnM5H$vfR4=(~l-hR-WQi)59iT6|TZNDzuRd5R$FCqNF)uguYRz#H!g{_d46@8|;6d)C31g+?wwl9UWUN!_7@iU(y8@`IP%@=plBH%=02yJ&ynGhwto!JlUOX z4ZlnR=UHTX(m73bJ`MY1G35^fepmv;GCh*P+1Gq7e{QeWW1q~jSlc~K9=z&1x!R5Sw+>Pg|4d7B*QMP1eov^xZ{boH28L>~l{Zhb#6){LZ zm@6K(L>wtG?EU!}!s|J9nZo{zoX5(&q`-6B*F~kgn1RWbz(S1+KjtWyba%eqCgn)S zFN@z*^(@_A3M$`75z6`%5E_kQVPm~8vUW(0-)Xpgi@MOw2BBVq0JDHsQp!G~mGIp` zt+g8jO^5Tn$1Dr}bH1cx#Vy}6vXegp0~5m=T_@{(WO?uY<)bNy2_cW;Z}e~9$_)RA z&R+=3N)8DzIPSQK_PD=ZR~&XP^$^GwPJke6@_WV`6v&1R5CO#>3rm*Q23Pe8MP7zS zN4FFUff1Z$-rRg+ zUOSJkl&izPHIn6>?4GZlL7(#}f01PSSIocavf%MkVJQEWW_pySvMFl!k06}^LD&vwY0rFM7Sdo{5 z!^6JOK>wyp&Wqx=>r``x(>dQU>14zTUKY_wnmZX{if`ugWtEtf5;FbfaR!RPMaJd=iiHkN)+{o zw=7_sEj@pbvWiNnS(}3oHAP5B$P(ALz1hl~T>aEY-XZhaZD8r8xs7nX)xEONDCjv; zOUTNmo&)Iqji8`liKzLOKAgKFJ{IZWuJ!WaoUryyAsQa}xqK??8(ZCEx7KBV%c+g} zKb}#TJ3DI#oj_gNy)Lv4->#&&w<8@3lV0qv{JbmRz6E=*>$kXAjlB;|IzAm_X;uCB zv75{DlZUCPX_L_PUzBOWE8nZ-zMA|iD#R`FU!}FRHDgRnO#Q#U-C^A-YHAzgg4T%^ zfT5UUrP9zgG!1;~y?UlKy%*}N2#Hw+^T|{6+|stQd7Vod9abu&XJ-I|3zTY{NPshIIhMP4(e)?%0Uv@SDj|pppr_Jb?SU%^oes09s*d=hLG% ze`5g7Pg|}#S=k6d+Fsg@%%xAxPEJlx7nk#=$7MiN?f*(I2$fs2BE8qna6zlDpw#AvWn%g%t!02^>Yj~m`Jc$>iH%XJ+e zZKMxoZxEm;V0$ox2?Us2H~)s3I>H<)HIzL9C2*B2%2% zuoz$qGG)~AC_jk0_89l)-w}Kjrep=sU*p1KA%bo<^)Jj!7x@I^8msO> z4>V)boi5cC6{?m%cYKi1zpQXO7Sj9MjcR~0ZwA;qj*h|~Zix)e+JeBwbFm_`UgUBj zgZ3CD_k(a;jH;dGQ*p3Bd%O`FzipXyFXu?vx-t zRRt(cFcEnGVV%fzDOH|TTM%e0EhS#yS{w_?&t<&+7NqhCIwh;+O2Q)sTsc~YfC3bU zq)x}416#3Ue7ioXI|D~PEaz~kUWr15fPZx;b=F9Tzwr0+<&|tGXq@ z-!p+0U!6Y(5C2L=UkFUuSvc=t6~zQS+H_VW)Q7oK9zW$`$z@cCY7+mxgHfQ2uQhZ-Bg6F)mmEz|j=0p4I}11k?as_b+mA%#j1I8iXEwQXk2s&yQGt+}5bJPGb zYs;@L`=D=*mVrBcjz(@?+%AG&0^6ZbXHPVwwQtYzgjukZEJ2bVC7?Lfli&RQ%4ql> zZ3Bjstq?yQBdHTP-^B*^9^)d5i0V_-08a5k6bqU|cZ>@XF)H;$15M;U%QGN@6}Z0a%$-dB3&%`n1N7*~lrtIJ9})K!Z&`SKbd?mA;INup)SGh_}|FuM4}K3U^9Fb8+f3I)=dOLQaPRBqE3YvwXIT1%t;9*xKkQh>Kn}+ z;Q<4<$i;f+^O`#E1n{wLZ`^)rpKs3y{DkQ8RNlH1vu<1qB`m4Ar|D>VgG6BuM+f>~Dm_|t=53>L+p^5?Ap9utF zppJ$KnZyZs@R1KmMUXW&i^`seS3FLP#h)k*_YGfPxP+Lf^-x z_sPQj+kVNDcrI(6;l2Q!R|cD$^7tp7bf7;!t@&f~h=LG73c#aHVNd&EsO|JDJ9OOx z4GDygnsW{dr3{vF5xpd#k62wqCZIlHJHXZfU5XlU_yV<>Rk0;H^GfkD(N zC8;^cd!U6lfJ^KX0@2Cm4 zd=8d)ev_63t*c7*w-JRN1{3v+Dme+gIiEr$x928QNy2pGAlduak_Nks{AB%r4d$fH zGhrW1qRbq8d%BIF&*L}E-fw%oD&cP&m%}Rqb%t5mz_v?@_2O7|JOgdKNWi}S@&06z zP{QC}7)~36DkZQSelp`cI3;!}dXmDmA1C4Sk2IJkXca2?Rq1CeU5pr=Z$Eb|MA79r zy57rE7PN*8KqGRYA39>(eT2c|(NGsfr{%mU54|>P>rf=dU$a zVl0M^67U>rcEgp*DN>+P_xfhO3&BA|ja^IR9nZ)3c|`h+cLeVW#xuq+3uv}nG%A6$ z#JM=Sf#pn?8f_ORN$Xqfs`b>*IN!zY+eSWe1i~INHf-G~8xDL(&1R?cv=sOgWv}k5 zvteF@Dw-i34yuxg_Ahk2WZ_t?;YIDiHl87$aT0m1go-04L)r0u>_1x^n=umxKkx@T zXKB)YVqzl*IYMPkmI~jo$xVn-{?=xiJ8`YE6AE&DmVLZJomVQhqu;I!nEhjZSW}A& z9aUJDnK_|~5?EL>2L>l;*|=qJIbzC{IPFmO%Mz2JRMpR3A?xzA_EP8?g@56F6FM$q zslHsKdNUki-|0=HpNsVPE(Hds%j;8TT#?q*MZ~(-<~C%b_A2|X}`fHdwvLeljj~n5@a2L|D4uGt&g#XQX&30uL+(&sRQi{`=)W|ELz*%Z{nK;cEHSxzI zsrA`<4Rz6Z8InRbWPpDMuP7)CHtM(&gT2jMVmcA6EvW4pd-}Qk^8M1f ztkSjGqJ~jOXiyi^+0T{nVA#S8`(S%RprmWLq)G}RWd2UQ3q_lH)oTEP+*uVvg!LsN zp4D^TqM1J4uq;@HeYBs!V3ZI;JIi2~d7$Bqe6W{zYP=5{*J{>)N}3r95vhNZ5{>PP zuI(%1IF+xoU4$(JU!{WLTR0_#*HMh#ml-ls=i#wjLt?A2l@z+gU?w4~g&W3HA^s`4 z4F}UB7IHlO-A+nWfeRdy@gjRdiHelQl{X=Mi%^&FKd0*<@l5nI+3Vc#h{rC$Kmoe- zvpZ3bMn*|-qZj5396y5q;CzC9kn7s`b9To2s502|i^nLOu-LIIk#*-sAS;X19xv|T z{0*ls}liCZmq|f?Z;8zxgGkM?Er0=gxe!}5B)Cfts%zx+KRG~>%xK$^Qwgd zxLTF&cv9bZ<-o^?!iTp33(@-}y0vl_)2I~G)HF}92jl3v=#<)O=E7Rmdy2oglf|Hk z9aQ(|WOUn)IJ?Ev)&Fx6bF6)kaLn)DLr)thofgM!&@~!rT^O7bt@e5Or7|3fA$%9Z zdcgKVh^j?@4Sr`l^A=M|1^Pj?Yjm{L@U2TTmvgF?=HwY>`I&p}76Dqk08?8TUT-A< znh@%m4Kr8Z2eAG|xkt@vbVWTWWo?7J*w`48u>8$6i$5I*42V{tPKX%OlI`)B z()!3ib)k>8#b55^e=w>k^6=B1ZbJ{JnSv5WmCH4hd6f6{<7;|@XRLCWiDm99yGhA{}Wd_r0D9El$(X5~ZD@6Yeq%JJ_WnN-%QoT_^& zls{`|J%9EnfAEk~Gyx};)G-ocS*iS3?;B@s#A40HNR)|D9E{SnkxmA^FH@JMsvova zX8&ayT=lAu;Am_z@Z2<%$ z6{BjEfS{w}t2VXwOJOAH%tlL96JNh1#=Wf*>*+C=wXQ*vNIs(oaU1*ElJg~AWBoz3 zYsEM6%XU&tu#eu|7g)PJi(C|&tXR}Xn&hnaxd#gx>+j>0Lv??-YpZnq{-}DCX85D* z_dAPe6Sn6vuwUIAo->ylZ}KI6nQVFL;mT@J6n=}jh-Pjuf%pEv++Pdkdj*Zwp7DBP zrhTtVw6{gZP)&6$66l-|HJ4w8%;eg~^EPs4nrqIew2C|SrN%S*vk{KJr_g;`Y{qKK za@>)WYnnH1$Y7g~H208STe0$q*DizuUr(^Zw@=B+o)Nq~aS$e4yGfT@>daH^DKIsz z)cwow6@MRvpi+T4eltCU8Nd3QQI$>-+ zFgW?Ai;?m(M6`3~3t0MUNoj|7dB1(@$eb{5jiAiUXxRcsX%N7syb8a-?5DbXQY&dt z%QtBuucYrm*1Dmz5?dn{A0>WU5x4YjETQ|M(8cAmSNvkb`2Ku7q`f}>zYyO<$8g@x zUzs*Gk#M%Gr7AVQ9ITO)uwuolOW^S4&Y0P4a>%SIVE2Gjj|O|=mQu{^v#Iu?g_u(5R=+V-wf&8^chyn4kgY80C{bYAldhLKCoo`h9f)w_=Um8!5STwbwF{_U3Tizp{AyHC84h8bs|GATle zy3F*$Rqlvr6MURu^<;ctoJ-C-Z#8oO_$tvow?$;EY1DOi-(^S-2TIzlGbRuwb&;-U z7yZw0dus6-cg1D}!{`%wA1C;jA;~pKJ=T-XVnJJKHRncn?@V?ZaEO-)De!(_{cb?PQZM`vF>be}<$+yB>x!n%iMG>Ka&r0;UkQJwE7Mk z$`Z}Z4DInTXYHvlPzHtdM7q)!O@~4(CDCwL6=&P!6H8`ZIu8IG){ebnFOOsgJi5#J zo*23$q%3OfclD3VJx10L$%S6mR#0O{Z-fm;yH{W~(%~1?#@9nDw|X-FV4WFNNE?u= zNsUpZB9>X39R6ko>B{E@palselu!fi@;2{*99+kw!vd7==a0k1U~mtD?VxZfC+5?F zS*Nm{8|Z%mvGF+lu?k#)zW_q@WO_h`!~-H-wk-C(55!&CS9;0~tCO{qo!|0Ld`A4R z!?{Ezel$sA7!Q>QFASC6a)j3Q9elimCP?JOd_Bc7+iW%D38BRl_}=XI4MruG!$~Y! z)#3G0Ing0FLqmnq)N|04xKy0c__kp$%3zYQ=jbnv1!pZF??**9r5!AA6QQz8HaBac z-)DvlmLg^;&QlPoDNzVPj2csI+Sp_GZ>Y~5tTw?OOsB`~G>Igi6#!z}vZxRn z_mZ3%^Wj5E=1-Gbo9g3YL(lz--KY}z4P9;Rev`lWf1j9yQ{VAp+Sf5=7Hy=={c`2yJadWY-BAQd=Lk{ z_R5BjK&)=KT9B)gLV{1>rb2!28ThgAIW#%qO5B*yF`N0*A1srvHd0v*yWIk#^?)Ts zM5jSs*i*xFz2D7|(i500ZVx1J5^<9kp}e$hGHAC2LutRtBH%ytYf07!h%N|p$O_p@ zM76*Ze2pGv`4>e77PC@W+j}V~VvZ$pJA7_u0J6SUt6#Bso7y)~k6x^LN|Y_7g&3t~ zc23kDyJ8C;xg3Hmb~#>n;^|83;U1180$1Uh7VPe5U0Yj3=xI`|HIDic);1 z%Zx)#tB~cm_w^{1mrymqhd`i6ceMi`P*4dL1i~7qi~&@l{y)Y0VJg~EDlzW{YoEjk QxT*u><#`b=z=h}h2b9~@i2wiq literal 0 HcmV?d00001 diff --git a/common/rtc_manager_rviz_plugin/images/select_panels.png b/common/rtc_manager_rviz_plugin/images/select_panels.png new file mode 100644 index 0000000000000000000000000000000000000000..a691602c42c3c688f636b57aa7449486df296094 GIT binary patch literal 67237 zcmYg&2RN2*`@fbHA(T;x%qSuKNBqVzjE}T;%A=&CrLb4@{j1*tF z=3KCh|7^FHRnQ>A|2)WyAL8GP4(G2rs9T#lINz`{A-QQ~ecR-?y^)=XiIu&XwZr7L zaw!rL77~SXG8!&1V?E9;8beV_>yvYDS{lrRMoL+Bouhqd`r*!@13tFo??adDO8AD0 zmAxh>qK_}%&Z#xcRfw74w0$n}M8v8}nc`ITu)!A6Es2V+Sa;3;lxtmGeIQuFO>^~o z3WId~Xn&5x$)TvB7_mXAMfE4?FJH1hVj;Un{Lwk%mBIUefA>BY%>Cb&m5$syCqum* ze{Rt1W#9epN6A9DlP(Kbo&Wb`naB&qG&}zN2%jup1Y3l+L?y>yUgn*Duk~@Bhm~~; z$%9H67V3ged4)y)d*P4rKYggAJ`_1R2S-Mdoe@1~YH1l@Kh!k+pkzSlLoF|6BJ(tDkPSPFv-#KFVATpP6om|MwYe&R$MyXP_XJVCHvp zoPU>>cSDHj+Ivez{mqrli?+4`ipfuliri!qPMkk~{$)yv5_hHLsgpOGERTecIXXHn zc}U?>7?_xDnIWUcVNzlCROf&Te>m#L1h_f}cLUrhx1ZETcDR#U&?$PiSs?Ss0?I$5$>#jjsqILbws0{f?3hKGmyo?W-9u*%Q1Xy?(^ z*8V#-c8)VtH?VzE)t}`uahH4#-0v7o&dQ2!X}MgYhmY(?M^7ITY;Zn7^_PRVxw$## z>C=zr<{ZYHPLGd|M>bB=?ce`v)>X`HE>Z(G>Ss`e!S8S_z4OUxIhLJ*c6N509;r10 ze;P&dTUuLvT8EEF{q&VfVwL)Nj+@TV*f`AKgiD@C^-j*`(f=x=pzl??1r05&!lg?b zOo3n9+b?m`9bpQz`=hkS({tU}#N_F-XQX$k4U=EJio_Kv$4MLwSJ%}1^pxXq_SIvb z2kpWXIV|#+VwL6Py3e1y|%D{GL6V^_68MSy^2j{`=QpqFe0c%a_4XQ3{%x`zs8BQE@sBcOF&tedx1! z(J`uU@nW?_-q+5~itE;;9HsfUI;_;x)nC8*u5#(p9%12|)2r*gm3?W2j8b$*F3=Hs zw(q{}#+vKGxchkE7$2l!6`r%>oW7-H^((^8i@uaEE%x{JF*1b|kXBj9TU=_FfR0LVxA^X3NK-$*TRUaq0O7@^K{aWt* z^z>tNyZz?8@Tw@)s+#C1vE1#2I?C9_97D2g7I|S25xFSRPoF-Cy1(Bau6W7LPSDoY zR?8?aljnaO%%*bMMM6ke__DhF{>F3EPA{=Xd-RC0#pPS^OSp;?7XB}qw1 zmYp-=Ugh!oc%K>Na{2G5%@U49> zQI9f_&w?z2xwWDn^16{2wQS=SO1Y&QYrbo>#-@F9uU5T2S+QHDqvO9HrRE!9qi27U z?icxpxhgn+V_U@F=xM3pybn`_3O;IW&lo=oe=6V5eWhEP;x)qj_pTApW_PUEW zPiC|j#h?Z4NJe*7?7ym7 z^(y{G_3AA)=Iu7x{iiejY<%I2;jsL9~XEC2om2dLq6M@a9$7P3i zi87|SGv4euoE4qk=TO_u+5hP92aUDY7fByyrexq97q0x@wH6y|XDk%$rXinP+r036 z_SgB|N%5KMKd9Co23ybZd>g4uZF^p;v#}Zc{%^nM@l|E(MBC9&t!&Nz5W#@)&8C<_ zt9w-uDx9kAH)3W=>D2EOUzGo>GNn7Ht$Xd#S?%{4*Z(rAt1NHaqo9v}F5(b=i-Uj| zA8TSQjg5a7kjG?bT2_U8Z(OU_4D$ol_o!!W{^-2CVYM7sy76CW z>i_O2+DbCCv(W6$QgVC90jV?Fm+FpRmMT6bIOyKh^!cx|-r(+C#LrG;JLzus-*o@g zzooyiTB}g7<4aNDz^FvH{zRPHfqy^3qO%|sdWDQ zjs#URZId%dWh*?l zZ{I$g&hl@^S-I2=d;ImI*L#tg`oKo?Sm}G4K3UVo=hff8GnAH=k^o2W@bG+YYBHVf zujUS))Ln~CeGxp>+_*$y9> zH)&~|h4x%QQY(KSXfLb}s$M=^O&iG|A)SBgPAaoP*hcTr?DVGt*1@5nl`&%Ws%fdr zYc#Dte>VLrb+4_fJHpF5dNw2Xzb5|7e1|`O+0u~&)raVLN2!52&U1zexG#)R(b*id zj*V&X_?^N{+UU-I{qp4synup=3YU}=bAHz)b#>Z4o5;cXi1JBs-CLxW!sM;)=}*Km zu)i-3z2h96^f+RJq@FS~f%qYzvh0QQ}N`|9*dYZ;%e(O-oCPLY*f) zR$To2RD~WAF5|<)2^krCQd|9%Q`xw_jl$_XY z@5)!dcCGfy7iz3a)XAGURz2d17cUMUjy~vX%&D_+!4w|@mwfv4f!xjjOrBu}yM?i9 zdTzrUn0>*u##PnT(}VopQBR-J2r=#9FPYkk(gfJ*a}dY)Gd4Ay@#FW{Qrz>TvhNqb zUEf$|F0M5}GwoWA@vi)CJMyDPkJ64{D7h{VMu<6%(A_;lQmE7Pl;h&}w>K(RcheYk zC3>3mpBgz_>=G<0C@5=U!qN5qwvmeqLzI%7q2ZDE)$SqgW5;6VmTz~x&-Pq(GHr_S zZyu_gTr?7lT3Q>DR?*b_7IruJUjZJbYGp3yw-Nn2F_Ds+8yXkK+-Vb6Cpk21(sHU8Juc!9o%07dZ#Pe%QHPU}tnX06k?Y3{8ZaCR&(=54Y zkf+UW?f28@y2iJ_md;Lvn>V?7dU|Mc3b>SG46rIMQ&VRj>#yCj_FnBemvq@{HoBLZ zYYY<+8h@MnLUn2@|)yu;w-io%i*C_xHSe_wLucTk>FiU0vj9XFi_rM^#nla&mLcEi8^ulTFp}X1#S9 z*G5URdVaG!)!Nh3`1$i^UpH(B3es)!^Yac44%Czn6ciQRdaqSIb{$TRxSDTuX!Kjg zz}=n9FW$USbelv`i5{f1DqnXpXKDTvc%U=eXlI3iP2CJ$wF0H2$`k(G@)YrXr|}=c z<1NGg`g?a|N>BIN(m;^?P^>e};ltRsHO7K_{gotB^_9H6rRAR-yn(lQrNLw1F%r3h zeE%;SQEr8cK1Q?==EV(MUyfhP@s&U98HNRN5RaXc)XRzhmYyPR0D%71yCeYD`0%^7 z1nXtVr?qb#xHN(mo7Hhqh$%TO?eUv8JbLcq!mo$h2d3Xz^%N;YX+$@E3i6s`^I9L( zpKXob2pb!-WOg4DuqqxuMMFcwXPwNKeY|YBJ&^AEDUyN4<;^*j&Fuj}@14YB$GY-^ za&I;(ddelWaHG3kV3!pVFFdNB+ParsDTk0tWFfGSe$wym-I^GjY3r6 zRqyKM*`a}Xx6TsRlUd~(;+kpUUDoBEKSu2nrf&aL$n} zg*tre(M_K_uB2&RzJC28t`$usP_7ar3(e`Xan57P4=%+Mb{4|SU#d5%m)5;N&#;TH zW@!Gl6SFQ~U*M28)W49dW=qTsv=k;Lrr`QP6vB@mKmLx7$4=>nT=31#7N9&VMFB<` zQZ~D8`kU|lo~Hx+8wcq2?b}OF57u^?%un{OgE**gUS3}4is~mJAt9SzAGUzi|NZ;- zSEER;@nBte>+=D%|EOEXzQFBu_4UJ(lc{LY8Q1e^{Lj6;dJItgd1B%blDZtW&_9YE z6E6b%3I}F)lZy2PX;pOBEff12Y}aN80Rwv_6rI60ld+he9-VHp@1S!w*i zKx;pLW?^I77G=41*}nIupjbkolT%pvb+6$!?HwJoWZOSCHpU8_VP~Ka6cxQ-Z=c)N z@x@qB0HXo(n$+j+k007#@=;My;K1EhBD%V|VDU#Eg2yFiWL$bKj~@;J`Nilsbm$Pa zgu0&IlaA5Bx#3pP*nv>#Pbt6AYl_D1j4tjpwmf}OOntS}b?93DT#tXfPX!B?N?es` zE?&T2Ho*XL();(PNJ&XS8}V*mzkVfPmYLbZi3zKmoE+RdiS@Zwa6DpL9UZ`sl)Z5y zA(HE#Tq5-+Wgokqd2cs`V^8R|YW|ETIvvf{Wi&rCeSm99jz3aoTdR>gJmD;f*T5o9 zwP$&S2LuFA<=gy&Hd0^tI=jbgM3?Y&;X!xY@f&>~{SGT~Fkhq8-yHbeQR9$9ue(sWCix${=VN4-JF@rS zlh@lqsqAwG4|ciC&S+dpFYo3ZvUCpP&vF&%V{~8Zu2yokshj^jEGft{l4|u+Sd6OQ z{+<2nJU8j`8Z=}ew|F7J|JY<-k(t$#bMe2Hl<^7P+mKa-cvvI(Oc8|L09~K z2I;*0yX$jj$15WhpZ(TT{jC~&bqmv<4Tt_elHygqSwv2=#VeQV^EX;`6_P_nUp^#y zh{YEwoPRpJ5|^25eXQ>BPM?G3kA5xaujD#6X-zE)ze!UU_z!a){p=yt>&VEyHD>tX zRKri|jBS&;)z^jvt&U!KE2_VIm$Pr)9g`@sF2JO&^(cD~;IKIX5?VLP*HBesD!~NHHc;DkyHC z^k^F;hu5#1uYsXM+V4gs&BAF{Wfxo>md*|&1@9~&UF)OT_&oa5cT3$<_X}U$IZIR9 zEd-bXzmG2{G(Y@DJ!J}*_;@djO-z{D*iie)-h~hvx9EP{#b-BVOwhouWfu?DYT>k+ z!7n_O-C+X;GWW^T_#9fAYX|KxRqYC6peSP=W*Q#QMG-eel~yL2gP=UKq@sesvMzamHW#* z0euc4*Dxhxl{)VyVkkJ8`*v0R{Ug z3XFvo(ioh085cZ6uc^lJ$UYalP$Z`GOjzkThY&0G+KZ2UCtA$rigthg_eqyrmVU9F z$TV_u^krNiqjrB6tL2tuq~XssHSw_jDvPTC<2N$#c9%ObH#YuC?^pe&t*X+1z+tGKsA>t)VIcXB`3 z?LIP?5ln%9w6)}^tr)Ou&`vQfnF8fT;*XQvvq^fo!cIv=1x@^dib~Ds{MVkIOVGCa zrjq01DF(KfUb(WbckPde00RZ;!ux}1E^CCoK3kylo0Kjt0UG}k5)w*)vFO`?mbQC~ z@8T-@rd_@aJ8*|9f|S}79!zTa3@S^4+3zC3j0F*oePSc3#6o>0SuH`8durK(ozrVd zOkSE&PF}vV*qQ(A*|Y3GG61r3ZtHoO&sfMJsCUTbP|L{vP5J>PDCpqP&`3z%I(%GA zfrdg%NBQ{T=K3D*;6-mY#@t>5pQ#v_o&s^XI~4ED>{DSgXt5z+E4HQo*Dn_NAm+iT zzDjluj%>UGaP0XPElVm+NI1* z`JM3v<4&ZcD`Kww+oAe@rwM)a%H=_XzSr`P%+K;?gqS#J{8>V4x%v51%gW*i6vuR) zlUC&-jqm8CFmBe51yf(n?ObnRwKCadQ8i%A|A3Aih;P@m%3)(_Bityr(m0@Nj6I9I zmCB6dsW^z%ow+x$j4`QkZjU~FlGF2;J}))@S|ubj)IKAPOJ~c0QVchx_2fxf_%op0$;rtP`;L7o zi8t|Gv^Frf6MNd(Fu$vC{CyW779Abk6v27GccwnG1wo`fD)>kwa@$Ty>MUt_t8a$E z9CsTwI-HGJ9+&^JuHWHZBiQOSLw?fbFY9c3#+>{YDB|ty?J?5cacylH5JAp6JBxU4 z%*I1=PcA69`tCW|N5_wkgU;r(N~C$j(h3ESs`QC%Y&$oZzj%H(&A3x>o2B6i2C(Qz zCsz7X)py8fve<)aD)Yc1F}!)0Z!u zpD3ACo^z8Fj0*Vk$Mjpq^%{`MtoaeL!)IVe&{3XbUaS4I$?Fzm?l>E#eoyp> zR&?FN7IJxJoB7)HB>|eSXN!DiBA*J`z;Hng0h543Y4C&#Ma>RAtsj(_uA+tTKSi*0 zm{{2AH6!h5XaObj->(1u{d)>bNagCpf!-xjl!O7Gcf=SqGud{R*+!9T#ZJf31Kup` z8W|qGVQjqpakZ?XV%?SHrts8X&GBqI_)!aB4WA^H_V$9>7LgZr)r|G5^qVciT>vKT$WYmbeIv zc4Tp#JURO&J*#;rsi^2VK(QUX2Do472HmzbhX9V5!WHL^Zi$rkmU7G6W850=cZMeL z*J2@zhPb!hhs5lBk6(ZPrMFi_^x!9$S2rq2t%`DnO2$}-NtKTV-viv;DkwWDmrj|b#Y*Ebr-1*^dAF=EZCg@v$(&4Cr|nZ1;uML zfm&=%QEig2n9SPS+q?d{yE9VE{?8TN)Yk`b1tLFPd@?inNS;1@N~~1neUwwo$RDEBCbL6PMNX$DI;!d7G7oH$H$sT+KGz0V{GlnqXE({X|dkGHzZi zTad)Z@%&z?!7_HG{TzyZYogKntnC$TpLlgr$Gv)d?~J(qjJ0uD5d&k+t|2k};^ECD zdA!FJ+2xXWb`DBS79TU^ADTzL&a-5c~~=|5f;_MV=j`iMJN*Z;nPl@*(yf`WCP_ z3BK&w@>$`uA`=S6S?6KR>^#$M##E>hkqU+O}<*#+565v&*m2(n9%kyqD|s&CJXQf|B3m zdF2~F-R^ArA!W!VXSKDN;+Gq4s&dnz^hZWV3zz>yXDLdi(73NO&I?+Uqhv&TT>ms2 z+j6aY@c6OT;HRg2+S6*hN56kwieDeQc}%w`ESy(MvCC7OKJD(^E!x+wM+x8hwt8(J z30%Y1>FHG#d3v)ffKs4&cRNO_4YR-e`0=RG6lTv^Cb784$P2o|@ zine|0-CKn}I(iBx)bujkedy&kWN*vRSF~|oJ|9M*PZPh;)3m#Nt58m@dIcnDz&2j3 zOm-=$I1DtjV@!^8{qnfJz8e-67A}~@aALnw@Rct&TDxE?UWMnbI9S}OBoA6cxl>qJ zcn!K>jll_YRn8ZL7&v0gM*`3bY6A~)UB zd*?DVd1E*G?)~|5^$*{(^Fq;$5fW#5T?Y0v#Y;DrxXxZsR(^Jm62hdK8V!0ERG*n4 z>CLGAenUHZdqOVzXVz)jjC~i_z$NIvThr3_+y`}AR(7k8kB^xU(|&$EB}K*E#ZKe> z-G%mR8;j-gsjX;q&cV;0E1o^Ojld|-KH*Z!$ZV;P;E$MKg69oI1nR{8V^_~yyhx6k z2WmrCCge8q{l{2f;Lk1-n-;8yMA-kQrZeyPTO=gwM?1mZyM>4#r zNqJ$iR|Rt$8x{iCDa-FH!gp&>)3$SeFOy6ZPfD48xit1f{^@|IleCG-@*|Dm22W@IjzKO0UAdJA8~#UfP-Dk?&0A8t)WasHim zn<(G(!aU#%{|SHuQ5#Ap#x?F`A;kF$7cQ`%u{claDaT6%c8qfH@jby9Eh;K<83+_P z!4x<*KQCbYQ{vgNYcpHke;K@GW)}QM_TZsI)uy?9Hlq04oWjE4HlhKjVFrr+f9VTW zF)>C|CVUk1Wke+qY*=1irmfS%-83;Vp|$z~e*hx^{(zczie`p(bpYc@5^PozU}6di z3TppzL_rM>4#uGIe*30LNRtL@oSOt@eercH*T;3eVobAw1-mmqpOSY z&><7xjc~>Dm?);XQI8(&#@(9z(`W=~`NPr<^NvhiLSdd=zHsrPQD=^c+O3*Nuf@~P zj$fBkQK156!VS#DJq8fo^`H_~jnDbV`$NLR9XXuu?gE=gP`#6Q&sh6M65T%blR|G; z_-iNx=})EIH{g~%!NB_SrEKPuTJVA9hi4V01EyteSdYJn68e1fn`GbjY$Ic1V**)V z&Tw#We68+;@{foC(}4ppblADLE^fKgI!stcF;bq-aWm9j8;}3U1IC4(-C6D}4WtS< zsl-W3PS5wUpdeDxeZk;V^fe59h~$81(6m0nQJcb06?fbXc!(DCWD=;vY3%#m`oXOR zF!a=D{<^7}yh?5Q;+N}8mn?TtEhe_Oq`vTMDNU&1Nc7DN&H76sJ-#hwRR$*v^1C1| z4q>pOTQWx}-PcSrnICEYup}&U`gHW+wO>04u#%phuF&!2yCAP-It#2fpNbFVo2%um z?d|8e>H49PX=rK^!W+;jNuIV0x7OE!~?fa{C0dsKfcz>%S zlLHZhIssB3DPY}QHEy6e3=}gn_57_cG1zG;%!KT^Zs)_%AkrQF823Z7peL1y=O2qY zjWJM3ubog#p6SVqaVy2;fFy9ZUZ*Mj;1L1Lo?!scUTW|;n?9J0AoJRP}6{GWt zNBhz6fWD`fJq}8^1RK>nN$40wdIAm7LHrN|(C_0;;HcOiN%{F&MYGZvq^7xy#XZVc z9b}0zG#^(Od@vTgk$n|}mR|P(FY7_d)-PXJbYHr=yW>?Ol{m2?+&nx-PM!?y?$&wq z_;GUVJQ_dZNN60fv9Uz#g}cKr`^2qp#|R8pGtl=rgbjs<_V)5u>YF!2q3op~Rk&~g zQh1P-p6hvwpFiK3(_);!0wU(pYxPu!*2!>(0k#Ovf~*R5QYe>--O|*JHLp!yH+0uN zyF$<&VsAn|PoB7V@nZ6}O2h2nn3w<{jD&={l@F*OzP?<4iT8v6gLNewfX$6{tVVzw z8x|pSAp?dappZ|k@eyj&Z0fsZ;mYS0Nth%y5C*v5jZdR%=EmypzA zAjBjPr38qn7b@k|*w~1c+UHaZ?H7(UIew_eNz`vdPyWCBm{yPQjkV@+Q_<9n+YYVy?(2m0ak^N)QK?vPLQ%vRjUK)i9Aek? z`3^DL{_Urt_n_gXT#9}v=HiSXSls%P$81NB`agL>NqI=T3b8;>PtTH#mC?C{g{Q!i z9v&qc5#MdS5D*v^E&uV`j`94m?dL(koRbuP9e00KcevVMVX3)C!C$+7pv^dC_>0-^ z-=@0a+oGeRAERngQtnq&5CMXtJUq{XV~8$){P=MsA~E>jArAbi@WCzwSH(mM+;_^(H@#_b?S8FuK4+$Xw){=I#qjs= zT4R7b%#~Box9ghI0Dm4ou5E5k#U!7Ym{?5LA0xdExY5>D1#bFxWaP-vqq_hT!S2m5 zY0=8oW}A~*%z*wl1q3d;y;6YX1f7UON=on1tvK%uSGde!86_~EP^M9-@m|k~|I_Qa z+yyH{meOT)Jl`Lc#lf)yg{Pe%m)d&c1OvuA`b+=70Hz82J(UqF?s)|`jUQaRMUXOR z9cC90m`-eL%&v6_lw)%%^^)ihtH;Y2)js8bEsynp>JhGN1%(rdJ@Z92B?W~W_#Pk^ z_Gl&ch09!v{IvobR`4!>AFO6b{ovS-yvMLF!N=hkBBDZcklIi#;JpyR7{ajU3{`mE z^l@mmZ*i4KPh@9@0E82H6QH;Arly=!5>q7Nv6SV#{5!6qYQKI}8|E8OES?epO8-4L zXgivbPm~sx2>CYvA*AW%Dr-2EG}GF#1^4=&Bj7quxMERp$y*)Z?Z7)yhy`4KZ+T;V zWu6Gnd~mUG18@hR43b=6KqELPYIBEJP)x@C$F-{#%N8ZUoe*RkfKV6NXo|ZDv)Qn> z%oFPgDFQ5&f|7Cu$rt25PbGNEryu2aiV$z#pfCQ6#)}sQ4EiuSp99Ki;L+PqTiX{R z&s>37Lm1V~ag8hUqb+7aYaQO}^nh*1)%p_66o5r)HmVs8)@5$Gv7X|H2h47S5z9_< z13qcD>rgD*K%^$1SbkOa$6tso@3PeQAj|FWkz_T0wPmT*n{{E4$B%}Ev0t!X=iuU+ z{c@Jd7T_7QM$~=b7^c=S{W3xjMIC~LzIgd^JHupg4&nyD#bvwBi-?F|fhJGD3u?yS02QgvL2-wNu&R-U2eNm_cTm>-zB~jB4hfoj^5)hW)-$ zbppHnwn9}xvsF%|1rP+_#uf_Cam6(zUyR5>>KJo?Q#i|WAvc~FW)KbuEePDDmN%SapI*IXLv;|{XxEk#$ayjxsYu>1XaFUC)Etn<%Wyc{4rkeiHE;M6=9-tV(T;6~~uBN9)84xcInFISR!6E4sbW~68v0lG< zCViD1K&jnOSbQsQfN0hsby5M==!yvoF|eoDSzT52jJf%+aFmkNxk#z|E|FlPJ53+@{!WP*=V*Z+YoU3W6yyO@SdXa{xNA z8r|NT>mOW1Qph`Wf7JU_Oy8uhAs^7rC01=Sa&e$KN^a;@SW$0}$ zud=X}m?s$YKzIOnsHm-XKb(&k9~t>L?9gvlXm;xs8;yT(M1;JXThY60?C<21xSv1u zV9qyy%fVTIV}W^!Zi9OUqxpM*ZBpCNwDn|F*1kOLMp_jK8NfC4Krk$@1oPXs2a!;Q z{Qy}26BOzXLA3~IR($DB@kG(Bx?@&j`R>!z^yvPk*xE4?u4*fd_*D!e7Jvr;D3iP3 zYTU<LkMaIQo&G00U;BZQ{vEXGjwIjmEtN6sB|G>wlr63(19}ln7M>+=m zH=>@yB+XI$5Mt-xYCqjZj{`+81(L=$%Y$1deCd@UU#q)iX<21yX=+|L?|JOFe3Gz1 zUYWZ_Q}!cM!DU}lb0~=9*c%(ZN zdcC3F@bL0tTK<>s(95EKP-)wK@0_3P6UF4ds^Nu2-TR8kQ90K7nwrnTWaeEYh}1q} z6WcC`Xl66=qV+<+Hokc?sbiFpk?{r;VbrF1FK~MG=)9o&f+fbT3ZyZB)=G;!LELgc zrr=sm%3lvnwi#t{u(Jn0d&VhQ@fs;MGoiX)zh2O}5O0Bez%9ENw<+6aoIjr){=K99 zlyTFE!3o1S)oyu3nIrz%yr$!G*58bV?6IGbKv4p#mWsEoYyOmZ(XP-#O@g|MI1_-0 zju4`l875SSswUn1V`mM+y{aYVOJN9r~I zLxdcW)kHslM$H@~KjJKgRI0+E5B%R4C856fk<$}|H5cRL*#3Io@x$b8HJ?5Kq5&d7 zz_PU+SeGIKW?snAg+xX^tQlaHl+?8tsY7`5+jZ-~Khf4Os4p^xluLd$q_4V~8{Z=( zwWyCI3_vEbu*3!hzaSR1wzgIR4j6Qli>qgk@$REt-7McD3bOk6yZN-OJLIKiLL1!#-#;p{*H%u|P?G5d2Kgi{3_hIBx2g)UTy~{qfH^6$6-)s6YA^3rI zAeJc$O%q0#ny%4ML81MS9X8+Y-Mb<8pyCi5-L-wIT1vB3{w$J6g5u(eT3Q{x$ILA? z1HP|RS z*64i2`ZGrR4!`pn9p#%XjrfbY(?vyq;1zp*{W1^P#odq$@Up{Sle?XJJtLJ;$`^tT z_EMo;x{l{{dmdVLwrL0!-_|fe#Rc6_tZ*Ps?hHV8zHfmVP+5 zvX6zv|3Zc;B$*5B9|^Q!t~z#+9}whUU={R106j-)v9K9gHZBw1K)Hc;I~}VRlD#*E zyi;IJ18e1&w{jW_MmY?4y>^i+Dynj#gLnd$ZFF8atvpscdN5atrP3*Fshd1erR;Q@}}fG7WpQ3}1L zI*Y-9$QG9Rmedu(N=EnjYF34th7AaAh4F}DdrZ&o)qHm!6tsKywwA55XQ^GiDte>% zb-M*LZJhcuU7YA4XvoT(`ER?zIkFnafz!G1>j52l?twEib|5|>wZO&JI_i1OU(rmziWD}%J`v?4&H*O2TVPP3A)b_wXh5a@)QUfo z+o^<*8}5t|-uElXO9&*InL1*_vmqDG)jfBYUIwtuQay;dsfaP7pGLs|1tv==vC(-Xmz418 z)kF9@mqb$#3V>NBi*!bdnJ7FD=;jc@aL^FBx!~YC;2?lkcsujA^Z`M34x$G!Siq`a zPV52JKf`i@6(*sO2aL8qI0C@|c>)~e1$g?{SeVsYKpNaz@S39HVtl>9B#VnBRy~MW z@=>QHC%{)ky`OeQ+21pCvC1NSqSh7`MI$0c{{BwT+bek0oGj0TD5pjX-PW*t@dx)3 zy$yQog=W*{&!2-$3m=-4P(hE0-RAD*_HxT}d<;YQ2e@bE7O!=SofPc7zjbuz&lP2~ zTR_r)29pc#Z$F>TLwtTv5=2-K8_)dg6!nOlYSpA>Im(W9$gH~mtNpExts9c#hfi?N zw^BwLqFxEVjZmc!+k-G4m6F1##2MNq4BbXeU!T8+V)92Gd1|Y|wQHRwRB<@J@pNVP ztvv0cN%=6a(Wx=u&_hwtFO7PQ1r>bq@gtD$f(0AsqjxurJK_9=GR(je%tP>dF6!zU zUaNynxMe_OE7CKFy+42cJY{Sr-@HOVW5ly(&+gX`CKeY*Lpgn{N5VhmoTQn?)p%ge ztLphYpvwNoCbGdOZou2%=czLNyw)NZ-W_3{kW9{{9KDcoTk6d15o;d%Bpa1|36x$p}{V!fMcrm9lFlh=w}=A z`kPmZa$O;e97PC3l|NanR@b@bJUTId=^PD+Q9Lj&bh#;HsvNNK~AMc_ACc{Mu) z=PW=F?9I&0`&N55r4V1mn;`$|z^6VxN2OY8>4!!d3|CV&X6Y-uo#q60v7^j8E3?~&Gjwm?I==YxNBm;f;QROQ^B3QLyOlO? zaZC5ZtsTy1s5|LpSQ*HC-`->+S1=V^U0L;M3Sl^NhGlzibB)h~$S8ZMcrmVqlA-l$ zKjmQWb(VRa2G5{4B(4$5Gp&Gm6cclBUUKI5kdF8IJOjmjz0DM6k13g_p(;wOOSLLq zAJtVPtG^~+^shE-h!G1VI-EZ(7xHY4MnRHtdV0(NW)@RlaIy(RnI1$EyB;lD5N3hY zEDidSNVm;4IHANq>3__>mFvC?l-P@CoJ@iD5wW&`hJaI_TDp$m;v;nW0SwJN(Mp_f z{MxkZ7S;{9i)*>2A<@xk%0Hx+S3CxJ_4|fX<4s|CVb%eZ zpS@2ZHn>)M7eZjj1$La}#<}84J-cfs7t_ckf#8UJMwBi5C7eour1^j4d?3UMF|aSph#{n5Q^DdORyYD*V7YxEtm za-{8iyEBE#`bz!z2Yb_{yfVwFrunO|^NBMlC4fs94N#~*tuOWl;edkWsoC{OZ~am& zm=}L=;=^F>Z;R*8$$_M;z*i$BzeBHRXh3Qq6z*$OC`md3AuPCRg%i4xHYRkrDK^8#BLa!LJFq z>uG2xf9KW%w&s$^Okr^25JV?7C*)im`$plCsgL9z{HX?^B5y(T44x5TDPk_*lmYw? zsx#0UCXq$+E(@sF19B>Mp8Ny79r#`y-o&GQyg0!B^4+_z#Kc6es1N*AL)Es)(cas& zrZk&*yfO=ecKI8v6km)#U=i$cN|}jogD5LkSJ{W;{EA_!$COU&{}`}0_RrS| zg;m!SyHioU>)GyWW`vhHMgf+14MQ@pymdPt*;tIV3v_uF?o z3oX0Ys=`?|nDcRlqsP~|=Skh~t}CxU~| zKy1^lyh6NbznU7vF7`vXD7>CqNPFYIYe}T)fnd8*O!epGiU;%9vSo_C)5 zRk1PQgSVJ909$Nso}9EIW6e|*Pi^h&^~4X^B3$~)@;s++fmxF|`9ZwTP!=E-RGW^` zD6-cFHve0_9?qxe)po*(7JJ zs(!gm6rdN1{gOx7tT{#Vee^W=ZXRHj)Z)^sb|)Yph3JEUco=$rJN7q0~UFPxt(nb^!$3rP#8rD znbSBGub1e5@2h-(A+-kp1#?hr$defbr)roTxiaLz{mAaFJG(D+YAUHeQ=|Wx6U-Dm zZimEum`mgIq&vKiT#%Ulp1RW?QLf*}Gr~th7#pS&%J}xesbJQ##E~o5ApLl!6r`SZ9`>My-5?`#mnlpx=`tb#IE{PtSc<9m9@1^uS1IC~ zQ>FO#KL&+q>#)gLind^(R@I{8d2~-NYqI@GA~%NAe5An%vO9WNTr~cmP!Mjp_+pfu zj~INruzZWPs&hF#s;d493$^IbaZg|B2x;M>){T=N)K2<~A95?DjO9xq)x)J8Vv1!R zd^y(NNg+~MQ`yNG>JOBH&aa@P^joBSc@L^J;Go!3X@utk4v0K~6G!{Fnuu8tC%n?q z$f1cMu!y6{RA9qvV>hLfN=jlN+v1Qaqm+jzq|lcyUc6B0!r?^1B7>VioS*_0zJI{# zMS6Mwl1e|lH$5Tj>96(EZ$Yq`i0#;jHXP6ImIWh23gks$AtSK&ONGz^IKw~(MQ8F+ zOUcD*xeo~-izuj@xAR#t+qt15aB-3E^tA@aR&@1F7NH(}@~7mlbT zwWwab`gxB?_nj6qW8{A?mwHxCG9lR z+;l2*NnyIL2ak1u^xc-wU7*VC+cnNfO@^bmIJ|DMG}SjU0ErzyQPAuQ3&aD^1{P{E zBnuOyZZihTr6@%a#0IHMrC4#87m5fAApwA38{{QCA_ubX!g$xtGx!XoK3m|fp}XZ` z#R~!dV>(9n%X&z~N_2aac@uqhIL~G<)SgCVyKlB0W98DveS`6L6NxZ`*ycWG6m3>V zTGI}RYslcC=l##f1xh)}%d7q%6aWLCRd*1V9h;PFxE9 z!XRL{oe&0auX6BQqx)2cn{ba9PCFe#!^L6)&Z6=31BW6z>yJC7uC49%b4w0&|Lm}0 z16p3Dk_D%LRJX?)CByh*{nDFh|5TBo(&J}wlo}aWxujkj68F^@~I00@%)YsE$u%pDKMRo+(3wgz!)tG4N;{ zY&RHbdp&Zn4qN0T4x(}@Mc$E1>chdrmzkLpC3^X|K!u!#vJdb6o+H@7B|msR zmCcE{;>NN4>f25u<_~j#NV?-FC`17$pYD>q!w!k9!wiycLOA*2IQos?h|pQw$`CLN zMi8UI;1{9@IA91u>K4oOFWqyeW7OoM?=BEw_3PKWcq0R>Yl%IJSl6RORSXv>A%k{s zONf<*uF3uABNiC#Npy&u&v(fq)IfzyK=m&Pp5L>&FgY~bo~B|TM168d{m$BngW=#s>l z5*jU0a@I@<@jEyrjqSNpHz*>!=p-N_Kzv#QZ!DA1E&!69Fsbr+cj<~_at4y z=q$o6D3)zFEe*E-T@ex@L|>@YS8|_XOA}f>w5V!BnY4C1tLQbUEzsZ<@~+W&H~bhP z8VD#{*3oG|NDmSZLOyC58t33G<9rN)s!%^{#A9*SuwSDjTm>OeTXaDR1^+?4A=YNm zdx?|QtyJ%L+MtangRli1euDlFA3%c3^kPP{xPyn(X+<*Ds^u>g{vRy63v5TID*H|V zKR~&_Gd&tml=xTy3szX6q>AJ)9QQ|9ojYiYi^M4blW#_()#sXyn06-o7gQY+yfgZUP%qJkdhT?xe0|1Xn&T)ZG>!m6;yG$Z= zf8t2Q?N#SP6Nbz0O-T-FZtE?`oV?%p@BI=tb)K?JUv;N1ezIVbep<;i@yR!3s>u$= z6GD}B3o8Q++*u}*xV`mb8`q18 zqrO%vw>5#=by$JMf@2#7aBdS_J!aQ2OtrsK*OLK zMmiJ!2`7ABy~G#~O%pjc$G(R~szrVq@1&J1?a=+ZetqT{@j=NyvcF%SKlPNg$@9Jv z?rE^H7jW?)!waYNQ==!q%1_|JLd2QtR@06-)GI_U3#`2W{T=WcKujPba?7TPNJ!Jk z>ABI+FAV~fLNopbh9?Wq6F7j-V}wUVIc_uwAZspH&*K*EJFM6(T%iL|8=%hml0iAlbZY z#l^)0G8SRk2`CBHITR%+{;Rk&hm8Fwj#oT7)W-N^foUK93qM)akKcZOap%E<+ants zPgPKot;Fg)PZa>WKj^*~T-ng6xgESH5f536Im8-xiAnmDp`snt%fGlH7$#}8Kehg) zW1yLT#Cp+O`FV2kh68gy9|bKwqjEU;-X?pd(0#yUq=M~YXtP3Z=|Qe7UMmYfh#P`n zYdFn+Ul`}vbPo!Qc6+45?Em2&2}egXx}nIpxOxcaXaB`kJYFA&>H&`?hWyzfp31(4 z$hkNB%d1fI4*mY!1NrC$Xr;j2p{mS!^yue(Kd#@*Rw?Qzm_W5b)9Zt$-EMs2`}H-% zcM1-gs5PNO5)u&su(ygf{+wBWP7o(0ae){bvJqn&G(FH95QQ}C9woRjWB>>UvFz!A zj0sr_PqA!a{$EMNK8LUwr|0DoWOl$0dl4=W^a1qXZ94yEynKl>wO-rvA0x6={L%zT zdKED%;j+V3f*`-Mt2qcmS@r zHcJy^gDejxuXghwDHZC<@B&1Pp83O2QAjLp@P#1UrFLJl{1L2Tc%;C8LPG{$Pk(=` z=|$+2$wx+&a9@yyQ9$D-(l7QAoO_GcEG2Qn*-6|F`GI)rB-ha!^)Lz};wy7T1QMI910S*cA)5(rWgOB0duV?1 z@d^ogh!1^H!kH>~uNxl)c z3=45PU|8Y7edf2^#yf}C{89MbH>ZD0=A-NH9X;A8`gQ#jd)NMjLa+%qqR`&G0}v6K zgFf-Ndj|3_6ek)f^6`ir&CpKO!rJOAwmt3Ffkt^{K5qpEvBn<=sCxOO?tM&zZ(5=6XhlyS6^s=nEm(K6DDqq?~`-y-5O6Gs(w4LuG zxHMWhNQkuA)}+3J@ed+>oL!`!QvsEVikbYH(0W2`f{{4h-rILZB?l2qCBC&p3&sXIDSQ?eXnIB~XpyIL$4 zdlgp?SfoM-PC&0Zy_F3>H;x_x%1Tg~0P7*`fVdkaQ<@+Rs#&yu@L!^yj*hNH9fOSv zS>os1vyGtOYk;Vr`^7^7k_C#v@=u{jUY&Ij@MN5wLyUqij76UrJ(2s-40S%-kqBmD z34*N-wnm&F?$%+fG(JQVw`b4h#P=EHF*sZyHLx!=#F>q+n&eJja>omu#kK=cGO7Uk@X)2G5zicI-KRsQYm>g6kk=blj5$ zBA*9jkh<19aI)~aX9YJhD$y2QEpCMYQ;Zd78q`)NnAWX|Tq@6rJ$0G5NY+0p7Ggfb zNqeKq#cwl*w7Yd`Z)Y1&GU8W)p6Rgy3ylvd-3_4mzQ2Hv1RAc5Nd|X%oL<821?=sa z(O@klUq1B3b-*kYxudddqCD;mMIAHh+>Td89D-pm3MJNoKcQj$e_~U&j)P zV9r04+`80v7S=muLRJ>9=%oPM1}vJ%IZ#8eiNfIoW3=As2~S;jnd?Nr0of9IOlaoG zNdUdROuk)zR%Xg;BrBlB%hu1qEex%^A4-1E$hc@S0pjDvhWXCpl*i}cDbjU7LK;r? zrdeCIJ}`T1rn!mK6%A-V(cDDE#87;Imi=qdWm((@NDw2@cC=jbaafjMl}_a_=e6V9 zA8CzgFi(R53|Kh{e8c5UoT6y{_`z6Xm3mowoiQ@9LXH>iaHs>sY3ZgFlWbMLT$~zkMAQA#&$NfF&3p83vxa*( zRmWlTCSH9JfC>=wF1kq=H6F25pdO6@QK3+$2@nO471=5fVWR>gr~uTNQU@$0;<3Zz zCqEnk(}LsWWQN9$ojY%%+r#QNeBmerY6ROAT{Wup)UTE*IrI3o1 zrP0cO=w`y=p?a$8wp;Tk{v86on1ITl1}$G0Iq7$Ax3I7?;=*Q$5xXXU5x4c=br1Ao zbH!c@`=zgbgY>va&it&tJ_%d{P$GB1>#XRu6-a>boDQeAz^GQ=*f?Bx=;-S1iaE}} zcXxXXOfg}JDTjQ|*0nt6VzHHb({x+wPTmTXjr z5waGLZhVx$JFpLd&_i7TY!jJ-2r)YkMR(7(3RO+bkQc1!PoIY2?U2A(zbyjGzw7Mg*U-fX8DieD^Zf&ZymxhV(c;1hJFTUEDo>5;fm#)gw2b5LA9^3{boV< zYC`9XhY0N>`e!ypPkFG_*1x2Y^@P$J);bl`viMhk-B5>nGj0k5x(I&nx7`AsSliE! zFy6xO5Q#5@0Uw6}jtUf^D6k=}JTDRdiFp|R5;UCWWI8v_0kOpQ8gXJbu{hD37JE*F?KT>JshM~x98;gv%f`YsbP+_gdY%};%rRx1yM`X zvke|4}oa1+D#tNY=tTWKT}bCAO_Rz2}Kak=zuB^u{TtKTiHVsm}ld%$B0 zqZO$nJuu{GTj~$j;U|RW>>C)!k&4^v3knPzjLdh1zAXx~JTMlP-KxiRo&ve@@h3LC z_`%fnvK(PbW_%3Y$V5N)@qwB~f@A0zk&b5i9~r%kOpNgj3n2fa3dl^ z6cz^0thSQ)mxjhh#AT9*34jBv07YT1%6$Gj247tWyAC{%xFsy0;zg5Smd^@7I$(P# zqSXZChxoNmm6MKGBeSo!m6I#~Q@KzeL8nQ#1FZp^$c(AYb9?+M4 zn?7P5bd=z(2Dym(j|qzy2Nxp81yQ?yyWW15XtD7jdg1Q}Hx4rzP)*|PNjd07Tsg$w z1DJEKd-o>5@6hGBwv4{-AA~}larKylv}QY&BrJ9jxXKar0qpQR3Lhfp2Um`F0yj&% zYbt^w;!yy@CPEwvFglVD2O#Q%kV5MOV26}eU?ZX^v2w)kZ64GXN2KVf9Q_p(ELisk zZj2CXwzX3(ta#+8!YS9~KB1|wICcd005%MIYXX($o_(pRriNqlsq2(=m)q#UHAHBL z$`Td8fB1VdjA_j|w&-^Mgq?xFNDwp-WDw6A$1gM} zs)(??e`2ubdsO3>1gXm;<^wMW5FNSSP`+0KiGX_;4vMF!>q$Z`3WC2*$PEON2T-*Y zpBmEhFG%_UeDCMA*vtJ2Nek$k@sQ^R(?dzb29BGhMQc*@HD5er$8d}$jV-5{g zyo^0BRFS`X(N99$-~i>%I6CDRBErSux%z7SD3nZybtT%D7cb=SVw%8Er~(imtrH|^ z{hLf@DQwH<1W=@t!aaEVff#5&N!p&5^$(DqMt6m8NheFqK#xEMQhMlM*-E{o%JaJCIV zNC96e=_JV#fkN6JRXieJ&|Z=u1*musgD8#uNM{tqFJbZ_ctAbpf^Y?wm8BVckJE+q z6DZ z(5dmz^RNVw01a$<)L0jxen26@ifxL`At)q-E;T9NmgE+Y>b1BNXpuf;5DF5)4m!SI zj5XC^!o_x`_$ojoh=GAnw84g@2ohGn1(Ou*KFeFKItViW;)4ee-MD7Pha zGyvZTfq=5cj#uWO;~>3O=EOA)zPoJud~qwGMg(qw!XCK*%CNTKc!Ng-F_+4axpv!O z|0tuRLN$s`7(E<<7k@S?N zz_>j)fdE1XhQ5~bfKxEm+v$U_F@^&S*AQrxHSXVid~vxdKxpm7!qWCdUBbE3{e^;p zU>Uej(Z^sLfN=x^@@ZgqVA`@mRlowm0u-i>uUm8E5G~l30S01>YODHj-qIrk_XQ1Iq|b0sJy3P5}DP zW!;RaHQRv%m$Tp93ubv^N`Ut0KnG9aVRQi0p;>qC3qPjNdYw?AVChVk5Iw_5hqZ>5 z1IKICIhoOGUMb`wzyV9d!ldLUFv5?OR)0Z$8Y*M&vZ*Ap*)Fk z{>hVvrKRyjm)pU$jcn>_hs_C}4vh)&Q%Cj{!RuRvZYC=)&k{zW-(eE~V2E(nvtor} zdhzYUHfsn8z+>PYlgwLgk4c@p0laE*GaztG4XYD45D}Pe-FbBKWFGnGn=VvdWVj;D zvHo^<)WW6&4FUL&@CHLT_B%lG#f5Ky+xW-Q$c;Hj91SWw528o>{jHLqdk02eMIspp zd?s<%{d_T;3k1VwSBmp3>}$}_IOF5q|7x9n53ztGyLQL^GLyU})1a3)o=Jo_G4g|- z-N+~aVPn?wt=zc^vXqZtFu(#AD@1@PN#9k5rvfocW004{)p5B66IvSW8U#M1^COD4 ziXeqbLy;Jh85pC^L#eaFO-1KF!#|TsH-L*i_#h6Fp9L)*4#J}$>}|tcAclT5B{!1H zQ*0|_n+#kwU0_G;I~3ba;9+Ay^*@dlM?_&=phdl3b?#7J`wUG<&jd9{)R+dZF@&Nt{%RO`n0I!MqXZ*N) z*ry@K(~G~p|G(8A)6Q%rrP( z8q5#pw3&>z8%*Y5m(p>{KpCdyb;GBQ+bgLyXxj{7Ew+8}e#a*q=Sk*%QgXIV~MBLY`~4#iR+Au{?Lzb=PTXpk03?@12?aJ^8sLOV_ih z$5Ov38)vpo896#+yLn_RFd+G{?cH-yd_F8m8vr?sC+$D6k7r-NVPiUoO`Rc~Mf$<- z=x|am=TiM^5U6pHh}S@EnniedycZ^kZ~v*B{(mKO_4}S{mVl?QIa6B2N#%$#bX(Rj zav${{yH#@%3RiNmLe38?-TV7!69Oq<&&AdPmi4soY%Rvp~wYs5-~CudjVJ z5j#fn3*RE7!2*GmyAU=wvw#j_(;ECGYmWhfhI7giHwxAWNKxEsfSo+1KP*Zvbs(7G zK6Wy>2p8P%vWlm*D@2iBNrX~DmDaBsX7ygDmp4OasNW$H|KP!;eWRz_TUx3Rjdd14 z;c_TQfzAGv1A5SH5Ir&0&?1myd5h>c2s>McP|W&3k8_>zAEXEx?O*Bhl%}JJsYZ+pIZxTouiviCAOPhcN zZupre^ZxdKJPNlm?nR=*1P2G>3iNWpxI&4cmmIYeDu6&JQ2MtZ^?xeQD;ZA(JvF`! zdI?f0ZNKqN>HPWgUN`b%opll$_v}Og0!yU@cxH0W;!JZ$&|WL?Jt!>US#c^f`3CYq_CNc=bSj!V~He#h}#^nF#v#?MKYbLGYNt5dJw z4BMkbJvnsxm0~8sbBLIA6|f)BngjSpk!<&*J^I{%)^ zb9-fc4&?z84awbKSVH4O=+kMhdjSIJJeNK z?FbbkBo+xJ>VgV+-}8Kj{b(#1}Twm>ZdE;d=q z55*&N87SF_xoHjD>g22jA#6_IiUg@3=G~{jPqg(GLqUgce+-8ce6Zy8LyP%R`v56p zpLbsEC15;?B~C@pB(k!hs?jwvdW#WP@1guBLB~EnM#)X+K9ytIPY!TXbc7`0i$H$l zn;cV|-%7COzWQ)NO586jj5H7-AyNN>-gG)W`lCpkp{Q`?wwhGn4o4`}Av~0dh3}&%QmeY{#2ormJptiM zT>GT~mJba$u{r~3lki-0CXNX#WQVU4<)r9A+YX>Z92%8nEYIw!zYyaRg$em?1Oz8E zEI>i|_A_3F{$S*9+?XJ$YE1qLf=I$7bQ=?snER|9t}!sO=`in+-Hf^v=PB+JI0PjC zh#d`PwSOZ`+-#F)IDn%Cb1RZ$P%w5%5CbEdEEA6gZ}d0|aEaU#lya)U6LapUIuP8D z=ULeSHXX7ql3xh~tcKX$@dm7HZ1~tibWvihEHD+ zQP&Vw2U*I7Wm;&-(O8j51BT`9vSeHWeun{Q#F61z;5=?dG`~d6Pd+iMW0)?{aH;tQ z@@-%$1wEs9C1e0ZizoaW82>;*v}PMkmv7;uN3z2Zv)wA4M8dki0azg?4mfdIl>CwLD;z(cdyi9oyrYT_V<;zof`y2;Pl*6kr0h3DL z;^N}keOsWettn*-yT;JFIuQWi38W2U3rogse~UoKu%Z2KXxJeo#f;F9G(i%6xau2~ zl!TYM5tSnDd6GNGa^zzP8h>x=83~IU#;jdGbtD(9G{ONS&cCKY zj|hKt9~AZX;1Ywai4+_Nv?Hi0fRdct7r%pdJCvF11^E#d=loXoW?&jY_hBT9ldzyc zjuv18Bxt!Hb(Cb;08c5qO9ZC)6O@Hw5ZR%^Q9~065^4Zv23+Rni;PN#b6*0*;(EG? z3ltib<_a1YHHAvdNWjW0>8%gHNe$KnO5LQL=E=7Fpj zC-76mhmh_X$rtDo@F)PFqp$->26(oO`PHGkEr3Gne=JY$_oZcf-FDg(ngMCFfPnUq zMT6fQy^2<n;I7#qj#!hWkW&cCiEv>jl(jS1!S#0NbNwQtvj@S@1W()xH*8&HIZ- zGF!6-K)nO+fw?Le4g9WM``l;8T2r+1+QG^<#WmPqD+AcVn@8;l$N}FUWj|y+P$S@u zhG`N+5e?oe=p*6^g0}!ffprClI@m!dovH${cLjGV?;pn&4+pOYsJuDO=Rj!)IVY?n6Fe^w@(_-4GCl}AA$kF#1|zpT zIuZ$>f2-Q=)rSp87|TZ(ED`~oL6%_NP7F}%e5}svSBI<;A7 za)}2CSK3)n2FD@ZXBS-i+l*F42gF**_Yo6-n`PyvW8R$;y9cGOw+L5~%Ie5Jh)m8h zF>a&raos$$D*8Q5xm!gCT4YFsQxtJ;heDBsmvky;Dk@JL({ZrDuqM0hN}7d#q0sNJ zfkt%F=maia%uUGrrn_r-(RmH|e^eAGuJNcL8E+k0zexYE!MrCsC%1<*EhyMkbci8~ zjOjtT6`)E;a{|)@v1Sm8jm`-19E2;wD@GrFrjhuR$d-C`rKZAW@)*jnjbQ5W*Diqo z#1IyAwFErfD?&<9d2+}fO~3?Bo_v9 zspAp38U(SO`yo2Y0RKb23|Y$mDUlP0aTMoyb&Up&z#|i zj*cb^5)~#;>(_9JHv;**AZ4}k`*g)ym>MBNB03`gg+S4qJDzVvsRg7L{Kz7{0>T;m z3sOGdAhs+MDkazfshfx{0qQ5fI5-~A+zFk`c(hg*5D@Wd0a|KlY1#eqG)ATl?E1qVBoC=b2Fq|fH;On{uTt*fbWpa_YwXyB7y*1h1fB~QoLEB+C)P` zpeP(oJ^6K9M|79~1=W+7Bq@%4aOfaT@c#OjNZUC$d|p1MO&ygD%5Y*Z!p3d+{5fO& z#Y~ieRXF{DtCK<*8yToUU>c?i;h{oc?v2k+k~E2P>bhGh1ifhS0k|E{yQT%o4$t~2 zifV<~pOm;pz)@M^!*07?QG0w(O4~t~s^&Wz8SP{^Q4XZH4uQ3@0H2Q6LGn*lGkOEC z$kmfWUErWYJ70?jx3^OoOA%RbH86-_v=Rb(K_C%!=GLuSyMLFX)tvp}?S7*ju258* zIA9)CKO5ij29gZ`uf)Cp|0NDOGl_Us33FH6kJ$1_ez1}CknJIN z1It2JU*8ATg`y>RQHg4ua32u&2A~Thv@KpaL@|eSn8;0pdfP+IQ@g1KElgLJ=c(p{ zZNeib=gd6_X^PuupXro2fL{KcJ9~qhheoA;DrdOPPY!9MDl9#2Lep;c^Q4*O*DHoe zy6&x)v7SU&$f5ER>FDc-brhZi`M*vQJs%;XM94*I!SgW`eu%xSl})w<1q7sf^stkT ze={btMI-th62Ff>&UTO2aa$tra_ReKsBn*_kEe?JqJAqKtQA~uE|6b=Egp;~oWqQJ zz#70hCUkC@UB{NX0k!Bh{O9z4f3Y*0Hy=Z;lSc}P zqgnH+OAj@as&+FHh=QPwfwBO1oX7L51E+}*2_Jm2aB`9sC&&pVg4kwcyeTwF`q<%; zgba~)7dO%QQTpv9WdN!aT;aGnIA~hAU!8b zQ1_JU*n$VpS&+9RjOSgPy`vLXW`2J6tAfD){cvMmqbDT93A6-vxvxkqrlRmH zavv)VtTn^6RK##rj1|Eq$oGLXF5OP~_xbNdGga8861c__MfkDbu zm|ypCaU+2iX^f;;#aYI{mjVO5GH6m@y^wj6a~D@1ytL>hKSCT#M4qTki6|d}L{glQ zxD;SkQ#WRSDbw;0{D+DI=VZvy6t(T%oJEpO%JG zo}wt_A9}+2_e(1sZli*wZx>)KZ+w^BRXmz~eD|mDJ637b^gNa4YeKp?s3i$PwL2fEstKT!IBy%vaU zfc^P*-Ilh5Jc!_-os2a0oa?XYwI})6@F_!`Kss0We$lZWgJ9xCZY$!f<1JGfpc-Yx zlw=4JFMx(7w-@#XDST02)RRsI&kA4grgvQI#)xr~!h+39m7gySz&e1ReBC>KLWBju z=bqIFHmkyy1FRy6o&*mi#E1oMUc$XB)_E;%AORORxl4?5Bg?2b@K55&q?y1YnmL#k z@n(~W*KE&N*`w@0h)0b@7nccBeNODor3B|Pg}_?g4x9Bd?^V+H)ana@MJft|9ZKIJ z;9al-hM=gk+k}Z;D7NuD>w0oUhO3TcX?p~s!y)q)31J^2dRD#2h#!A| zoCa}8No7Q&LQYF!r3^niA51%n@Yp7F<8Cmfgjuk)N+RT}p9DF86x=XnGLmzfFOV0J zfvAeJv!||1AOxQL4J5y{Sx2LIC6z67F<{C-5zz;Wr6#&MIfu9)rf?|%$It7}V#8>vBBgP*yoVuOL;6`-2{qNC z3@0Hz1Jf`cT`+ccX0fU}I9gQXvr9!LzEDb3V#Me$0udvxHOcys7;{4w{8lK20s(&_ z)WW4FV3#u@D2Ry#q|u=(2MQn$z)-zpbBPAPO0#4gkl`hmsnATGHhJpk$xv$%5ijA* zSGBv4Qj%hT$WV;k$7^u}BwUbO^M6_Zd9LQQ(v}izFnnv~8<-;z1w@-YBn41StN}vx z&8~VgjJG0u>Bo+Af7Lu5<1qCB+Q~w(1F6^o&ash+JS@e~Sl&jFM>Vfie{&GnCh3 z&k^D*U56NPph+;$RxcS$3!6JjG=b6C0%-584=ljq8_WCUZG?>^`7p8^RF zpJ+JnAaPtF%*GEXx4KzO2n)n@;)eq+cXhkzW+?+nAhb3Hvu$7wK;#L*f?mz`1Th7v zIG!DV2NMS&rIc+uBI;n!KiVour9^_?Nve#9fnBA0#w7xp(N|ZLO-oS_m7Cay_#07~g1sx(W7HzhoT_M%ok7Ajh;*vvhib{TYkl}HPfHXlZ z!=)}14XlVxo=f}S&NfL+ z?7!LZP_K6YQ-z|fRfLPNlpe;K7;RgF^?8WrMCuQ?+UWd-J3Hw7(oMV`>1NS~)^(ta zCRicut4N1j!M8hnI&iv`;Io$YMI0w#XJmfp7jP^Hb^;dH@+* zzQ~4ozccOE@e6~OpXw7_J>EihN!bq~+7+HCVD_k}*$oHhwCS)0Nf# zj3oeL8j#Prq~3*@8Qx2G!n3mpCQQ=p7bi0nNiWA2sYcK|?!vD#bM6|6(ji3qO&luM zca%2kyOr2jE71ckz|53uh&CTj=U(y$T!tS@&|P9~#g-sx69DHYfJ7Et&E!*?@(hk4 zgL82JVB@5ABcL`DLxYv*e>clA7rfA3C(?Mh^_i>uqe}@^2?*WcN!9}G2hh3~U0u3v z7CLC;&OOi-A^{UM@2>s#8w)JRU-tC!1~ureptz>PCdT>I$~}*0MPYXlraKmf*F#UB zSSS!7Gnbx%O1qG94L(XJiN`)3-%Ezjrd?vs&MrXmEy6-Wg;m=y0T`tR>d4mszSIRG zkuyw0-|%i=5+*h`iT+q4b(1wf`M28cg_57QHbJ)BjTVhRS?i=|EUaj`p@muN1~E@M zom@nq-ihLs1m_W(3YiZn$VBj8GF`HUQ9DdSfIPFUR5$lE$yAH(H^(-b5J!N#xQFYm z*34!Ik`QVRpUW^rOv|d9lx5x@3^*19cnYarBDj54{~IkfnczC*8tS42gA=+RKYWGh z%9Lp-`Y^Zwfrk8>0bGkRmb(p!-9dyF!#EZI$qeL_z|m+rhC8zkXbG5};o1~tmGek{ z!^z}>lNo(j2{1^4--lbo6T1fKum)df!M>P^5yb{C9EZg7#?$Wz1`=J7Wr~Y0T71{| zdwE_lgn;ufFU z16?&JTe42=&a`;dozH8K*=?=a3QrVOMa3z4UIEAW`@$7KRU6FX`#miUqrFO!FAs z@u*B2!*Yil+0T-3#o)}0MPtjZ*j{@pj)*@n7^qz2Ix_5tcnjo6s?9zd8wIff5k|UW z{zotPr>UZ?Z{GYtA!cK6=EGe&Ohg5<#Z@DM@9_N(o}n9WdGo2^J#sl{<+Z;|wZxwT zCYyHCdgAl`K+jdvf_E*hdCYc+d|h_fEk_( za$moE`QlF7(SBN%m39X;1tJ066^aiPr$JH-ZwpQl^3Si^c;;2-{NWz2>s8SY zLR_j&R+QrB9y{*ms?Syr>HIl^t+c+lPw?nyt82u9$4~58SKR3X@HR3a^2#r@I+g(G z|Ni0LL~)Y`G#*N79%`IW|K{XhNQPS(vh;^1 zN3EAOdn7CJB<9=>Rfe?I5X;x`C10BkMI|3gdH3IEeDjN%`j&TNKi6LOJ>Vd>s%+HX z6VW^%#Po0Q1s(D@AMMITLHD{F*F~5*e?z}kD2rQN;uaOYQy;7Ke4j^l{^RTK+E1Ue z?n6<6Dign*pq@ZVh}&D2*^9>BoHhlutM@hPBJmx8yg;Tx>wE_K(;Mh6$Yv-dx?rI%2ex~Vr zvuX8oY*bGjHRhAiXGNzU?GBA*Yzl89Tup;RbZPyL}p!l{E*IkIy}C&<(5*pnmp*kes- z;UT-jOdS!2u6eU6hb7+|u(x7+9kRo?ispjt;@(?43SqEU&!kkg$y3TYf7fwg)GQ0u*>mnU zvqgfWArIw^t_}e7jDU`|XywIbB%pY|wr2H}k)0jxW>o+mjZyCEmUCJt4t- zxVl(usK=Im_|-4(-WUtB-^`N^ERBzD@$mRdM6~+#jh%~U_s!t-r!%kh(i@PRqVYYY zO#fhuN?d}kug)HCi=MGpt?ea7!M88*ygcIX@T4mppTmmZyw=p~za{wVHZL_VDvy~3 zaO*P3G?)x}^QMBH{=wV<1OwuAc%ouQ$;?5Qp!<;~+ScvHY=n`g>kaLx(F z1OB510lOr8KC5^+=}oX4-7o%p>V{ysfLzm35aTPFN9GG`UC!KXhm)Evdj|7R8^ko+ zjW$&}c~YZnQj22yz*_nr^VALBz3%;e+__QHo%;&Ko}A@rXWJR@UTDEWv6y(MDR5l) z`W}S@FK~?CaCg%{_Jw~D{hVdTEx83@5ruGeC!w8 z#^(6>PEvU+rWvxFdgvYhWo_+N5vkIZdj$bKGbdF4WF!n8OjFgkp?K^^j$&oId}3DP zXbofX&HK^ybgzzjn(w^1aUj)-`%kghY+poRZwZi)JL~rL-zPbkKkw@WY zcNpK4mN=MGC2=UMrSVd5wz}-WV3Ve`EEy&lx~%I(;x_^4t&@_v3hE8bJKl3 zEgP-=x$~o^0aMUA_*HPW!A6Zp-NUHbQ9QH087f`3ClJ@sonOCLFf-vMuRSJE5qcZO z%87f7q^AJw07%-YCX&sE(;?E|pZeEJ6PYK$`mEK@G#>xq(NpI+pu|)~A9j{^eoU#M)4v3lC-#sZs!J=jRq%FmyN#aHSe!k}i{RZvC=BG!; zijx~2Y${iBohiih=i?9Qk2$$o$@DTxCXEa6W%|udA7krWlu6@@{4y@NJni#r?=o*$ z-r}Z~OQ-3R^C@&3BDhD)949E1GxK=l=gerTIas@9cNb6&S4!IX9Efvl**`A*E-mTr zuFe_ZZlTyTs%ft>i_`Qstb~0tbc1cH+a+UPpV&4srd)Y=H$UEtl%6gKcjDl6|D?kNnUVCZD*86-JzYSt(k`_{K5x66Ab`}@ zt}ZTf!;I2ZQ4U5!u(Gp1(9KF>dfBxXJn;!Z4Ux~@NLopJ49c?52Tn)66wI9YC_l4OwLLyiuzg;5ld8Yp_}l*VUFV!z^Tum3S_^(^3*Egc(7Lid z=F&&WQSHit4>`>Lb|%I>Urha=&gR=SzhdWnLGs-zx;3Y*c>h{@ZOtscshgb`uDPY> zU(pA#;a0yQ!zgCXl=W0P=A&O<`fFCu*j8)s@%!_r*Ix~&?>jD@Ej)5=!{$Q8+kKO3 zd4$%=HO-`@+A|yoj6%7tgL**HJ;2iT~0gw z;S*oRslz83&F4+ud}@4G@^68<;FgXM&+fH{?j?;_I8NA(NhaBQt15?u9o!mqXy06S z@tD+m{sSd+H_T}YcHPRjd!Lz6&*2EG(MHp14SHo|!yR8dS*`@xs?bv%mF{Z(FmEc> z{jH*sG4ZC(o-poj-n-?)0|r)@>9lFfqB4@$@NC!_W0$4U`p%tuuU$V`e&^5K9vZGx zs$HvR*U2p?D^H%>2WPfsD|kWRVD~P473$4zy}tcyJLZL$J32-|k@G35RVem2nmJUG zTF(PE)DG!^8VAnBNw;gx{cd?Vm*d$?qbFQ~46yuynIQ4aYp&lffzpFWSfIt%8qkT! z*s3COq`E-(UQB*A|D(^YI}&`DGWc$vT35Y>Qsjt_16wuQdNqonv6tOO=V==y?VLHb zZkM_l*Hsvp|D@u`$wwTj~%j~R!Kj5&HQXNL+soaX$@)L8Igx67nD7h;G4njX~#@_O_1Tuy0@IzC#n zJ>Kc31l3x@jXifY1d4BL-?ORw&{eC0dnQ(>@&35|!QjcXB_Y)mi^Mm$hkvCqJqCM`$?yc6hbfl1Ej`U9&K&&+o&h zl06JppcPOsD>1$bTmZUh!$q%$*n$%Qb8OcFk9M`|W4j<_-;y9P!?Q(-w*}jHhJE%a=5$Q5VAq>C!snzIa!O+ji%<7^XO4^Ktm|T`_=OY}_XIs+>HTwd z=9+Sc%Dp*_w0(I>?0dIOj=bxqtXr`6+qJJ?dM_7BtIh>~# zv-jCWS)?&AnKB?0e5Fbvo7FX5*P8z1-q1~YcFw$l6tP-3GH06b}+i2RX*J-v2M(fFQ?FDxU z)v|Qr!AoW>{SH9Zftmmb&Cc$dz|fVCN{pU#L)q8ll1Xq?PF}9X2JJYpwSK)z{HIDX zlT;qAY>^3(pljsZc9X|>(Lb{3NQ$e_MTaea7z4b!^&cfjG`(Sy;oa_MYGcI^(Rw@K zt51c+4mw-f8|K35PbsNzYHk?jVQyQe^IAev(3$0k$XwsgZgOFsElw|7A)nc zjd=Q=sFS6RU`#jS5mE@X_+4a2btsIph&^HS{walHk4%p=N?hK*VYTkblq%|JSE5*} zm?o$z94*MQ36lp}1#(Bv0iG0&mVoJOY925#jy}jqQ1Lp-UGG!7OSzDn_ha-(#DnqS zGYQP&{s^u@opAoti;m8e%!PLz82I~c^!3qKyZ`3c!V%Z-dt?kNI|K`l+Pxaaz+vt?!SIto?kv-T(5p%yvNg~lh z^IHAp>-RI9(y6`*^4~}oc&T&i36!oZ8M%>PrzBh0PNxqGmxfQtyeW2Uo0Ue1!y6g9 zHPK}cwnc4UZu$#>jQjVQ>jW)b$7|$J9Ey|$X2ou$+Ncy}ZD#fAQL#ll^(}vshKM1L zYxI%_mVenw+Br^&O!NoHmTVlI8ag}!w=*T?V8JGdkt|I~Y{T1+}`*)D8z=yeAs zAm;`1j;kw`+Q%1P`~t)7R6!p>6b@Qq!jE5lzSfCJvDQWN`U^4Ozc?ACN`Ia9dDk? z-{C7g5&2R)wTL;zdhyaW$t7xhk;h=v^%?HHm+$Is@*<#PplxI=5AfM`Tp9;s~Ss_ zUldl+?75#fqZ<3O-99Vpr0EJpHuj%r>$zlVIc5h;R3e@yGMw7yHAFE$>H*?8lqxHv zDvFNBMDP)G#jje1v&r5*4l~Ru^5(RiM}O5vG@l3@efac-R7}({9V%rtW%e6PYyt)m zsPq$X!Zu*`75rAQ*hLs{1Z|isRP%4cm57^U`TYeR*d=)!8hN-@8$xkxycZ zf#UA~@9wp)LJC_x@x|Y4ZwS3|RF|SAWYlWS3P!frmnex7|RA6uiO zVCI6DuqF2d4%!`~){xmIRhGDDx2=Go617nM~X^wASS(BHD5n_$R#7Ie=^QmmFBbG2^Wd|-;QSt+kZ;l>&;TfuWxT8M|gOGrdi5j0DUlbaQIRq1y=f{(G7zyJWr6HUO;SUFje7P12QQQFE2{5cw}q9f>n|WOUSQdB`ySLdzPKrfU+DpSY5?(!&UE66i)ysMV^!npvHE&! z*U7nRoU!>Aj?tlEaC6%L*&9YRN8P;X3H=Gw@7Z5 z(A+T5xPXyR%{+&`mWtNYa_B-4(sME!iVTkCoJ{{U8@RZ-p@RlY!xMqm}{{LHPA zP$|}da+j2JHriILz>Hr=a(^*Bt?vo_=-k{X5afi(?E(BAXO{1LxXHsPsOHnJf%;qz z>V2h~^ExX#cI;61d|D|f;`k(V^7x8c#;{sK-n?9s)rMPw$u@RY$&8g)IXHG*3opqUmM-M;VZ;($V#>bc$ zo*CvERJO%E^0T%V$LUtbJGY%?|053bWtq4_i%puf+e7TTGj+4{=mbB!f@FcIHsA+0 z87uBk%F4k}c)W{q-OWQ`tKfE(m6}Xa| ztQ!hMCe_39Gd;bJY0&#q*{Rdr$@Za7yeV^jBhNXy$46K@eff5)dL}y-B<5Rs9q!B= zRi0J}om)Iwr0Osb>8;Y6)XY_U{cp9zb>IEpf~uJXP!cE)Elo8(mz9Dt;v!N{h;|?L zk$=a$CXYe<$LaKrMC(8mfV}h_XeejHL=G;Indl&h$g~2$O360Y(CGXFVULDIG?t_K zL*0S&M^TOZwAC_$ak^?xHr1QuBYhe|p8)HkQ=j=$70Yqr5uk3>h4V&I)1lPpNsTT* zli2C+e}*@o9p3U6J|GYV8`mF!0N~@$vpRG5UT@NPSF7{} z!W3SBWIM@Yx&saXDvTHrjqQa#;v#@HSOK~{nhmq`Nb+74f2{Fq1eHOg^;mJ{-~VX= z))37Av5DYsLX%5Z7hbyzkqMDYl60>$RXzP^cqURHY)8t?Z24c$3Ni|l%zanoD;aq) zjM?N0+4fNRlDL0km;UHuOby2xzkFdOsR1BX2_GV>#Q8X{Ae#k+)+6ijFo;n84A+7H zdNV+BQ^)5P!6f~g`?k@fAxm4#V7U9wlq(K6vwYS2C+-=Da9(eZ_9(l5oS(Tukw8eS zw9s|vwDHDk{ECY>qjezMDr(|^X<6urpe=qpJM}c5R-yxWa>tNW=@8XYvZI^Q29Sn%%&jh z`qOh&w|WH_LU+YjoQG)U%KrPyC6^L5$U8}1``fz|T$nG5S@4`Fii}0~!{A;rrGHAF zSzjNC5LCB!e7kz1OeHltoBKe%U?49#$Kp;x4ALv>6{(8GrsH{DTltk>5WKTdqT7xN zr?m`cRlkFdaGUE9?kAOgtta@*23apuJI6X6YPsVvdb;tB!}@*YlvG=fYu>(pTW~Ff zp60h5TZTr8#on&F+tSO~=J@-d z0h$ZHEk3%X)G;Oo?EZ3>M>ea^7hOqOw61@|t=;l$fYk2Tj~lYOB|5y=(E6x{#_62i zUCzR-iT3;<*OsX}wsRG0D$^^!9Wp^%kJ8rqJgZ2M)qs@w!s&G8I(%dV z)^X6?6RfL#eWyQzk&utc+n-~~8=vjHpEU9?YGkd5e7@GV@GSp=O|6e~-*46lwyk=^ zM8Ey2`yR`lNvYJ2XZoO8AVE4H@wq>q*^jADaNRVROW-mfHe86B$DF4t@-dBu97o2Y zbtG3)I?)1h8}mX#Vj?Rtf*s9dN4$muf)%*!QULUmiD$ymY0&XhRaMQ4>+54?yCvc@ z(95R@k`)B;6s{+tIK=2Ud{bMmg)^Wp;}9+dq8@S)iFn#HK_c6Op8{HfIuT=Vys*ny zVpJ7!UBsA+h5)$>lp}#@VlbuQ8pHSI=L(&`6i7@FBd!b}1H=@C zpo)(lP7HUHAt4q&gyb9`ma4O}5i&fW4G?fZ-SNBbuCJrqdqj4#Zr`pF!G};b!fZqH zfT6qa3z0C9+qa9}HKM4xOF|J5LzHj8m-t=+Y9AtT!y%1d7lK4&B@wlBq<|*HIXISj zZ;Tf0{QeKa!ll$cjI7JGMNQi~Y!*u{HpMhryzK{~ zxMB0=5Xg_hn+LGsQH2E7z82rq(z;eH0vZr-nxK_{Pc;wV%|Vx=F)_IATla_S$(Rj5sgjMReNY4!WB(_a7j7M!YnFB##pO9^yT?In`pP^#Jv^{zR7c*`RLP6 zob`9dxp&bSrm=HZehkg{`M{f}o;u=!VoUnzz~Cz$tc?Y6-((VXU-;e6T~XR=>HmJ7 zsbjf~_VJw1_JMCVTRXk)3#k2djrewATZYcWRpBb>go{7Ci|wV`zR};d{eIg-eW$7x zlh>x`KlEL`P1T9J)Ttxfy?Gk9l&P%@D+O^1kHFqw8<`(4{N&-s~Ce4v~MDZzC=({RS`Cpul|N&B_Cv6EPN;`%C0!#VPfigVHRK^M3!?)+?AK0Mp=r;yXPwF+SzD_TJp4*%Ab_1> z3424nhhgprr0$?A|uA1eq2S{a-S1ihM{Ncazo03zV zcv}i8B{bg5#O2dc(o*I`)v0gs{zIkqM*VN?+x|mLRS$Ve4*r}t&oS)z@jaz&<;Gu}2)gI{5BiWIyc;0Cy>BtmfLrXDL2^Xd=EW%XC!j572nw zZ#B>srR&BDR&{6Mh~?XY18RRcOUp zfIgan;jD@WUQ^omE3sB*oOhR&U1sfEmYM6G60CA^aBB}6afxr(=CR4E(=719nU3lD zzq5(IddHN*YmV3sq-p?mz!W?SlvrfRUd}De@O&q!39|D&kzbatER7c0bVv{x<`&N} zNbd5XMQyZzYXZ~k-mqEPAzY9g{YaH0HWp$gL(mx+7CU#JTkA`>oO_=5?SHH+c*#OawiY5zxj(YWT9zJv zaY-VeE0SzRl!2V$r&=X7=NA^{h?o#blW(TlO|_*H`~4IbcQh-m$V1sfVNHQtpYoZq z(ZAdFCa6CoA-aU{Yq_3`eA-YFuS%5cn?y^z@g2toFm8$H!}0&ucw>?&8Cz+OdlfP* zY>Zuz4Pw&L)8%=^A@BXF%Kr(0rLdC;#?D|}Uut|;Pk8SBl>X=I$t^A6u2#qR7_y4v zjtejX%4_Y)K|xd?0vL27m37xEhf)m*m&L|5%(CUx;@yg|A0NLNZ(32q7Z(~jo;~o$ zavY^SxO*ACcM*XnvJR=$3ZJeNyKnqPudbI}xcBWBNu@pKV;TpQ_D9rh>Isj%!mjW8 zV_s^!n0uX4%}|i|T+hnw19Bb}1G4hsyeIQ`zE4PWaxbtKG#;B+Z0;R<^He10+lNF| zt-UF}fKCD*%#KMlnUxHCOI!E0s!-Y}I5b*&0id>_XI( zJ$shy#=h@MAwmctdyx@B$i9`ONtQ{{Bx&s1Sd)fq-+Ac&UDw-ny@@gNJiq6h``qU~ z_nDufu{eL#{cqokKlRSn*58ZDD!Hz+L9`F&R$lKnIr~-rgw)Mv52l{nI)45-zv|7$ zhu&mGKTibm2_B+WuU9Y9*^2QR?eo-I6E)T6ZeLtEJlxLwJXx4ZB57n%bjVIT!Ghvu zV;g+a@v)h!3?;cn92wZyg$9hw(Wu5J2`1 zTj*QC(2}8NzopclZ|2DK@jX!UC3kms|Fsi#8M!uJJsLyneiSNys>@E*u<7tDDh7P4 zKy?M9xmmBxIVp{mqLkD%H@6~^n}Wi*^~(CAEn;)6+3a6DS^Z@Mhnz={}6lZQoX#tEWpH zoO}?pCpcv0g6!KLrK~T{RHDw-?kWCr@}VjvPVJoJv=~LyEQ1KD9x4qb&;1wQY|7q9 zvgD}S8=7Z$tzi9V>ICWD?BTt8lQi01PGBgnIIb*ONgwDDxopuEYk3MFh3bN&i~ljT zv6oaj9nFNEPd@x*7qkYrbT(GZgJ)mi`Sw0lO1Xb!D?xetl>6lemML1Uy!E9g^+r#A z&K`R^>}&mqr^dOug?cpDfbNW5QUrG9Y3fb!OIM4VqMCW zPc&zQh0aKYdhFzNpgJIcjUa9k07VsgMRr7zPi7Sc^5Zu7nqGgX=6G)R|tB>`Hzz~8Nys-cDEt7(% zx3(1@VH5LAN-(q;U~kpiRcA57AQccUhna#TjJ$9V6A$L0X7KQ=2!8g8g_Z^5ajHGx zEpj{zxYAPWMxEm$X+KXkx8T+YGk8Ga;*Mwcu)x45@E%|GcQ(?c-cAWHhTS&ciCk2s z2!P7l`=RUF7hbN+o98jP@vEDAz$5`A!bwRw^bukqA%38K>J?FwjpM2Hry^{kr&pS? z#j~&0()>MsErL@sT9IcQ?Z2GF;S*yupmJ45UTl;9;b+O8VU);g=mbEv4R-S!-+eB& zIBms$dvjq)v|IiOBWIf$IVCICV~H6UX3A+>ts`QaM{UL#lJwV~&5WaPfn!WcWg~d# zj1S8!(w?Cnl?64~QdQ8c+DS$n|28+&TxMEFX8lJ9__?5MHnE=Jw$mA5+`@_N9a~!g zv9gR}U{LrC24$OoyqM{K2a5mCpPRwsgHo4pGgtE|4kN@OHD!p8r~3@9ZE9+S!0hYV z+J_%}KVt{v7HFDFI9b!)bNf~w)S{aR@(94xoRmsSx6Tl{L$YF(UPptGG779#A zu2(v-fdz#%2#=wz`2`4a&*fOV8p8?1j?e3d%R!hgy>|9~LBT=B$c32EP=hGm7NI}u zWrB$}U)nlE+2Lc(tQQ$EC^oJXIh{%T#HbhtLwnuQ6vIZa0TK53HMTEv`FF1xo3Pv3 zVC=!Ydo1+1;$99Kb(B3aWkCulmpN~;R7Q%AS z3xUlJpFiJ(Nv?C}&Or>xu12f7#DcpNlm%;pTbO>B`U@zB+uzzX%MM+p`s| z<8HJ<;v9Wb7=5;8LqwqAcm=!WwfC+a?0t>Tx-zSHWmhdf(*6jmvHsC={;*!)gR(KX z3lUV4#aDlY#-vGumIXi>o+O&Z+p(4T;dfMj-MlqO8~&pjNK%Iu){jk2U-6LqWZpc# z4|7H#>=F;Vs^iDH^YWSSuh9wagd%H!7#rbx5RqM%;a_2%op zUjYr;X5q`qIr4$t(z(hD8WBGdU~iHoOtK+HvEUrD_oYuVGGKy?z`{(@)w6L!z(x%r zyT3|Hm!1Wio1nTD7Z=xmeKqo(&P8VWz)-jEQhZ%)?aWyXq6bY2vP_6N_$|b8*rp&q zm^QuQOZIwMV^}N$y#nwygcek`X8z(jX*;m#TQ#Jvr4Q#~QQGxC8c)R|Sq=#)h@aHX zADD01aV|wFbrF@(6IrWN2;f4pBNRo?pL4)a7*BEs6d!QFz{aMCF!1$j7>p$S`2)-@ zVz2_Wz)I1WR|y;#B_$(xIkl-Vk%c8tLr?UUO6&p0wAWnd>KbV=BJ39+XdfK0ZD&g1Or9pr};GNf=DLn{O z6O>Tb@7!5$7qye+#DG(APruIre*PkuP5@&%khgx|<)HnT9ad-YfkmqsBcBAr`+bi4g^Yv^K85pDx3A!^Yh&B*jysAw zrd0Jcv!wiF))iebb{~E>bZ0WpU-R6r5zO`~ykhJ(eX2W0 zn9kk1b87v|BVHi8(0v_UsaTcmO{@GxtdCAH4veLoVIYdQEfkgg0p@~M!6l{(hMsq} zCNLN8-m??e*TPs_1iEWC!h?A4G|9kF!CG%`v^0bFxu$P+B~Ui=&~QCB?M24 z+Ow3zGX5mYBR-7)ilHPYPf9Sr*P)w+3^hQV8}A!@Jy-J`n4`gaM;3u0OE(c09mKtN z3+jUX;HnE_40vQgJVMRvXEuB*yV)zd1eRqBh@XeT0p78Z(zLna4U zbPOgNXB8YT?hc-HK`3AQy-oO;v3VmY(<^X0^TcGUzL|LG^)VgohJ=RrY)s@?X(~+g z)Nz{rdjbdE-`LmrUrx=5AxY9-+Qq~}ENujJLFX z-oP~~KzjXDCf5c@4GVZ=`ez)X2vGgt z94-#K($~DU=SeQ)ul&eFa6IQB#5cy2+ zZsq9%1r;5(x#{=c?EE|jf63=Nkrm^v7^Yg(M$k6%7HP}wjrTpr8)Mpsw8Lm3ym?Pp zi`T1jB;I_p^-eZtV@8(K+92z@p~NluAMXe~A0=NtmPTSmk*c>TrO9feac3+ZqCW+TB}Ik zss8)0@Aw4%Yi%d}k1c}w5tT3UWOIJ!vtBo5kM$H19^a1clYDO(dFAhUeR}qGafYt5 z$8bEpj18_KB8GG|3h;Lm`%*)z*7?WZvYzQW#@#+zCUW-apn{IOyIrhh65aak<~9!f z**)|%{>R~m3>)2t`JF){zPkDp=$tN>NyHtlpa8reV7{e8IIM*O8OMgW~EEs*KtFPW|@|YZ*{ro2|cS!0u@8{>ob|FUWB5SO;j0{)I zXxH&qIIO*c1N>Oa@MFiSTQXVKwfnjnvNJNyKx&X;b#r%bH7k8vhR*bvIN2;T2G#bM zYt2gT^x2qE=h62^K{A5)zRJ-#LoQG|YBaj^)VKs5HB;-!&5v8?4p5x1ZBAWB2FieO z{JSZ$8OTPWfLa!6^{$v<*gzW;lr`?!4rX-Ns)5c)w_~)ln0Slj3UaM#UawrQ2*p*WJk4$;s)2jT5O6h}1U&b1L^JQn7(evEXj+DVcS!r0pr3?L$P#c#s)P9IFcAMn% zfTTB@N8DfbZKZC`@`#x4i;4E+6iy5|GltL4@5+a92Bs)+Y$`ZDQiErWlb3HQP9t9n z1Z(Kzj+HT%G@ie!G~`0%-ZzCO>D9b@Wrp9WO>eD|=)8i$Mrbt7#-69g{AA~?{7_+H zxj@`H9ZA^KrDddhKpqa{(4Z?mah(1^pGUF;3_GdxJ{+aOm%5-PPfYTe9BeZ?q^Kdf zKWKR^{oP&XVmV0pA1&=RCp6De`%4ivzZi$;_9sd@Zn2ktfB$MrA!*zp`<+A5JT-RE z+&O>d?icmb1a+S+`|n9G3`5|YAA;+;gSxP8(;5_`-Zoi;Fdm+z{y4J z52wb?E=q_dg)nLTo%)B+qpntZjDb;&ixD%4A~5@M9aHm}W9i!5*9*?%#3M>&A)&6lIDI{m`0pz@%b|=!qy6$NLa$#P z4l{j5KJ`F;4~fEBODIBjOx51Urara053{c2{eb4NSt)3+6n-p0sqQhmaZHe&_jsF@oF$QXl^J*SAHxzZyF!> z;?|S&(72|cpeA~$b3suu?-EWM@>E>Bo${tm<}o1?%M%3$__pCee4WUCPC~I+mPi zr1;yzg@dD~(gAGr9FSr`vxK-dxxJB9URUSG!BHwnyQaGxm=(0#%h9x{l(4<#Wm3O@ z-xMS@wSX7h$N+pdTVq+*k&NaO#`(?$59EhWaO=M4k2g6(u+gRYXvM0ZB&w1Kr=~j< zn>sr;_)If@Z=ueGg9q0;txg=LBSegfF+>Se7#!8sPU*s-ncR-ku?^jq-7~o?9<+QV zs0Heuu1o6{*wkNF;O`qtY}*%edLHgL=;O_|7HPL1BV_hQZ_r;g;J;&kY^?ryJHEZP z^mu#YXZooALn?Q4wCU68ej0{5|VhJ;z6H9}UfGajj`RYPOZP zJt{kV;L!H^_A0;U%t-V-c4})v86TvA!w=3DlNaafnqZOlSqwc~7uaxeB-He3)j%7i zAthR6;<8}FS93Nc>Z8dKW}g&lEGq^&v9 z#2wOKrVQ1|=f)ISQsz=qgU?$*Som?Z=$YmhSDLwp+pAH$mPS5zZzz|aPYiCBU5j=I zq})2=uB|-bZ6Ge!8MNAed;z7pmSGYU7F>K*fPp&LSqDBJ_`JE+JmPHk&cqMaRq3Pc z%h2aLg#SBs?AW)TKXJxRFi{~UBST+j7>m)?=1b&T@!f}sf2#Z*Jd5Hvf4(V8f&ZPn z1hht%Rg`C4myO5^BH3thMlk$f7;N8xPS`MEYnA}o7Q~C5lEL;D3n?e)Z=Pe~(HsBM z0t9_TtgvQQBoL408m|Rb()v#dSd&1$3S10fL%g|`VOD)nH)ur{xmNzxU{>hiu>O`S1Ctjjx({HtlJU;2@MjG&D9 zk|XJS$p`vYx|D}J}Pe$R0jKUsWmhiUUcwU~cK`j?U$ zpJ)66?87&fg8Z3JMr?0gAZUC^JW{g-A$vs=o%FJA2KGU$)UWP|8I9*`518c3!^*=W z2tqP_CO9+KG+Mo-z>c}Ek`|~&bNxh*J&#-6CE8q=u4()(m7LH|inBZk08NPIB2CE6 z+>h2W%ficRkBm#!zf?BeSF1^QYIzb)e$cwpHvfOfoI4K#>m2nwxLb}OTzA&yto0hQ zP35`z5V!2wAz5%GU7h!8isHn8qkZ54B{0^iyd7q&KeBE1uq`En1MlqIT!7gPf2X0P z?$RhW+{VO(%UeB@2D4w?Eih;A2t*{D*GMfjY;IOt9jJ5rY~Q~1A6@jo%n_1jbu-wwHdZ91|GH)AI(_k8u{tvl z>q8#gBinRkm|U{3*x$fst2)bfBzV!A6O*mQhoowzTQ9K9-csKmb*;Zy57S_vSLAR( znGT48lG&j z7Hq6#ZHpsE5Ls{lqrJ$y82Cd)lmti}T^j9U+B|Ycu@W6UurIc0b^Xx7Q7O*5bp^|U zjips+=NU1HK+hP;NxX#P7Oed4kln&>KSj)4 zcKmN)VLhkd?PBQ2%JINlw03ydrY;0Fy|Q6!A{gB>ew&@pJlZqfqlVfdjSQKd4J zy?d%#;b(2bGsz>Xot{ey92$fU|K-F3PP2*dJmyS?hh#D1HVPIvZqb*_C9eK*W3p9+ zY}x);Qo-f^YC%V#@D+nXQL&&E)nsN0U+3UM6IRu=&ruG6bNx6QCr;|-bAWImwLe$q z%pxRkUM%ltAh;b09{Pe$ZIvSgG0`i7T18hxd-EM0_uj>8t8zIz&-T!_7pIazFd zK{oJlM;Q%XKYEEwIj`J%jp*@x?pQGyN~gtu?N^PDZq1!g$Oa$OdY=T+Mi*%e%qc)( zDEo^WsFfYVU_h-%7}{8Yv41Oc_rV1-m~{9)CyJ1_$c7Z&WH!S%f3p&t>4e*jw**=T zneyAu9IzW9gfDulhX&b#{3i*W7xZw!1^$0svI|}{2ct2CSN|@WR61kEEmBPU8r#(z zY1e3FrAK17HqKaJJFa*UF2B{scoODqjoC zLQX|XH6@2n%Y2O6gU&;Qdd{^X21a{Rkub2w}O$7gKXS| z!Fy-1F(flJElV~jG#Cd>F^OF{&6U-pZ&v+qHOfRe&N6~9h)%HTI5>ZxJ&XR2GCzLV zA>m4p=*U8>(bm>Cj;>e!&WLWWbPO;z zh{(|u|JNe)Ve+#3pdQgw1L_C#Kn1>}rxFd;oBd7JBrU^CXE9k-caTrl_CP zBNA}tV^^2UmW6P0<(V?{=5J>42W@7sy#*K%he$J+j`uJMev4l&p2UpqOE3Y>71?x} zwY@P(m`%tp_z3*gd#Ie&!2b0k^yecA3O2xFT?Xj3_va4w)Sq3hGnt`lJsITZ^p5hA zb#a1#e`*`U-u2m%U=_O_w{bGi$PoC7kw11#vbOMCO+WA_|Nxq zHLchk^ZI+lRxz5iHNqbm_LP98ZVova7J(bj*D82vn<=8=j0#`#S7HfMbLZzy{M<3_ zY7}?pxuJmhK-5Y`E!o9MHObA!fVD*kq`WrY`B29W0BaXGLN0YUJy3a3CBe5rwZWL1 zrbWf$x5tcKe!)zLg4uI*vEF;Da^T2N5-?|noap^x+Hr3S>&|bDU!knNBj7hy-&)z@ zUy4UecK8_Er1_6O;G4t7o6mu$CeQ#4fP&9x9F*c=`|VlD{8<2 zQMHaTD}D3xT`CNAKFsDmeN5J-#?UcUw#vW(5N#@sqeC2p=ZZL&%fVjHzQ zk15pu+Gcj&wgRkX;AyoHY4*s7GX5EmUvM+8yrO3B-2u_?@bzmw1+){|5&F;uSA>`5 z`$9fihOT?s-xa+0BC*2~aZe8jKZ^bj^R*uiI+T zt8tkJzs)nOnIlOPOA$?53i?V@?drKvWpy0Q|%84$fa(UQ-UeHaxxKbh%U{b>7VW*|+rSFtj-ntrE(7&|De4$No;L$`?zQ&x3@ZxGHA0Hl%rC2`KrEl2 z_*nQkc+2J022WDQqmYn*W5@o$@6H{Ze}+939&Y&k*-NqnAW7Q_B?d(lE2~gi11aQo z=ryGD%=Z-5?H4(++DOQdtXh(yd_?ugJS0oNeJc6n5dzrrw3B~kyG7^l@Vne^+MVOB zT7256T}(Kv*}##l`Gr6wx=`Ie0^_iPrNde5nL$x$UEQrz$BU?A0AZOM-bB0hn?Y&q zZw!8b!l+|1f20NFv!Q%m)Z$D02*#Yhe2$VR0EL?=y3tQd6i%lAX@c6S5m^$Jg&sTM11L?k{CIj?m&c3~aXQ`ilL{>_QEU}R_! z+6x7|2Dm9fyQ_3=e@@TTkH&*X9-K|Q33G!`1NGQEob(AbUCh_zgZi2Z!aKhau@vDD z*fGKw`b${^XQN5w2|g3WV)Cn|K(<~q$SjI<{LZ8^TYt4xB1W%>)zO1-EEUJ{+E6}= zHH96U3K={in?*kfIZ^67|IuSpU+aB?ATUD!&FKUAIL&;pXM2H-bP!yu?;~xt9MIfh zHA3yqXA!{o>66DkBO(xC*EAkE2s)$E*sC+Ir^SBsFft^ziKxI)O1ybgyDNuBJeb{o z_S{{FxnM;TbzVyw99w{Xs^s6AH}x=aDScWpu=T27=E#F)s~@A@QfDJs1$n^x+4Lz; zbRsI&HHIU#0+kJKDVx^=Z*%UPF=zg<xWG5K0Jbsi`*8y7CI#wvWZ=4I*{oof=w3o?#P9S7F%YHnJBgWk1gW-7L z2TFiH={oT!6l6H8W?>(IEuP3lsfETm^)fUJL%@ut{`p5Oh@;}DCpc_7l{ zzu}AgFs^ec>R`ybilQWGC%GXYMjvML0HU9r|M}MA3^1G6R651u`biGW&rP4|>FnxVHFqkq!XpY@V^z)yHjS zE>`5@KOyi z{vvu#3^e<|u{H5#B2VPhN-f)|b6n{eE7$9$A1ew9X19 zf^&0cJU#zF(}5;DB+7WE;t9bH(vNM01hBeboW%@@2uLZBQ!0P{a6+Gh@TzM3`Yr=# zC^0ZF0I4qN)>Fdd>;kEcve^~-5IPfjI*cu!`UmRXK`6i7&$3?svfyKPH<0m(oEZ2; zkMQQ%JQg*E!mP3S0+82wr|mhQEC2ABw+@_n9Xs{HzQ>j+)9&&m#Pce*L3cGZ_1L)N zRyjiV-D!1EE+Imk&YG6kjXMJA=#D2hEtTE0`7d+`!xptm5(_zQi6QLln6Q^fmTrT3 zT;G1cN@z;f^PJfpnidFbFoCNlkW3T44-T3s3mq`vqh;tjvA^7|Y_9Poem{|Q-T#cf z&I7A*j7*8ce5jJf#`C6a5fx1DlVRt#a8wNidZ*pNzn%OuD$gls-)^X9zM-K7UR8ZF z1)8EA!iL1b&=#J^`8owqfk%vbH-vmPs%qvfZ)hlYJnJty+Lg%!AGh&qrKld8xh}EWR`!a?-33Q@fGwd zz~qXkoQ%Ehof$rwKL4v>(g!X*zzoF%i&}Y>h3eyRaLq=2a*7(yaDqcO_WzMz!rFdJsh378w)TM2-N-GP81Et^PfR z&i8v_N1;(mU+Ag*B+6I9JMr=}O$j3roDoE~H%G>nXUx0KH9G1He|o0Q1KL~=4T7f+ zn#ntJ{kdONUL1}W4n?`2_pcvrHK>I4vF9P#yQ#RQN91UW5V6kh6eb?P3{zBF&Cip+ zrIO!0Q$yh)ej+&IXz<&hnPxK>v{2WZE7sxDb`{@Ga61_t_N{XBTb)jpl}r}oREUG{ z>DtUrYHMF1Y+)tBy2o8@QdLX!Aaa4VRm>12BQ}U||pLX!F zgh3AgE-Hf}oC;j8K+I@*e{o-)c)z{~%|2~yScVd6`oUeohtHEy`*Q3Km7bc!O``zu zG(YMlYa1|UQmhV<=E~*wCVkQ}1`j?^+|6B=I67eHJ)U3(S*OczeAwCfIXty*^GFN( zo?($nU^6ht=uE)&%yc0%fgI+kL{F)J!5K85<~#}Yu|Hc3&^mpeA?yKpTVViR_DVU& zG4X~SH{Fr_3^0u;52RB~C#>#L6F~)jahEWa$WSlR$tRxHOKS6%F&9lM%4!Ms^4g8Y zs)|W!@|XzEy6EE5)x`=`ex3d@byMSe=oC4C@CI(avdYVQI#)X_DJSP#@Y^OPIgKPY z__A#UU$oK*MGZiG0Cz{k{wNC4{)cyQMjce?1p;=sjoAc%S??Tc0&cp(CIxT|oEpzM zc*h5ml=E(LB`oFkzcXIZOoxbMQ&shncp8!{Br|Mk9IkMZefRA0RTw0~+Pw(Cwxv%Y zvxYhd3i#?%5^=on-o8~fV6RG@I_ff5e&Lk&X8^QGmVkANbWQ`$tIGWdpL5G^TbM=x zRs^UBRHa_u04DZ0l}vc}LES&p2JxIb@cy%XC74tK_=e~AMUtvoyhDir>&dWoK#JfP zeE`_Y&CP(jyt{8lVy!-GVf{ygALZklRPhvx((>|-WkSYwYP)3fCy%>R|N$<Vx?*GOdW-}NB;66RsL1(6K;i$EJ0tQq!Qxk)LvjX3eIS@-(8 zBJ_C5a5(8c;ebChhn89TZ4Ll(lP!IvZ&}J5B zS{KQd?si7;IFzHSjcRnO%w?I}a=UMM^`7?V!XXTO4jJ)>grUe$u}-UAZdal*(F5G` zaM-gLVGRZu#^J?~W+6@*1w*3#70W9xwF)3%8xq0CPAYD6rH`dW7&^0BmaiIB>-&d>RKgOPo zF$bMQWf7nUn+l1ixOUe=a&&Z5TQv-1pc?Yu9L|g99IY*!9D?3yzJUz?zlP`@@CBC0 zV8;}iJ&=D6j+U#9v1AGBvO;bQJs?k>SIwpZc>r9YkRo~%_Ze07>M@o+!t_4!H*P6I zLq7$G{xyG3t{}BCbPl;tBMhex3gjIuK3dzISg7~^zvu+`LcAZ<&UGu&$?Lb*Msn6Oj8S=!Lm$&xi8PS zcY%eRH9JKE3b2fH$Jz&crtv#aC4k%C$6wdg{}GbeW(Q+S=$Pr zr+aLEmmapk#J7)Iz}@b%1>dM8spOWHkPDaIFP2Fjnr~$LH0=)MFWGKyj!Mh#6NQs; zuonw3HPj{0&F4+aGZFcBml<@tzTUOA4!={CX(}WHc$l;i2$N%dw%`d}-v0>3#fR3D z{botp6?&cUd;)6m^rArgHWiu*n)QJy0md)vMa{)f4=0!Y+eJ6cyy7ukC4noqL{!Y- zd56Ac6u=wA1ijN$lup}k(;NQV`x*JIJ&>S6~ zefh#0riqia0s{k*z#jakl*5t9J>Qf8v)#&dgIrmJ(RG;+xori)s0HgEXoQ-wfRy3d zo}PWXD1th6%A(Tsx%z*c{-l;=DSr}o3@NL5WL*Zhrh?QwJ@n^k`=Y%3&esm>BaQd$ zVD8aWpV8fjI|k~iLvlkwJWtjefp7>ly#X6Z!~J~D@aT3`KcM`JRkYI~M_->m)GP$_ zu0P4|?(&58jira#k8LYJ3JIm-RjRg2;D!$6gl0a~jq2hScZs#uoUw#6?-P#uulQz& z6bZ4$>Y=X@KA!k@tMbHBitJgq3JUCF1k`YTQZytArgxa@=@;9}V5v`*17D_m=dO#C8@eq3eW}u8OWcHOp}XLPYFI|q>cZ(T49qKL5g(2w z^nd(g-lY?h{g5Y;y5+G!QDsVhdI#@nBj_e37ZVNH;yL38c4^K}33i;APMbHWr5l(N zI;>-y=!skzr;ty%!H=EO$B-pFvn>la+YRa zHV>2OanQ5x`1bO(nMFV+i{>vp(L?rLt3ZjBK1Jy4OZk8zAVeEIVUX1})z~dY^0_dd zxgKg2u5?Mi1G@;+D3Rd+ z1Y?iQjS`|Hh0tIqATA*>26Z45weV!ZINY%Fy-~3j*uPr;Zb>MFcNn?=xd?1I1uD6+ z;cn2jKrz|)Ln=rP+shv56t;c+ddwNRO2H$&^h~-0MVxU-kE7RP&XgTSvHI}}ijB?9 z-(R`B-^^*@094S9RUO_^wSVBY?-H06`tZK^;V}Y&8cfS>^>CK}x`6)%=NCZ$6>rIl z$6tShAB{5pexhgE9>Q4rII^-QtmR#^^-nq%m70DgEU|j^|JmAhX|M&hf+>!F1MH-&q!}OCN z2LSs7!49a6InT5uZ-gG18qYX{iHG>Blyg7(B{Z1=o367Q;KaC2XWmM(w9tn**yb^) za&BJN7z;rG*pOr`BmFo3(*ihl#TIJuMeVW@CeW<9kjPh10Pm65mDCdM7#t7>QXQ2O z@fItE&;?25f+aZbhRo)tG9DNTgj?!s6<&tI2VV-5qiMCTK(e%rr9NDZR+nq3T=oug zU}q7>39js3`JS?D(1Zcu4hR|)HU3eMFwh1vrX)*X${QDed0VVM7)z+@{4CN|$xL-W zwwMfT1-dZUVE|xa*{p3Sy>^$mKFkIm4tqYjUKkixm?WDebAG(6y^LnS^G0s~gJP~h z5pqOFYmc=IFDkgz3rZ?#K+W^T)}z^3yPba*5IcaqH&B0i$n5Z;mU~cAE%-6(l=|!F z)ae#xh+^V}X!f-TO}SSBYm+%?JaY!!A_Pbu0&hI0l^AMd{bwQ@rrOXdZ2?4AkhWWd zzV-LNqXbMW_vi9F^rchOfawzQ;Gd1ZHLu>&NA6x*AMOM(z*uYbHyJV>5$+7FZz&2c zC>PDQ7R_rFM>-q8!wRHk@ATN`xetto6(NO!vP)3|Qs}h%-)p|=GFad+E1BWk08iLv z2DOv35!grz_H}1xzl0x#s8CzHk3OOOY0uPVq{BbL5fje|9F01Ura-br--ZNA6@o^x zFJ%G^=my>{j+lc@eOVB^ur3zkHthCn*(Vy9PR)EM^Po)hmTIol^^z~SG+<1JKc0%a zjb_4%-r1=i-!0ci8MA^M4{Q!aGDK8fBmJ*K!L&`!|8a1dGGcg&g}-t9tNB@@3ctJ7 zrhV_Nfhc`GI1h&cOw18X+pn$Fx4~7 z3M6e^BD=td$|KzLR4uegt`jchWl zPgV1X1uGv$Q@gCt$?7FUppaqG%j_a|WVRcvb>U(T;*_0;1{w&P`YN`~dUC75B_(6_ z^T{wd2rG7WM~EKKu7J38tr=R>P#A!IM>GG?WDV_QPtDyhqxErI`$9syg?wse01Iq7 z0bHd5qbo{~Yg7{K23uEMivG$Lr%G21I6&%v`vZ@Ifc9WKClCN|P4l&g1TIUVsd7DZ zv2x!^!qB?R`Z@&~udr0G%2E0j>N-$$l2y;3wg)ExOMOVFdJqWUgV9!u3hW+eKK&%K zRIW1gr3}>@y}y+U?9XGkk`qAV0eBHKM=q#dvWKjDjq$43fbsq}I)%|#CYvhBG#zp4 zib7CSg_YWJIp{Nf78!?#jiBR_AR|SM8&!TZuRH^)s=Od(T+>ia%~aXvf{BNB23ZvR+o8fg&d&oz8Tjqn&_(;M7utqz3>?rM&6EHL>Y1r)ZPxERjG(fO zE+#`GFo6&-zYL&f{%3<5*Ewqb4!Gc_ReHN;Q)Kx^#H^qKg)33*v&>Xd{|h#CriX-V zUgEXy5rGAhjxgz^w$k~J3~+!lds6jUVIQ0>a9g^5Jv?^B00OaRokFP|ppw9qLNCBJ z`Au%C)zDVy&KS^w4ZMHhWt`pJH@@wcS@8`~%?l}*0L}~g?1BP=Kgfj^Ams(&BJelR zoj+^J@`=2{YIj|y*WI?_6aWAb6$H`&Az&`c_KzVMUWYP$9Y7;2nuju6{10UTdQUQ! zhsIsSq4emog6S*>lrZo_2{rZRwO%#8?1H*<>L{Er(1tI;&M_vu%amsV0syd9#fP{z!*c{l`AK%{qCorA)}Z6GELY)dMX(ZJ737=IZM6w_%VVy zk}Z^~2B2dGBqi|r?<*acqSp4U^CjK1goKoegU<&E6N*aIb+dy@G`ZN1&SJop+~#A% zil#xRhDrwF*ZrXGGCnA6859TKC`4vYDL2>sdp94;`kt1xv;;tFucPeS;9$-GXo?ne zT~a3srsRl6L61FjH;d=6pknzj8BMdVS%^Ibxs=gL31+3xxBK?!LxN?BW`4b6Xc3SD zA#z#JoI4Be9G($KI1CS}i^H+0vkh1_rN$q$EsQEsSYUu_3Cuu4kSjkyA(urQLkxczCqWYMBX ze#n-!3f9s%%z-)=$TafG>$$$w_T^e;zTIaZRy7DCjIcmNHZZOC`$~WNh>)0AqfJ?! zT7gproHzF4`1V?&v2keYt)Um#HJI~I>xqCW?bq5y%Ajoh|4uJd(2!P)OjJhH85l_v z78)JKbkJJ*HpCPZ&>YwdFMiF~-EG_Y9M~3U&jquCczytMU~p{)=Rvm|A8I4#zGtUf z4@;V$gt*tbv;;C4?_SOUA(~%n+HyVZ0X^ma8gMXefL-k4_|9_yJ1PvE975c+PI=~{r#`J5$kxKM2Bx_rIGC!X`S zuMrS(eYWKl6=1{+vCMveNp30qrETCPYkdvZZ0kir5QBZhsj~uJvfHL&Cjv5uGR-rDE|xJP+3Aq4(YeQ?Dy3r{rcZ!)9TtIc?EoejjJaC z!b67+j{0$VRSZ!V-v;MGL}l6&fGjT7zsb4uX}Mk}Km#Zc;w-1RS|3{fUWL3J&k0cv z=vELME5A>xRF54QI|(EuqzD*flYn~(BWsbR{z&(`O|NSq;2^qxC zgBmIb0l~w=_ouQ}pF)cbqz>>nHj(^h^}n-;{ErfW7y)nS-LooeGryD+CLY@JFa!eJ zEewJ5*m%!KK4@HUaehsJya8_#!Y1sSWWB(%3(V3%&PZvvv{DVD8X)@KX|H*TgYLjX zjrnR8*G?9R1_K|i6ON=nO6LNdYToUuaK~MiCoa((kUkLdE+sIAtPCUTRbH?c8CsK& z>IiQVRanA)G;XrOmTb@_gRJWvw4ny117F|<{@wF3Ljl4Y>6@uskT6hqoI#Px=zA3a zHX^5_e;xnP+bVJd8IEzgjB^k}AwrRrpTncDv8~^}Hsz~k$rVxU8Bl?w1T~W7H9#_P zxWeF!q6X+Ctb|Wyti5>xY6*lZZidPYwujq``pnlbF9yJP2t{4;_wGALR!zl0FpsCX z;#CPeBrMm=A_(~UB!jC@woYLV_LK|C#os@4vIdfWQichmYbx#q_9p5&IvMFOz8N_4 zpKn9)RN?9#(B}x;0cii4Pt*mKCiLh={IbKkyXN!{^2(Gur*iq#!am3)hQzg2$G!i4 zwGQe!w3O`q?qiu=zY>n8D(~+-!*Cw+UF$RiPhc-tV?{3|Wm#OrBw3=Fj*N)G=nQ;D z80myT9U)ZaR?MKbE->&S22-^$WC=2B_;5{lOYGqmXgpq*DFF%#D{>01N>|MC=D&`s zEw>~G@ND^7P$)wQg~I#1Wa1!3fKT9IMF^p(xHwDR#OBo3DJQ1l@mkVLf0ZBD$R5TZzZ$stjF-L z5uMEgN_Gy6aFV6Ngq$oto&Z&UVIL1etsYxg9GUIs-DO4EuO zfQp-4X5DyMC61V~2qO)OguO2$*h>usO6PVtspdWyponTi;a#AlN{e#=_9&`tP_kns zMwcD}1V7(AeEmE|zRFLzgc~XTZA5m#WN|S(I`CEaudol-XD}F)rkYn>?3Np-2UJH1 zpBjH**`e_A{#k?BTJabVChXvYk@wMepyLnlK9SEpjuWPoz^$jF4DK|U>5>-|HgCsk z*#3&itp4w|8L-g&eH*V+r~uY?qy+%(0-G_}=#Dh54#1*2LVhcW3_848rOL+y$gT~> z$q<>KBnUJ8ME1DSl_gl7F!Kv8%8r7dr-!4mp?WGeCXshx=XehRT zyf~=IfV4F7%J;v8ybM|G=G;A*7$F+iZYGN8TRlL}27vXrVzQTRsd33plb;^$Fx1i6 z$?nDfD)z)a&O^yJb8qJLn%dcg4?ACNBGpahIzV9riA^a7Ba>w<#ai0bL~uy503ISk4g!NqzEFDifwS8L4pU8*Kxh^2T_iJ?-pO*#N^?fa279gN!WHhbOwMA#u{86g)F^7{7VA4+75gZ4dx~h9+By3@7ZL={=$c(XF5)NKA@j_kBC${*%PVzW~uLz`J%E z5eslFfW^^$o!BVwrB1a~ie(k^Q=?FN1TYC57vBbuV7LSW4PXu}288scc zoS~Y$8F)RFYs^PU!rdN5_5)lhy$3TS%u|^%Cwm;Lcq0J|d|2M- zm(KJ#OivFRYU{-@W%AnEQciHt@_N*%gNF(i4i*&Lgn=L-*c628w~yz%pi{``liARo z^D*3+C|O#-ck1YVl}mtmKu(gB`-(^|DgKR34Ff=oQ!MP0=xhlrD$i{S3%v&We4R8I z9z3y(a2SAw!CfIvf?H(vYBnF>_+}ShphUz93O!5~(-0myuXpAu^2HM;j^i8gG9I}b%Qt}87hQ5dIHR7re&x4nX3F^ zyIrZ+ySv~nY}!-gb@YHOFisynQc#;HHNek*c>VyY?CIPNN!^IVAKkt25{PpeC- zW0iRmiEZyD{AA@E9hFRSGBf=#*@T^IGI9=lp92EArl)zqOO51YeK@O!yuv~ghKxf( z@XjQyR#(~VC?}Tc_W(cy=`ct}3bjE)gkL@|>nSPYzshXN^5I;zAUksULRX`+Klp5K zNoP(jfw#vp3x_un3)+c%>_j=FlZ_v}kFu=FUz#6`U_Tyou z@wh`x8^5-{El|=k8==2jq!oLgonkap{->4^p(!V1$aVr3Ljt%AS%E&q^OqU zp6_9ff6N0rz5lCp81>l)IhyBdebDw1?F#WX&Em4Mpdf^XAD}pnjxvQ3#U+s)&3vs< zbQwB@U=0&HR^_lc^OscK;59cLK7fnp*&%(%O!;1@Jikl=2Aix8>poICXGl~a9!=_~5itwg+cHc|SmudbnLDzaUiRZPi6rv1n4DTr z@XfKR)9N-%S5(e9`~Hm)i^0TSn@N4A36yMh3K%$Lea~S4;Ai|R*0tO9($Qg& z#bkJ3Ec<6Ee*QsnD7=YC`~q&I5Sk}IjrZ7e`ZjQ>ocJZRJ=PRt_b&(nZ9Z5Galj@O zO4=uuJ}+4VF`riTl5%G~?-&wWTzvJC_J48R*tnY7HDO9}<&tjVVU8g93^gL+eN4Iei`zi^(ggYyPVG zN!7(sWZ##z2~q?Lu0?6!!%+Blm1ltb$EMON4(o=ukO1bi$(z=BV5=kn;Q;DD6J4nS zZtdJp-TZ$0&yHTeNCWB;(F(rX?!UEoDXD7UFv)}-5B2qNQqZ<^$2dKS!W(I5P~yKw zTN_*P{rCEn@=~w5Gr*b&(Xeh<1Ja4-gcxVx+Ui9LxXz#mVwMzK$j+57QO@ak)^E?M zR3_JDR(iL_Q0gz?r(>0&iOC+|yg;;}oJSnimv>gT8`}dCkOyi+8BMU0vPu^z>igl%kn* zo|&2XmCjN^@pKK;2I-lZ!l$BvGb=BrRrBS3UBK7fEd%2FN(VPr*XF*y@wECpo}Kp} zmCJLu!8ogZj9Od!(B3RsHvf;V%l0u|CB*_e@2#OfM+-z0L}r0ZKXV z*49ooHf{8>CX#L%5vlI0u^fP3`3rVz-)0a`PEK|zV8iWPd_mRn{#F-Xn(%X{qTkQu z#62ze?(ZR)hSwxshOn2E`+Hpggd8Zgtk-&-P3_X{lQ8`SA>PG^_9a9FP{?}T*)&y{ z%~K~{_Aqku@o9%a$K?a?{pt*B4wqI&@{?Dmc8|XXP0JV*zU~r6@fK};ec&TpIv`E* zsrN7{A9RGdHpQBgyjN0`|NE(>E~1jK!54ae9HE30Bws9 zWRXzQ)h``>TDw`Ay0#e7Tq>^IxWsAT$&*(^!MyD9fjLJLGLube;L{EIusztnzOfhU z1_d@7qfA*C6yLelNOd*`@eQ)%nP?o^@wZaicy8 zGpCw`yuad0vh2j_4>frN^o9!&$`wvOOD-SW)T{eW_i6oZhEJH+x6evrI*zjJ2h%$%@mlIGlXboP5%f%T=(i8zewfhB*I zeV`=sJ$KR;G0E2huf@GYu=1F22sI8n7$P@PsacKZ!x?J^*)_Uz_gGJ%4#s?odd4I} zEWoj3G?;zEuiL_q;fb_t*jhj80@U01ijw$IklIFRk3kq%^vpVA246bR_Nd=k!CuFg z>c=u*gj{fjJt8V}WNLpXKXrhPYX?FKjT>D}_k0b(x`d{XE3_IB_r?Dm1;uoiqu4xp zPhCp%iW@QlF-JW*-=E(n@i;aWr`^$hRj%;IO7yq6uHIdLO(|59a#^br*$^HnC~;(i zN<~uMLBDWUbKM4zhFFHV(b>MJ$Z?0yZ)`4&=G-DM;hbPv?4H65r*!z;{p@)&p!lNB zdbBoHEQTeyJR_e*6F&|AnQWt}+g>SO-Wn}zxAQ9F3>U1w23SHR!41jGDGGgmgw|7~ z=xUiG6$~@;2JkrM+GwY8R|Mrxcg&);o9AwX%NM{k7c9B=vorif8^#{x!Rzlw>SOy$ z$u!zKP2ZUS(jp2r6QYbYb}ytwxa9l7MCe;B9V8#KSwh%O&^8ke_CvKFkotZOv? zUm>=x=9s(p#5Xo}q~7a!LsP}o@@h0Sf(85K3sBSDxp~&p8Hb2#xZPiAb*7L3HhpYQvzs4+lA6(+VbzLz=ItwaBDz+Kp zec37gq)%%YjoTMBttsL9_;$TR<#Z8vSJl>z_Y*lgxH&s)j&~0z@^$}%_W)(fVDq;Z zRu|J6VmK5k$u~KC;?aKkgQrht>)I)5Z&YmTC-0U1C}D=_`yNEK;7?$1_1T(9Q#xOX zWjkrC00jBptLn8@$8Mt?sM}NzBRRk-n7deQNZebV;Yp}`b**rAp{)@XOSoHLy^B{F zgcX1ObQ=*Q=P1jEtA)1nfOLVhfs2Se+N~CF^T~{-Y&TKOK?k`!U%m0S-FrwB9~$j& ze}{6jXd3LWuoxH?f(Ie42Cs&PmDH(_P*-#Hc&~OVjkc>ehakd~$Qx)rx0pLXsUS@C z$ZEvm+d-7d^JNaml{IwFM8A8({!-iM<#X#MV7bQ*_* zb*myQcQRy{$>SKFDZUaD@4B>_FR@1jYYxMW4_gXPKV{kf+p(2iTCX;C)s+&XW8aoHC5 z=RuTa*ye>td9|2cI{yiAO!O>@!^t=ls0UH)2~jH8{&H){mz{cg`ZBLA8-JL#am{av zM9jt2`>m!gK)c}kX@C>yc!KG4&SLTX!Gc4ZrmY3ap%jOa0^6vJr3Y(DScT=Mo1-(` zQQ2CJ&DXq7Pks~Rj_ekdstw}N5$x)<7E!WQ)ODRLC6M-oD9#1 zo|S{_|dl zG4rlHSokFP^>||;EFd%*EcMkalqTc%X({sAr7uSIk|!d@DqE84&#X^07|A^nmbvj&oGvx>k8I$tV9`~QeMkmlG-A?tJ;pEakq$!i2?QB zQ5cY(k;Tjb4ipnveJ46r+IX3Yz8_hHiia+)pt0Iw^JHN-0_-ow_|VBU+De`BVxRcxd1{PE0Zsu&EyS1at2js z36^xL)u%gFPSGBg61gu}O}MqK(H^Y`G@|aqN81^w12GF7pNF^xJCXfJ`T+sil{Zv!+93zD7)B%4|Lt zfi2YIqj1XqI$)HV+YvhUSjvI#)ux(-8K{Dt{(>cqs=}QA3DHx$>Gp+|;KpFd5-E$j zYV=wE)k6q~swQ6csKEAeh5O_SDk6Yy;rn*ycCtogHBpsvjNzcD2xC!HcNMpUmM?9U zw2EB1e$(~n-A{-aU39 zRWUV;VGe>JrX;JAB2neWT3?3Dx_P*G#`FBDnrQQ;d1AtaY}n?i$xJKye3a~fxy|oS zLyiy8kvhRQY=}Lu_h?qDs@VG)DFP^nZS$ok|>C074YJJyo{M1+VZtCI3 Ni{i6IylL;T{{zK3R#E@} literal 0 HcmV?d00001 diff --git a/common/rtc_manager_rviz_plugin/package.xml b/common/rtc_manager_rviz_plugin/package.xml new file mode 100644 index 0000000000000..8af550e05ac69 --- /dev/null +++ b/common/rtc_manager_rviz_plugin/package.xml @@ -0,0 +1,32 @@ + + + + rtc_manager_rviz_plugin + 0.0.0 + The rtc manager rviz plugin package + Taiki Tanaka + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + tier4_external_api_msgs + tier4_planning_msgs + tier4_rtc_msgs + unique_identifier_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml b/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..001ae153e6762 --- /dev/null +++ b/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + RTCManagerPanel + + + diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp new file mode 100644 index 0000000000000..52bb83d6136fa --- /dev/null +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -0,0 +1,332 @@ +// +// 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. +// + +#include "rtc_manager_panel.hpp" + +#include +#include +#include +#include +#include + +#include + +namespace rviz_plugins +{ +inline std::string Bool2String(const bool var) { return var ? "True" : "False"; } +using std::placeholders::_1; +using std::placeholders::_2; +Module getModuleType(const std::string & module_name) +{ + Module module; + if (module_name == "blind_spot") { + module.type = Module::BLIND_SPOT; + } else if (module_name == "crosswalk") { + module.type = Module::CROSSWALK; + } else if (module_name == "detection_area") { + module.type = Module::DETECTION_AREA; + } else if (module_name == "intersection") { + module.type = Module::INTERSECTION; + } else if (module_name == "no_stopping_area") { + module.type = Module::NO_STOPPING_AREA; + } else if (module_name == "occlusion_spot") { + module.type = Module::OCCLUSION_SPOT; + } else if (module_name == "stop_line") { + module.type = Module::NONE; + } else if (module_name == "traffic_light") { + module.type = Module::TRAFFIC_LIGHT; + } else if (module_name == "virtual_traffic_light") { + module.type = Module::TRAFFIC_LIGHT; + } else if (module_name == "lane_change_left") { + module.type = Module::LANE_CHANGE_LEFT; + } else if (module_name == "lane_change_right") { + module.type = Module::LANE_CHANGE_RIGHT; + } else if (module_name == "avoidance_left") { + module.type = Module::AVOIDANCE_LEFT; + } else if (module_name == "avoidance_right") { + module.type = Module::AVOIDANCE_RIGHT; + } else if (module_name == "pull_over") { + module.type = Module::PULL_OVER; + } else if (module_name == "pull_out") { + module.type = Module::PULL_OUT; + } + return module; +} + +std::string getModuleName(const uint8_t module_type) +{ + switch (module_type) { + case Module::LANE_CHANGE_LEFT: { + return "lane_change_left"; + } + case Module::LANE_CHANGE_RIGHT: { + return "lane_change_right"; + } + case Module::AVOIDANCE_LEFT: { + return "avoidance_left"; + } + case Module::AVOIDANCE_RIGHT: { + return "avoidance_right"; + } + case Module::PULL_OVER: { + return "pull_over"; + } + case Module::PULL_OUT: { + return "pull_out"; + } + case Module::TRAFFIC_LIGHT: { + return "traffic_light"; + } + case Module::INTERSECTION: { + return "intersection"; + } + case Module::CROSSWALK: { + return "crosswalk"; + } + case Module::BLIND_SPOT: { + return "blind_spot"; + } + case Module::DETECTION_AREA: { + return "detection_area"; + } + case Module::NO_STOPPING_AREA: { + return "no_stopping_area"; + } + case Module::OCCLUSION_SPOT: { + return "occlusion_spot"; + } + } + return "NONE"; +} + +RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + // TODO(tanaka): replace this magic number to Module::SIZE + const size_t module_size = 14; + auto_modes_.reserve(module_size); + auto * v_layout = new QVBoxLayout; + auto vertical_header = new QHeaderView(Qt::Vertical); + vertical_header->hide(); + auto horizontal_header = new QHeaderView(Qt::Horizontal); + horizontal_header->setSectionResizeMode(QHeaderView::Stretch); + auto_mode_table_ = new QTableWidget(); + auto_mode_table_->setColumnCount(4); + auto_mode_table_->setHorizontalHeaderLabels( + {"Module", "ToAutoMode", "ToManualMode", "ApprovalMode"}); + auto_mode_table_->setVerticalHeader(vertical_header); + auto_mode_table_->setHorizontalHeader(horizontal_header); + const size_t num_modules = module_size; + auto_mode_table_->setRowCount(num_modules); + for (size_t i = 0; i < num_modules; i++) { + auto * rtc_auto_mode = new RTCAutoMode(); + rtc_auto_mode->setParent(this); + // module + { + rtc_auto_mode->module_name = getModuleName(static_cast(i)); + std::string module_name = rtc_auto_mode->module_name; + auto label = new QLabel(QString::fromStdString(module_name)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(module_name)); + auto_mode_table_->setCellWidget(i, 0, label); + } + // mode button + { + // auto mode button + rtc_auto_mode->auto_module_button_ptr = new QPushButton("auto mode"); + rtc_auto_mode->auto_module_button_ptr->setCheckable(true); + connect( + rtc_auto_mode->auto_module_button_ptr, &QPushButton::clicked, rtc_auto_mode, + &RTCAutoMode::onChangeToAutoMode); + auto_mode_table_->setCellWidget(i, 1, rtc_auto_mode->auto_module_button_ptr); + // manual mode button + rtc_auto_mode->manual_module_button_ptr = new QPushButton("manual mode"); + rtc_auto_mode->manual_module_button_ptr->setCheckable(true); + connect( + rtc_auto_mode->manual_module_button_ptr, &QPushButton::clicked, rtc_auto_mode, + &RTCAutoMode::onChangeToManualMode); + auto_mode_table_->setCellWidget(i, 2, rtc_auto_mode->manual_module_button_ptr); + } + // current mode + { + QString mode = QString::fromStdString("INIT"); + rtc_auto_mode->auto_manual_mode_label = new QLabel(mode); + rtc_auto_mode->auto_manual_mode_label->setAlignment(Qt::AlignCenter); + rtc_auto_mode->auto_manual_mode_label->setText(mode); + auto_mode_table_->setCellWidget(i, 3, rtc_auto_mode->auto_manual_mode_label); + } + auto_modes_.emplace_back(rtc_auto_mode); + } + v_layout->addWidget(auto_mode_table_); + + // statuses + auto * rtc_table_layout = new QHBoxLayout; + { + auto vertical_header = new QHeaderView(Qt::Vertical); + vertical_header->hide(); + auto horizontal_header = new QHeaderView(Qt::Horizontal); + horizontal_header->setSectionResizeMode(QHeaderView::Stretch); + rtc_table_ = new QTableWidget(); + rtc_table_->setColumnCount(column_size_); + rtc_table_->setHorizontalHeaderLabels( + {"ID", "Module", "AW Safe", "Received Cmd", "AutoMode", "StartDistance", "FinishDistance"}); + rtc_table_->setVerticalHeader(vertical_header); + rtc_table_->setHorizontalHeader(horizontal_header); + rtc_table_layout->addWidget(rtc_table_); + v_layout->addLayout(rtc_table_layout); + } + setLayout(v_layout); +} + +void RTCManagerPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_rtc_status_ = raw_node_->create_subscription( + "/api/external/get/rtc_status", 1, std::bind(&RTCManagerPanel::onRTCStatus, this, _1)); + + client_rtc_commands_ = raw_node_->create_client( + "/api/external/set/rtc_commands", rmw_qos_profile_services_default); + + for (size_t i = 0; i < auto_modes_.size(); i++) { + auto & a = auto_modes_.at(i); + // auto mode + a->enable_auto_mode_cli = raw_node_->create_client( + enable_auto_mode_namespace_ + "/" + a->module_name, rmw_qos_profile_services_default); + } +} + +void RTCAutoMode::onChangeToAutoMode() +{ + AutoMode::Request::SharedPtr request = std::make_shared(); + request->enable = true; + enable_auto_mode_cli->async_send_request(request); + auto_manual_mode_label->setText("AutoMode"); + auto_manual_mode_label->setStyleSheet("background-color: #00FFFF;"); + auto_module_button_ptr->setChecked(true); + manual_module_button_ptr->setChecked(false); +} + +void RTCAutoMode::onChangeToManualMode() +{ + AutoMode::Request::SharedPtr request = std::make_shared(); + request->enable = false; + enable_auto_mode_cli->async_send_request(request); + auto_manual_mode_label->setText("ManualMode"); + auto_manual_mode_label->setStyleSheet("background-color: #FFFF00;"); + manual_module_button_ptr->setChecked(true); + auto_module_button_ptr->setChecked(false); +} + +void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg) +{ + rtc_table_->clearContents(); + if (msg->statuses.empty()) return; + std::vector coop_vec; + std::copy(msg->statuses.cbegin(), msg->statuses.cend(), std::back_inserter(coop_vec)); + std::sort( + coop_vec.begin(), coop_vec.end(), [](const CooperateStatus & c1, const CooperateStatus & c2) { + return c1.start_distance < c2.start_distance; + }); + // this is to stable rtc display not to occupy too much + size_t min_display_size{5}; + size_t max_display_size{10}; + rtc_table_->setRowCount(std::max(min_display_size, std::min(coop_vec.size(), max_display_size))); + int cnt = 0; + for (auto status : msg->statuses) { + // uuid + { + std::stringstream uuid; + uuid << std::setw(4) << std::setfill('0') << static_cast(status.uuid.uuid.at(0)); + auto label = new QLabel(QString::fromStdString(uuid.str())); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(uuid.str())); + rtc_table_->setCellWidget(cnt, 0, label); + } + + // module name + { + std::string module_name = getModuleName(status.module.type); + auto label = new QLabel(QString::fromStdString(module_name)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(module_name)); + rtc_table_->setCellWidget(cnt, 1, label); + } + + // is aw safe + bool is_aw_safe = status.safe; + { + std::string is_safe = Bool2String(is_aw_safe); + auto label = new QLabel(QString::fromStdString(is_safe)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(is_safe)); + rtc_table_->setCellWidget(cnt, 2, label); + } + + // is operator safe + const bool is_execute = status.command_status.type; + { + std::string text = is_execute ? "EXECUTE" : "WAIT"; + if (status.auto_mode) text = "NONE"; + auto label = new QLabel(QString::fromStdString(text)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(text)); + rtc_table_->setCellWidget(cnt, 3, label); + } + + // is auto mode + const bool is_rtc_auto_mode = status.auto_mode; + { + std::string is_auto_mode = Bool2String(is_rtc_auto_mode); + auto label = new QLabel(QString::fromStdString(is_auto_mode)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(is_auto_mode)); + rtc_table_->setCellWidget(cnt, 4, label); + } + + // start distance + { + std::string start_distance = std::to_string(status.start_distance); + auto label = new QLabel(QString::fromStdString(start_distance)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(start_distance)); + rtc_table_->setCellWidget(cnt, 5, label); + } + + // finish distance + { + std::string finish_distance = std::to_string(status.finish_distance); + auto label = new QLabel(QString::fromStdString(finish_distance)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(finish_distance)); + rtc_table_->setCellWidget(cnt, 6, label); + } + for (size_t i = 0; i < column_size_; i++) { + if (is_rtc_auto_mode || (is_aw_safe && is_execute)) { + rtc_table_->cellWidget(cnt, i)->setStyleSheet("background-color: #00FF00;"); + } else if (is_aw_safe || is_execute) { + rtc_table_->cellWidget(cnt, i)->setStyleSheet("background-color: #FFFF00;"); + } else { + rtc_table_->cellWidget(cnt, i)->setStyleSheet("background-color: #FF0000;"); + } + } + + cnt++; + } +} +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::RTCManagerPanel, rviz_common::Panel) diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp new file mode 100644 index 0000000000000..59f1bd945f0a6 --- /dev/null +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp @@ -0,0 +1,100 @@ +// +// 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. +// + +#ifndef RTC_MANAGER_PANEL_HPP_ +#define RTC_MANAGER_PANEL_HPP_ + +#include +#include +#include +#include + +#ifndef Q_MOC_RUN +// cpp +#include +#include +#include +#include +// ros +#include +#include + +#include +// autoware +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +namespace rviz_plugins +{ +using tier4_rtc_msgs::msg::Command; +using tier4_rtc_msgs::msg::CooperateCommand; +using tier4_rtc_msgs::msg::CooperateResponse; +using tier4_rtc_msgs::msg::CooperateStatus; +using tier4_rtc_msgs::msg::CooperateStatusArray; +using tier4_rtc_msgs::msg::Module; +using tier4_rtc_msgs::srv::AutoMode; +using tier4_rtc_msgs::srv::CooperateCommands; +using unique_identifier_msgs::msg::UUID; + +struct RTCAutoMode : public QObject +{ + Q_OBJECT + +public Q_SLOTS: + void onChangeToAutoMode(); + void onChangeToManualMode(); + +public: + std::string module_name; + QPushButton * auto_module_button_ptr; + QPushButton * manual_module_button_ptr; + QLabel * auto_manual_mode_label; + rclcpp::Client::SharedPtr enable_auto_mode_cli; +}; + +class RTCManagerPanel : public rviz_common::Panel +{ + Q_OBJECT +public: + explicit RTCManagerPanel(QWidget * parent = nullptr); + void onInitialize() override; + +protected: + void onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg); + void onEnableService( + const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) const; + + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_rtc_status_; + rclcpp::Client::SharedPtr client_rtc_commands_; + rclcpp::Client::SharedPtr enable_auto_mode_cli_; + std::vector auto_modes_; + std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; + QTableWidget * rtc_table_; + QTableWidget * auto_mode_table_; + size_t column_size_ = {7}; +}; + +} // namespace rviz_plugins + +#endif // RTC_MANAGER_PANEL_HPP_ From bc1448e3247dd4e2368a58a08538c7ad3f4f3189 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Thu, 29 Sep 2022 12:57:09 +0900 Subject: [PATCH 26/38] feat(lidar_centerpoint): enabled per class yaw norm threshold (#1962) * feat(lidar_centerpoint): enabled per class yaw norm threshold Signed-off-by: Kenzo Lobos-Tsunekawa * Updated thresholds Signed-off-by: Kenzo Lobos-Tsunekawa * Moved the yaw threshold parameters to gpu memory Signed-off-by: Kenzo Lobos-Tsunekawa * Moved the thresholds as a member variable Signed-off-by: Kenzo Lobos-Tsunekawa Signed-off-by: Kenzo Lobos-Tsunekawa --- .../config/pointpainting.param.yaml | 1 + .../src/pointpainting_fusion/node.cpp | 6 +++--- .../lidar_centerpoint/config/centerpoint.param.yaml | 1 + .../config/centerpoint_tiny.param.yaml | 1 + .../include/lidar_centerpoint/centerpoint_config.hpp | 12 ++++++++---- .../postprocess/postprocess_kernel.hpp | 1 + .../launch/lidar_centerpoint.launch.xml | 2 -- .../lib/detection_class_remapper.cpp | 3 ++- .../lib/postprocess/non_maximum_suppression.cpp | 1 - .../lib/postprocess/postprocess_kernel.cu | 10 +++++++--- perception/lidar_centerpoint/src/node.cpp | 6 +++--- 11 files changed, 27 insertions(+), 17 deletions(-) mode change 100755 => 100644 perception/image_projection_based_fusion/config/pointpainting.param.yaml diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml old mode 100755 new mode 100644 index 0be983c455dce..0340add0b2b55 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -7,3 +7,4 @@ voxel_size: [0.32, 0.32, 8.0] downsample_factor: 1 encoder_in_feature_size: 11 + yaw_norm_thresholds: [0.3, 0.0, 0.3] diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 0b8fcfb2840ba..d5c860a9a5fc3 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -33,8 +33,8 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt static_cast(this->declare_parameter("score_threshold", 0.4)); const float circle_nms_dist_threshold = static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); - const float yaw_norm_threshold = - static_cast(this->declare_parameter("yaw_norm_threshold", 0.5)); + const auto yaw_norm_thresholds = + this->declare_parameter>("yaw_norm_thresholds"); // densification param const std::string densification_world_frame_id = this->declare_parameter("densification_world_frame_id", "map"); @@ -73,7 +73,7 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt centerpoint::CenterPointConfig config( class_names_.size(), point_feature_size, max_voxel_size, pointcloud_range, voxel_size, downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, - yaw_norm_threshold); + yaw_norm_thresholds); // create detector detector_ptr_ = std::make_unique( diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml index 565c769a08c72..42a15067d55eb 100644 --- a/perception/lidar_centerpoint/config/centerpoint.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint.param.yaml @@ -12,3 +12,4 @@ iou_nms_target_class_names: ["CAR"] iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.0, 0.3] diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index e5ae31efc775d..b7c2faf89c0b6 100644 --- a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml @@ -12,3 +12,4 @@ iou_nms_target_class_names: ["CAR"] iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.0, 0.3] diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp index 4a12aee29061f..a58a1121fc4a6 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp @@ -28,7 +28,7 @@ class CenterPointConfig const std::vector & point_cloud_range, const std::vector & voxel_size, const std::size_t downsample_factor, const std::size_t encoder_in_feature_size, const float score_threshold, const float circle_nms_dist_threshold, - const float yaw_norm_threshold) + const std::vector yaw_norm_thresholds) { class_size_ = class_size; point_feature_size_ = point_feature_size; @@ -57,8 +57,12 @@ class CenterPointConfig circle_nms_dist_threshold_ = circle_nms_dist_threshold; } - if (yaw_norm_threshold >= 0 && yaw_norm_threshold < 1) { - yaw_norm_threshold_ = yaw_norm_threshold; + yaw_norm_thresholds_ = + std::vector(yaw_norm_thresholds.begin(), yaw_norm_thresholds.end()); + + for (auto & yaw_norm_threshold : yaw_norm_thresholds_) { + yaw_norm_threshold = + (yaw_norm_threshold >= 0.f && yaw_norm_threshold < 1.f) ? yaw_norm_threshold : 0.f; } grid_size_x_ = static_cast((range_max_x_ - range_min_x_) / voxel_size_x_); @@ -102,7 +106,7 @@ class CenterPointConfig // post-process params float score_threshold_{0.35f}; float circle_nms_dist_threshold_{1.5f}; - float yaw_norm_threshold_{0.0f}; + std::vector yaw_norm_thresholds_{}; // calculated params std::size_t grid_size_x_ = (range_max_x_ - range_min_x_) / voxel_size_x_; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp index e005be30a4c1a..d790e896dea9a 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp @@ -39,6 +39,7 @@ class PostProcessCUDA private: CenterPointConfig config_; thrust::device_vector boxes3d_d_; + thrust::device_vector yaw_norm_thresholds_d_; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 4593b6e4f5234..522b488429c62 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -7,14 +7,12 @@ - - diff --git a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp index 147fb360d7c97..4637dc5537f37 100644 --- a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp +++ b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp @@ -23,7 +23,8 @@ void DetectionClassRemapper::setParameters( { assert(allow_remapping_by_area_matrix.size() == min_area_matrix.size()); assert(allow_remapping_by_area_matrix.size() == max_area_matrix.size()); - assert(std::pow(std::sqrt(min_area_matrix.size()), 2) == std::sqrt(min_area_matrix.size())); + assert( + std::pow(static_cast(std::sqrt(min_area_matrix.size())), 2) == min_area_matrix.size()); num_labels_ = static_cast(std::sqrt(min_area_matrix.size())); diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index 82b9ca673061f..5adcd11078a03 100644 --- a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -23,7 +23,6 @@ namespace centerpoint void NonMaximumSuppression::setParameters(const NMSParams & params) { - assert(params.target_class_names_.size() == 8); assert(params.search_distance_2d_ >= 0.0); assert(params.iou_threshold_ >= 0.0 && params.iou_threshold_ <= 1.0); diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 03fcae205634c..8942bdd33ceb5 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -17,6 +17,7 @@ #include #include +#include #include namespace @@ -54,7 +55,7 @@ __global__ void generateBoxes3D_kernel( const float * out_rot, const float * out_vel, const float voxel_size_x, const float voxel_size_y, const float range_min_x, const float range_min_y, const std::size_t down_grid_size_x, const std::size_t down_grid_size_y, const std::size_t downsample_factor, const int class_size, - const float yaw_norm_threshold, Box3D * det_boxes3d) + const float * yaw_norm_thresholds, Box3D * det_boxes3d) { // generate boxes3d from the outputs of the network. // shape of out_*: (N, DOWN_GRID_SIZE_Y, DOWN_GRID_SIZE_X) @@ -93,7 +94,7 @@ __global__ void generateBoxes3D_kernel( const float vel_y = out_vel[down_grid_size * 1 + idx]; det_boxes3d[idx].label = label; - det_boxes3d[idx].score = yaw_norm >= yaw_norm_threshold ? max_score : 0.f; + det_boxes3d[idx].score = yaw_norm >= yaw_norm_thresholds[label] ? max_score : 0.f; det_boxes3d[idx].x = x; det_boxes3d[idx].y = y; det_boxes3d[idx].z = z; @@ -109,6 +110,8 @@ PostProcessCUDA::PostProcessCUDA(const CenterPointConfig & config) : config_(con { const auto num_raw_boxes3d = config.down_grid_size_y_ * config.down_grid_size_x_; boxes3d_d_ = thrust::device_vector(num_raw_boxes3d); + yaw_norm_thresholds_d_ = thrust::device_vector( + config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end()); } cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( @@ -124,7 +127,8 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( out_heatmap, out_offset, out_z, out_dim, out_rot, out_vel, config_.voxel_size_x_, config_.voxel_size_y_, config_.range_min_x_, config_.range_min_y_, config_.down_grid_size_x_, config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_, - config_.yaw_norm_threshold_, thrust::raw_pointer_cast(boxes3d_d_.data())); + thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), + thrust::raw_pointer_cast(boxes3d_d_.data())); // suppress by socre const auto num_det_boxes3d = thrust::count_if( diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index f85b9006a7dee..da4000b450858 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -42,8 +42,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti static_cast(this->declare_parameter("score_threshold", 0.35)); const float circle_nms_dist_threshold = static_cast(this->declare_parameter("circle_nms_dist_threshold")); - const float yaw_norm_threshold = - static_cast(this->declare_parameter("yaw_norm_threshold", 0.0)); + const auto yaw_norm_thresholds = + this->declare_parameter>("yaw_norm_thresholds"); const std::string densification_world_frame_id = this->declare_parameter("densification_world_frame_id", "map"); const int densification_num_past_frames = @@ -102,7 +102,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti CenterPointConfig config( class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, - yaw_norm_threshold); + yaw_norm_thresholds); detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); From e615a72f62d2de33e6f0ca6c902fc44f1210a8c8 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 29 Sep 2022 12:58:10 +0900 Subject: [PATCH 27/38] fix(rtc_interface): fix name space (#1976) Signed-off-by: Fumiya Watanabe Signed-off-by: Fumiya Watanabe --- .../rtc_interface/include/rtc_interface/rtc_interface.hpp | 5 ++--- planning/rtc_interface/src/rtc_interface.cpp | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index f96520ebac3c0..de8d78f49a0ff 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -75,9 +75,8 @@ class RTCInterface CooperateStatusArray registered_status_; bool is_auto_mode_; - // TEMPORARY: Name space will be changed. - // std::string cooperate_status_namespace_ = "/planning/cooperate_status"; - // std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; + std::string cooperate_status_namespace_ = "/planning/cooperate_status"; + std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode/internal"; }; diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 102f6733aa35d..02f2efeaf6308 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -76,12 +76,12 @@ RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name) // Publisher pub_statuses_ = - node->create_publisher("~/" + name + "/cooperate_status", 1); + node->create_publisher(cooperate_status_namespace_ + "/" + name, 1); // Service callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); srv_commands_ = node->create_service( - "~/" + name + "/cooperate_commands", + cooperate_commands_namespace_ + "/" + name, std::bind(&RTCInterface::onCooperateCommandService, this, _1, _2), rmw_qos_profile_services_default, callback_group_); srv_auto_mode_ = node->create_service( From 7648decc380f206d21af0bd9e6462cf6c18cee97 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Thu, 29 Sep 2022 13:00:25 +0900 Subject: [PATCH 28/38] refactor(ekf_localizer): use covariance index util of tier4_autoware_util (#1917) * refactor(ekf_localizer): use covariance index util of tier4_autoware_util Signed-off-by: scepter914 * fix alias from #1950 Signed-off-by: scepter914 * fix covariance Signed-off-by: scepter914 Signed-off-by: scepter914 --- .../ekf_localizer/src/ekf_localizer.cpp | 43 +++++++++++-------- .../ekf_localizer/src/measurement.cpp | 13 +++--- 2 files changed, 32 insertions(+), 24 deletions(-) diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index be9bba8197be2..5b7c958657f51 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -24,6 +24,7 @@ #include #include +#include #include @@ -331,9 +332,11 @@ void EKFLocalizer::callbackInitialPose( X(IDX::VX) = 0.0; X(IDX::WZ) = 0.0; - P(IDX::X, IDX::X) = initialpose->pose.covariance[6 * 0 + 0]; - P(IDX::Y, IDX::Y) = initialpose->pose.covariance[6 * 1 + 1]; - P(IDX::YAW, IDX::YAW) = initialpose->pose.covariance[6 * 5 + 5]; + using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + P(IDX::X, IDX::X) = initialpose->pose.covariance[COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = initialpose->pose.covariance[COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = initialpose->pose.covariance[COV_IDX::YAW_YAW]; + if (enable_yaw_bias_estimation_) { P(IDX::YAWB, IDX::YAWB) = 0.0001; } @@ -621,19 +624,20 @@ void EKFLocalizer::publishEstimateResult() pub_biased_pose_->publish(current_biased_ekf_pose_); /* publish latest pose with covariance */ + using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; geometry_msgs::msg::PoseWithCovarianceStamped pose_cov; pose_cov.header.stamp = current_time; pose_cov.header.frame_id = current_ekf_pose_.header.frame_id; pose_cov.pose.pose = current_ekf_pose_.pose; - pose_cov.pose.covariance[0] = P(IDX::X, IDX::X); - pose_cov.pose.covariance[1] = P(IDX::X, IDX::Y); - pose_cov.pose.covariance[5] = P(IDX::X, IDX::YAW); - pose_cov.pose.covariance[6] = P(IDX::Y, IDX::X); - pose_cov.pose.covariance[7] = P(IDX::Y, IDX::Y); - pose_cov.pose.covariance[11] = P(IDX::Y, IDX::YAW); - pose_cov.pose.covariance[30] = P(IDX::YAW, IDX::X); - pose_cov.pose.covariance[31] = P(IDX::YAW, IDX::Y); - pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW); + pose_cov.pose.covariance[COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov.pose.covariance[COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov.pose.covariance[COV_IDX::X_YAW] = P(IDX::X, IDX::YAW); + pose_cov.pose.covariance[COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov.pose.covariance[COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov.pose.covariance[COV_IDX::Y_YAW] = P(IDX::Y, IDX::YAW); + pose_cov.pose.covariance[COV_IDX::YAW_X] = P(IDX::YAW, IDX::X); + pose_cov.pose.covariance[COV_IDX::YAW_Y] = P(IDX::YAW, IDX::Y); + pose_cov.pose.covariance[COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); pub_pose_cov_->publish(pose_cov); geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; @@ -648,10 +652,10 @@ void EKFLocalizer::publishEstimateResult() twist_cov.header.stamp = current_time; twist_cov.header.frame_id = current_ekf_twist_.header.frame_id; twist_cov.twist.twist = current_ekf_twist_.twist; - twist_cov.twist.covariance[0] = P(IDX::VX, IDX::VX); - twist_cov.twist.covariance[5] = P(IDX::VX, IDX::WZ); - twist_cov.twist.covariance[30] = P(IDX::WZ, IDX::VX); - twist_cov.twist.covariance[35] = P(IDX::WZ, IDX::WZ); + twist_cov.twist.covariance[COV_IDX::X_X] = P(IDX::VX, IDX::VX); + twist_cov.twist.covariance[COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); + twist_cov.twist.covariance[COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); + twist_cov.twist.covariance[COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); pub_twist_cov_->publish(twist_cov); /* publish yaw bias */ @@ -700,9 +704,10 @@ void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovar tf2::fromMsg(pose.pose.pose.orientation, q_tf); tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw_tmp); - double z_dev = pose.pose.covariance[2 * 6 + 2]; - double roll_dev = pose.pose.covariance[3 * 6 + 3]; - double pitch_dev = pose.pose.covariance[4 * 6 + 4]; + using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + double z_dev = pose.pose.covariance[COV_IDX::Z_Z]; + double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL]; + double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH]; z_filter_.update(z, z_dev, pose.header.stamp); roll_filter_.update(roll, roll_dev, pose.header.stamp); diff --git a/localization/ekf_localizer/src/measurement.cpp b/localization/ekf_localizer/src/measurement.cpp index a30d9d03dcded..0b3e65bd60f5d 100644 --- a/localization/ekf_localizer/src/measurement.cpp +++ b/localization/ekf_localizer/src/measurement.cpp @@ -15,6 +15,7 @@ #include "ekf_localizer/measurement.hpp" #include "ekf_localizer/state_index.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" Eigen::Matrix poseMeasurementMatrix() { @@ -37,9 +38,10 @@ Eigen::Matrix3d poseMeasurementCovariance( const std::array & covariance, const size_t smoothing_step) { Eigen::Matrix3d R; - R << covariance.at(6 * 0 + 0), covariance.at(6 * 0 + 1), covariance.at(6 * 0 + 5), - covariance.at(6 * 1 + 0), covariance.at(6 * 1 + 1), covariance.at(6 * 1 + 5), - covariance.at(6 * 5 + 0), covariance.at(6 * 5 + 1), covariance.at(6 * 5 + 5); + using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + R << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), + covariance.at(COV_IDX::Y_X), covariance.at(COV_IDX::Y_Y), covariance.at(COV_IDX::Y_YAW), + covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_Y), covariance.at(COV_IDX::YAW_YAW); return R * static_cast(smoothing_step); } @@ -47,7 +49,8 @@ Eigen::Matrix2d twistMeasurementCovariance( const std::array & covariance, const size_t smoothing_step) { Eigen::Matrix2d R; - R << covariance.at(6 * 0 + 0), covariance.at(6 * 0 + 5), covariance.at(6 * 5 + 0), - covariance.at(6 * 5 + 5); + using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + R << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), + covariance.at(COV_IDX::YAW_YAW); return R * static_cast(smoothing_step); } From 01e083dc7950f83fc34b1c35b0f43e07e9a5ceab Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Thu, 29 Sep 2022 05:33:31 +0000 Subject: [PATCH 29/38] chore: update CODEOWNERS (#1983) Signed-off-by: GitHub Signed-off-by: GitHub Co-authored-by: kenji-miyake Co-authored-by: Takeshi Ishita --- .github/CODEOWNERS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 97b27a10e3010..945ed4e9000fc 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -16,7 +16,7 @@ common/global_parameter_loader/** kenji.miyake@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp -common/kalman_filter/** yukihiro.saito@tier4.jp +common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/motion_common/** christopherj.ho@gmail.com common/motion_testing/** christopherj.ho@gmail.com common/motion_utils/** satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp From 7a125909a97750fed72a0eaba826e35e55387adc Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 29 Sep 2022 14:53:02 +0900 Subject: [PATCH 30/38] fix(behavior_velocity_planner): fix invalid access in velocity callback function (#1980) Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- planning/behavior_velocity_planner/src/node.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 3b8711543571e..64a79e87bd762 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -309,7 +309,7 @@ void BehaviorVelocityPlannerNode::onVehicleVelocity( // Add velocity to buffer planner_data_.velocity_buffer.push_front(*current_velocity); const rclcpp::Time now = this->now(); - while (true) { + while (!planner_data_.velocity_buffer.empty()) { // Check oldest data time const auto & s = planner_data_.velocity_buffer.back().header.stamp; const auto time_diff = @@ -320,9 +320,6 @@ void BehaviorVelocityPlannerNode::onVehicleVelocity( break; } - if (planner_data_.velocity_buffer.empty()) { - break; - } // Remove old data planner_data_.velocity_buffer.pop_back(); } From 767e76551e8e23e6e02796cc1b4fdd9f2e6b3546 Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Thu, 29 Sep 2022 15:06:15 +0900 Subject: [PATCH 31/38] refactor(kalman_filter): make getter functions explicitly return values (#1895) * refactor(kalman_filter): getter functions should explicitly return values --- .../time_delay_kalman_filter.hpp | 6 +-- .../src/time_delay_kalman_filter.cpp | 5 ++- .../ekf_localizer/src/ekf_localizer.cpp | 40 +++++++------------ 3 files changed, 19 insertions(+), 32 deletions(-) diff --git a/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp b/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp index 5fb615647207f..1a072bb43d4ac 100644 --- a/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp +++ b/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp @@ -48,15 +48,13 @@ class TimeDelayKalmanFilter : public KalmanFilter /** * @brief get latest time estimated state - * @param x latest time estimated state */ - void getLatestX(Eigen::MatrixXd & x); + Eigen::MatrixXd getLatestX() const; /** * @brief get latest time estimation covariance - * @param P latest time estimation covariance */ - void getLatestP(Eigen::MatrixXd & P); + Eigen::MatrixXd getLatestP() const; /** * @brief calculate kalman filter covariance by precision model with time delay. This is mainly diff --git a/common/kalman_filter/src/time_delay_kalman_filter.cpp b/common/kalman_filter/src/time_delay_kalman_filter.cpp index 3c06ef25150c3..24f06dbb9821b 100644 --- a/common/kalman_filter/src/time_delay_kalman_filter.cpp +++ b/common/kalman_filter/src/time_delay_kalman_filter.cpp @@ -32,8 +32,9 @@ void TimeDelayKalmanFilter::init( } } -void TimeDelayKalmanFilter::getLatestX(Eigen::MatrixXd & x) { x = x_.block(0, 0, dim_x_, 1); } -void TimeDelayKalmanFilter::getLatestP(Eigen::MatrixXd & P) { P = P_.block(0, 0, dim_x_, dim_x_); } +Eigen::MatrixXd TimeDelayKalmanFilter::getLatestX() const { return x_.block(0, 0, dim_x_, 1); } + +Eigen::MatrixXd TimeDelayKalmanFilter::getLatestP() const { return P_.block(0, 0, dim_x_, dim_x_); } bool TimeDelayKalmanFilter::predictWithDelay( const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 5b7c958657f51..7c467b853c49c 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -240,8 +240,7 @@ void EKFLocalizer::timerCallback() void EKFLocalizer::showCurrentX() { if (show_debug_info_) { - Eigen::MatrixXd X(dim_x_, 1); - ekf_.getLatestX(X); + const Eigen::MatrixXd X = ekf_.getLatestX(); DEBUG_PRINT_MAT(X.transpose()); } } @@ -418,12 +417,10 @@ void EKFLocalizer::predictKinematicsModel() * [ 0, 0, 0, 0, 0, 1] */ - Eigen::MatrixXd X_curr(dim_x_, 1); // current state - ekf_.getLatestX(X_curr); + const Eigen::MatrixXd X_curr = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_curr.transpose()); - Eigen::MatrixXd P_curr; - ekf_.getLatestP(P_curr); + const Eigen::MatrixXd P_curr = ekf_.getLatestP(); const double dt = ekf_dt_; @@ -434,8 +431,7 @@ void EKFLocalizer::predictKinematicsModel() ekf_.predictWithDelay(X_next, A, Q); // debug - Eigen::MatrixXd X_result(dim_x_, 1); - ekf_.getLatestX(X_result); + const Eigen::MatrixXd X_result = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); } @@ -452,8 +448,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar pose.header.frame_id.c_str(), pose_frame_id_.c_str()), 2000); } - Eigen::MatrixXd X_curr(dim_x_, 1); // current state - ekf_.getLatestX(X_curr); + const Eigen::MatrixXd X_curr = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_curr.transpose()); constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output @@ -499,9 +494,8 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar Eigen::MatrixXd y_ekf(dim_y, 1); y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X), ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw; - Eigen::MatrixXd P_curr, P_y; - ekf_.getLatestP(P_curr); - P_y = P_curr.block(0, 0, dim_y, dim_y); + const Eigen::MatrixXd P_curr = ekf_.getLatestP(); + const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) { warning_.warnThrottle( "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore " @@ -520,8 +514,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar ekf_.updateWithDelay(y, C, R, delay_step); // debug - Eigen::MatrixXd X_result(dim_x_, 1); - ekf_.getLatestX(X_result); + const Eigen::MatrixXd X_result = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); } @@ -538,8 +531,7 @@ void EKFLocalizer::measurementUpdateTwist( "twist frame_id must be base_link"); } - Eigen::MatrixXd X_curr(dim_x_, 1); // current state - ekf_.getLatestX(X_curr); + const Eigen::MatrixXd X_curr = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_curr.transpose()); constexpr int dim_y = 2; // vx, wz @@ -580,9 +572,8 @@ void EKFLocalizer::measurementUpdateTwist( Eigen::MatrixXd y_ekf(dim_y, 1); y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::VX), ekf_.getXelement(delay_step * dim_x_ + IDX::WZ); - Eigen::MatrixXd P_curr, P_y; - ekf_.getLatestP(P_curr); - P_y = P_curr.block(4, 4, dim_y, dim_y); + const Eigen::MatrixXd P_curr = ekf_.getLatestP(); + const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) { warning_.warnThrottle( "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore " @@ -602,8 +593,7 @@ void EKFLocalizer::measurementUpdateTwist( ekf_.updateWithDelay(y, C, R, delay_step); // debug - Eigen::MatrixXd X_result(dim_x_, 1); - ekf_.getLatestX(X_result); + const Eigen::MatrixXd X_result = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); } @@ -614,10 +604,8 @@ void EKFLocalizer::measurementUpdateTwist( void EKFLocalizer::publishEstimateResult() { rclcpp::Time current_time = this->now(); - Eigen::MatrixXd X(dim_x_, 1); - Eigen::MatrixXd P(dim_x_, dim_x_); - ekf_.getLatestX(X); - ekf_.getLatestP(P); + const Eigen::MatrixXd X = ekf_.getLatestX(); + const Eigen::MatrixXd P = ekf_.getLatestP(); /* publish latest pose */ pub_pose_->publish(current_ekf_pose_); From d75ea6ea05337cd231d0e18874dea50f406588f1 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 29 Sep 2022 17:08:13 +0900 Subject: [PATCH 32/38] refactor(mission_planner): node refactoring (#1940) * refactor(mission_planner): node refactoring Signed-off-by: Takayuki Murooka * organize files and directories Signed-off-by: Takayuki Murooka * refactor Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- planning/mission_planner/CMakeLists.txt | 8 +- .../mission_planner}/goal_pose_visualizer.hpp | 6 +- .../mission_planner_interface.hpp} | 36 +++-- .../mission_planner_lanelet2.hpp | 18 ++- .../mission_planner}/utility_functions.hpp | 6 +- .../goal_pose_visualizer.cpp | 2 +- .../src/mission_planner/mission_planner.cpp | 152 ------------------ .../src/mission_planner_interface.cpp | 148 +++++++++++++++++ .../mission_planner_lanelet2.cpp | 72 +++++---- .../utility_functions.cpp | 2 +- 10 files changed, 228 insertions(+), 222 deletions(-) rename planning/mission_planner/{src/goal_pose_visualizer => include/mission_planner}/goal_pose_visualizer.hpp (87%) rename planning/mission_planner/{src/mission_planner/mission_planner.hpp => include/mission_planner/mission_planner_interface.hpp} (65%) rename planning/mission_planner/{src/mission_planner_lanelet2 => include/mission_planner}/mission_planner_lanelet2.hpp (75%) rename planning/mission_planner/{src/mission_planner_lanelet2 => include/mission_planner}/utility_functions.hpp (89%) rename planning/mission_planner/src/{goal_pose_visualizer => }/goal_pose_visualizer.cpp (96%) delete mode 100644 planning/mission_planner/src/mission_planner/mission_planner.cpp create mode 100644 planning/mission_planner/src/mission_planner_interface.cpp rename planning/mission_planner/src/{mission_planner_lanelet2 => }/mission_planner_lanelet2.cpp (82%) rename planning/mission_planner/src/{mission_planner_lanelet2 => }/utility_functions.cpp (97%) diff --git a/planning/mission_planner/CMakeLists.txt b/planning/mission_planner/CMakeLists.txt index 9d7a2d030d41c..2c795ce543236 100644 --- a/planning/mission_planner/CMakeLists.txt +++ b/planning/mission_planner/CMakeLists.txt @@ -5,9 +5,9 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME}_component SHARED - src/mission_planner/mission_planner.cpp - src/mission_planner_lanelet2/mission_planner_lanelet2.cpp - src/mission_planner_lanelet2/utility_functions.cpp + src/mission_planner_interface.cpp + src/mission_planner_lanelet2.cpp + src/utility_functions.cpp ) rclcpp_components_register_node(${PROJECT_NAME}_component @@ -16,7 +16,7 @@ rclcpp_components_register_node(${PROJECT_NAME}_component ) ament_auto_add_library(goal_pose_visualizer_component SHARED - src/goal_pose_visualizer/goal_pose_visualizer.cpp + src/goal_pose_visualizer.cpp ) rclcpp_components_register_node(goal_pose_visualizer_component diff --git a/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp b/planning/mission_planner/include/mission_planner/goal_pose_visualizer.hpp similarity index 87% rename from planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp rename to planning/mission_planner/include/mission_planner/goal_pose_visualizer.hpp index 718a721239e33..573ee655ac20a 100644 --- a/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp +++ b/planning/mission_planner/include/mission_planner/goal_pose_visualizer.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GOAL_POSE_VISUALIZER__GOAL_POSE_VISUALIZER_HPP_ -#define GOAL_POSE_VISUALIZER__GOAL_POSE_VISUALIZER_HPP_ +#ifndef MISSION_PLANNER__GOAL_POSE_VISUALIZER_HPP_ +#define MISSION_PLANNER__GOAL_POSE_VISUALIZER_HPP_ #include @@ -36,4 +36,4 @@ class GoalPoseVisualizer : public rclcpp::Node }; } // namespace mission_planner -#endif // GOAL_POSE_VISUALIZER__GOAL_POSE_VISUALIZER_HPP_ +#endif // MISSION_PLANNER__GOAL_POSE_VISUALIZER_HPP_ diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp similarity index 65% rename from planning/mission_planner/src/mission_planner/mission_planner.hpp rename to planning/mission_planner/include/mission_planner/mission_planner_interface.hpp index 066a5549dbe69..fe92a9e1b182f 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MISSION_PLANNER__MISSION_PLANNER_HPP_ -#define MISSION_PLANNER__MISSION_PLANNER_HPP_ +#ifndef MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ +#define MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ + +#include #include #include @@ -30,25 +32,25 @@ #include #include #include +#include namespace mission_planner { -class MissionPlanner : public rclcpp::Node +class MissionPlannerInterface : public rclcpp::Node { protected: - MissionPlanner(const std::string & node_name, const rclcpp::NodeOptions & node_options); + MissionPlannerInterface(const std::string & node_name, const rclcpp::NodeOptions & node_options); - geometry_msgs::msg::PoseStamped goal_pose_; - geometry_msgs::msg::PoseStamped start_pose_; - std::vector checkpoints_; + geometry_msgs::msg::Pose::ConstSharedPtr ego_pose_; + std::vector check_points_; - std::string base_link_frame_; std::string map_frame_; rclcpp::Publisher::SharedPtr marker_publisher_; virtual bool is_routing_graph_ready() const = 0; - virtual autoware_auto_planning_msgs::msg::HADMapRoute plan_route() = 0; + virtual autoware_auto_planning_msgs::msg::HADMapRoute plan_route( + const std::vector & check_points) = 0; virtual void visualize_route( const autoware_auto_planning_msgs::msg::HADMapRoute & route) const = 0; virtual void publish_route(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const; @@ -56,19 +58,19 @@ class MissionPlanner : public rclcpp::Node private: rclcpp::Publisher::SharedPtr route_publisher_; rclcpp::Subscription::SharedPtr goal_subscriber_; - rclcpp::Subscription::SharedPtr checkpoint_subscriber_; + rclcpp::Subscription::SharedPtr check_point_subscriber_; + rclcpp::Subscription::SharedPtr odometry_subscriber_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - bool get_ego_vehicle_pose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose); + void odometry_callback(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void goal_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr); - void checkpoint_callback( - const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr); - bool transform_pose( - const geometry_msgs::msg::PoseStamped & input_pose, - geometry_msgs::msg::PoseStamped * output_pose, const std::string & target_frame); + void check_point_callback( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr check_point_msg_ptr); + boost::optional transform_pose( + const geometry_msgs::msg::PoseStamped & input_pose, const std::string & target_frame); }; } // namespace mission_planner -#endif // MISSION_PLANNER__MISSION_PLANNER_HPP_ +#endif // MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.hpp b/planning/mission_planner/include/mission_planner/mission_planner_lanelet2.hpp similarity index 75% rename from planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.hpp rename to planning/mission_planner/include/mission_planner/mission_planner_lanelet2.hpp index 1fdebd7cf76db..544783e19e430 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_lanelet2.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MISSION_PLANNER_LANELET2__MISSION_PLANNER_LANELET2_HPP_ -#define MISSION_PLANNER_LANELET2__MISSION_PLANNER_LANELET2_HPP_ +#ifndef MISSION_PLANNER__MISSION_PLANNER_LANELET2_HPP_ +#define MISSION_PLANNER__MISSION_PLANNER_LANELET2_HPP_ #include #include @@ -25,7 +25,7 @@ #include // Autoware -#include "../mission_planner/mission_planner.hpp" +#include "mission_planner/mission_planner_interface.hpp" #include @@ -40,7 +40,7 @@ namespace mission_planner { using RouteSections = std::vector; -class MissionPlannerLanelet2 : public MissionPlanner +class MissionPlannerLanelet2 : public MissionPlannerInterface { public: explicit MissionPlannerLanelet2(const rclcpp::NodeOptions & node_options); @@ -57,14 +57,16 @@ class MissionPlannerLanelet2 : public MissionPlanner rclcpp::Subscription::SharedPtr map_subscriber_; void map_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); - bool is_goal_valid() const; - void refine_goal_height(const RouteSections & route_sections); + bool is_goal_valid(const geometry_msgs::msg::Pose & goal_pose) const; + geometry_msgs::msg::Pose refine_goal_height( + const RouteSections & route_sections, const geometry_msgs::msg::Pose & goal_pose) const; // virtual functions bool is_routing_graph_ready() const override; - autoware_auto_planning_msgs::msg::HADMapRoute plan_route() override; + autoware_auto_planning_msgs::msg::HADMapRoute plan_route( + const std::vector & check_points) override; void visualize_route(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const override; }; } // namespace mission_planner -#endif // MISSION_PLANNER_LANELET2__MISSION_PLANNER_LANELET2_HPP_ +#endif // MISSION_PLANNER__MISSION_PLANNER_LANELET2_HPP_ diff --git a/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.hpp b/planning/mission_planner/include/mission_planner/utility_functions.hpp similarity index 89% rename from planning/mission_planner/src/mission_planner_lanelet2/utility_functions.hpp rename to planning/mission_planner/include/mission_planner/utility_functions.hpp index fd26c06f38b79..d5db1abbd7bf7 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.hpp +++ b/planning/mission_planner/include/mission_planner/utility_functions.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MISSION_PLANNER_LANELET2__UTILITY_FUNCTIONS_HPP_ -#define MISSION_PLANNER_LANELET2__UTILITY_FUNCTIONS_HPP_ +#ifndef MISSION_PLANNER__UTILITY_FUNCTIONS_HPP_ +#define MISSION_PLANNER__UTILITY_FUNCTIONS_HPP_ #include #include @@ -43,4 +43,4 @@ void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, doub void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); std::string to_string(const geometry_msgs::msg::Pose & pose); -#endif // MISSION_PLANNER_LANELET2__UTILITY_FUNCTIONS_HPP_ +#endif // MISSION_PLANNER__UTILITY_FUNCTIONS_HPP_ diff --git a/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp b/planning/mission_planner/src/goal_pose_visualizer.cpp similarity index 96% rename from planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp rename to planning/mission_planner/src/goal_pose_visualizer.cpp index a5e70ffef6d88..b26b399d225f2 100644 --- a/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp +++ b/planning/mission_planner/src/goal_pose_visualizer.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "goal_pose_visualizer.hpp" +#include "mission_planner/goal_pose_visualizer.hpp" namespace mission_planner { diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp deleted file mode 100644 index 000d7ac6260e2..0000000000000 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ /dev/null @@ -1,152 +0,0 @@ -// Copyright 2019 Autoware Foundation -// -// 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 "mission_planner.hpp" - -#include -#include -#include - -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif -#include - -#include - -namespace mission_planner -{ -MissionPlanner::MissionPlanner( - const std::string & node_name, const rclcpp::NodeOptions & node_options) -: Node(node_name, node_options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) -{ - map_frame_ = declare_parameter("map_frame", "map"); - base_link_frame_ = declare_parameter("base_link_frame", "base_link"); - - using std::placeholders::_1; - - goal_subscriber_ = create_subscription( - "input/goal_pose", 10, std::bind(&MissionPlanner::goal_pose_callback, this, _1)); - checkpoint_subscriber_ = create_subscription( - "input/checkpoint", 10, std::bind(&MissionPlanner::checkpoint_callback, this, _1)); - - rclcpp::QoS durable_qos{1}; - durable_qos.transient_local(); - route_publisher_ = - create_publisher("output/route", durable_qos); - marker_publisher_ = - create_publisher("debug/route_marker", durable_qos); -} - -bool MissionPlanner::get_ego_vehicle_pose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose) -{ - geometry_msgs::msg::PoseStamped base_link_origin; - base_link_origin.header.frame_id = base_link_frame_; - base_link_origin.pose.position.x = 0; - base_link_origin.pose.position.y = 0; - base_link_origin.pose.position.z = 0; - base_link_origin.pose.orientation.x = 0; - base_link_origin.pose.orientation.y = 0; - base_link_origin.pose.orientation.z = 0; - base_link_origin.pose.orientation.w = 1; - - // transform base_link frame origin to map_frame to get vehicle positions - return transform_pose(base_link_origin, ego_vehicle_pose, map_frame_); -} - -bool MissionPlanner::transform_pose( - const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped * output_pose, - const std::string & target_frame) -{ - geometry_msgs::msg::TransformStamped transform; - try { - transform = - tf_buffer_.lookupTransform(target_frame, input_pose.header.frame_id, tf2::TimePointZero); - tf2::doTransform(input_pose, *output_pose, transform); - return true; - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(get_logger(), "%s", ex.what()); - return false; - } -} - -void MissionPlanner::goal_pose_callback( - const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr) -{ - // set start pose - if (!get_ego_vehicle_pose(&start_pose_)) { - RCLCPP_ERROR( - get_logger(), "Failed to get ego vehicle pose in map frame. Aborting mission planning"); - return; - } - // set goal pose - if (!transform_pose(*goal_msg_ptr, &goal_pose_, map_frame_)) { - RCLCPP_ERROR(get_logger(), "Failed to get goal pose in map frame. Aborting mission planning"); - return; - } - - RCLCPP_INFO(get_logger(), "New goal pose is set. Reset checkpoints."); - checkpoints_.clear(); - checkpoints_.push_back(start_pose_); - checkpoints_.push_back(goal_pose_); - - if (!is_routing_graph_ready()) { - RCLCPP_ERROR(get_logger(), "RoutingGraph is not ready. Aborting mission planning"); - return; - } - - autoware_auto_planning_msgs::msg::HADMapRoute route = plan_route(); - publish_route(route); -} // namespace mission_planner - -void MissionPlanner::checkpoint_callback( - const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr) -{ - if (checkpoints_.size() < 2) { - RCLCPP_ERROR( - get_logger(), - "You must set start and goal before setting checkpoints. Aborting mission planning"); - return; - } - - geometry_msgs::msg::PoseStamped transformed_checkpoint; - if (!transform_pose(*checkpoint_msg_ptr, &transformed_checkpoint, map_frame_)) { - RCLCPP_ERROR( - get_logger(), "Failed to get checkpoint pose in map frame. Aborting mission planning"); - return; - } - - // insert checkpoint before goal - checkpoints_.insert(checkpoints_.end() - 1, transformed_checkpoint); - - autoware_auto_planning_msgs::msg::HADMapRoute route = plan_route(); - publish_route(route); -} - -void MissionPlanner::publish_route( - const autoware_auto_planning_msgs::msg::HADMapRoute & route) const -{ - if (!route.segments.empty()) { - RCLCPP_INFO(get_logger(), "Route successfully planned. Publishing..."); - route_publisher_->publish(route); - visualize_route(route); - } else { - RCLCPP_ERROR(get_logger(), "Calculated route is empty!"); - } -} - -} // namespace mission_planner diff --git a/planning/mission_planner/src/mission_planner_interface.cpp b/planning/mission_planner/src/mission_planner_interface.cpp new file mode 100644 index 0000000000000..327b246d841eb --- /dev/null +++ b/planning/mission_planner/src/mission_planner_interface.cpp @@ -0,0 +1,148 @@ +// Copyright 2019 Autoware Foundation +// +// 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 "mission_planner/mission_planner_interface.hpp" + +#include +#include +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +#include +#include + +namespace mission_planner +{ +MissionPlannerInterface::MissionPlannerInterface( + const std::string & node_name, const rclcpp::NodeOptions & node_options) +: Node(node_name, node_options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) +{ + map_frame_ = declare_parameter("map_frame", "map"); + + using std::placeholders::_1; + + odometry_subscriber_ = create_subscription( + "/localization/kinematic_state", rclcpp::QoS{1}, + std::bind(&MissionPlannerInterface::odometry_callback, this, _1)); + goal_subscriber_ = create_subscription( + "input/goal_pose", 10, std::bind(&MissionPlannerInterface::goal_pose_callback, this, _1)); + check_point_subscriber_ = create_subscription( + "input/checkpoint", 10, std::bind(&MissionPlannerInterface::check_point_callback, this, _1)); + + rclcpp::QoS durable_qos{1}; + durable_qos.transient_local(); + route_publisher_ = + create_publisher("output/route", durable_qos); + marker_publisher_ = + create_publisher("debug/route_marker", durable_qos); +} + +void MissionPlannerInterface::odometry_callback(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + ego_pose_ = std::make_shared(msg->pose.pose); +} + +boost::optional MissionPlannerInterface::transform_pose( + const geometry_msgs::msg::PoseStamped & input_pose, const std::string & target_frame) +{ + try { + geometry_msgs::msg::PoseStamped output_pose; + const auto transform = + tf_buffer_.lookupTransform(target_frame, input_pose.header.frame_id, tf2::TimePointZero); + tf2::doTransform(input_pose, output_pose, transform); + return output_pose; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "%s", ex.what()); + } + + return {}; +} + +void MissionPlannerInterface::goal_pose_callback( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr) +{ + // set start pose + if (!ego_pose_) { + RCLCPP_ERROR(get_logger(), "Ego pose has not been subscribed. Aborting mission planning"); + return; + } + + // set goal pose + const auto opt_transformed_goal_pose = transform_pose(*goal_msg_ptr, map_frame_); + if (!opt_transformed_goal_pose) { + RCLCPP_ERROR(get_logger(), "Failed to get goal pose in map frame. Aborting mission planning"); + return; + } + const auto transformed_goal_pose = opt_transformed_goal_pose.get(); + + RCLCPP_INFO(get_logger(), "New goal pose is set. Reset check_points."); + check_points_.clear(); + check_points_.push_back(*ego_pose_); + check_points_.push_back(transformed_goal_pose.pose); + + if (!is_routing_graph_ready()) { + RCLCPP_ERROR(get_logger(), "RoutingGraph is not ready. Aborting mission planning"); + return; + } + + const auto route = plan_route(check_points_); + publish_route(route); +} // namespace mission_planner + +void MissionPlannerInterface::check_point_callback( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr check_point_msg_ptr) +{ + if (check_points_.size() < 2) { + RCLCPP_ERROR( + get_logger(), + "You must set start and goal before setting check_points. Aborting mission planning"); + return; + } + + const auto opt_transformed_check_point = transform_pose(*check_point_msg_ptr, map_frame_); + if (!opt_transformed_check_point) { + RCLCPP_ERROR( + get_logger(), "Failed to get check_point pose in map frame. Aborting mission planning"); + return; + } + const auto transformed_check_point = opt_transformed_check_point.get(); + + // insert check_point before goal + check_points_.insert(check_points_.end() - 1, transformed_check_point.pose); + + const auto route = plan_route(check_points_); + publish_route(route); +} + +void MissionPlannerInterface::publish_route( + const autoware_auto_planning_msgs::msg::HADMapRoute & route) const +{ + if (route.segments.empty()) { + RCLCPP_ERROR(get_logger(), "Calculated route is empty!"); + return; + } + + RCLCPP_INFO(get_logger(), "Route successfully planned. Publishing..."); + route_publisher_->publish(route); + visualize_route(route); +} + +} // namespace mission_planner diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp b/planning/mission_planner/src/mission_planner_lanelet2.cpp similarity index 82% rename from planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp rename to planning/mission_planner/src/mission_planner_lanelet2.cpp index 2249ec4a4750e..7bda7f7ca1063 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp +++ b/planning/mission_planner/src/mission_planner_lanelet2.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mission_planner_lanelet2.hpp" +#include "mission_planner/mission_planner_lanelet2.hpp" -#include "utility_functions.hpp" +#include "mission_planner/utility_functions.hpp" #include #include @@ -119,7 +119,7 @@ double project_goal_to_map( namespace mission_planner { MissionPlannerLanelet2::MissionPlannerLanelet2(const rclcpp::NodeOptions & node_options) -: MissionPlanner("mission_planner", node_options), is_graph_ready_(false) +: MissionPlannerInterface("mission_planner", node_options), is_graph_ready_(false) { using std::placeholders::_1; map_subscriber_ = create_subscription( @@ -192,19 +192,17 @@ void MissionPlannerLanelet2::visualize_route( marker_publisher_->publish(route_marker_array); } -bool MissionPlannerLanelet2::is_goal_valid() const +bool MissionPlannerLanelet2::is_goal_valid(const geometry_msgs::msg::Pose & goal_pose) const { lanelet::Lanelet closest_lanelet; - if (!lanelet::utils::query::getClosestLanelet( - road_lanelets_, goal_pose_.pose, &closest_lanelet)) { + if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal_pose, &closest_lanelet)) { return false; } - const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose_.pose.position); + const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose.position); if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { - const auto lane_yaw = - lanelet::utils::getLaneletAngle(closest_lanelet, goal_pose_.pose.position); - const auto goal_yaw = tf2::getYaw(goal_pose_.pose.orientation); + const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal_pose.position); + const auto goal_yaw = tf2::getYaw(goal_pose.orientation); const auto angle_diff = normalize_radian(lane_yaw - goal_yaw); constexpr double th_angle = M_PI / 4; @@ -229,14 +227,14 @@ bool MissionPlannerLanelet2::is_goal_valid() const // check if goal is in shoulder lanelet lanelet::Lanelet closest_shoulder_lanelet; if (!lanelet::utils::query::getClosestLanelet( - shoulder_lanelets_, goal_pose_.pose, &closest_shoulder_lanelet)) { + shoulder_lanelets_, goal_pose, &closest_shoulder_lanelet)) { return false; } // check if goal pose is in shoulder lane if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { const auto lane_yaw = - lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal_pose_.pose.position); - const auto goal_yaw = tf2::getYaw(goal_pose_.pose.orientation); + lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal_pose.position); + const auto goal_yaw = tf2::getYaw(goal_pose.orientation); const auto angle_diff = normalize_radian(lane_yaw - goal_yaw); constexpr double th_angle = M_PI / 4; @@ -248,31 +246,34 @@ bool MissionPlannerLanelet2::is_goal_valid() const return false; } -autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::plan_route() +autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::plan_route( + const std::vector & check_points) { + const auto & goal_pose = check_points.back(); + std::stringstream log_ss; - for (const auto & checkpoint : checkpoints_) { - log_ss << "x: " << checkpoint.pose.position.x << " " - << "y: " << checkpoint.pose.position.y << std::endl; + for (const auto & check_point : check_points) { + log_ss << "x: " << check_point.position.x << " " + << "y: " << check_point.position.y << std::endl; } RCLCPP_INFO_STREAM( - get_logger(), "start planning route with checkpoints: " << std::endl - << log_ss.str()); + get_logger(), "start planning route with check_points: " << std::endl + << log_ss.str()); autoware_auto_planning_msgs::msg::HADMapRoute route_msg; RouteSections route_sections; - if (!is_goal_valid()) { + if (!is_goal_valid(goal_pose)) { RCLCPP_WARN(get_logger(), "Goal is not valid! Please check position and angle of goal_pose"); return route_msg; } - for (std::size_t i = 1; i < checkpoints_.size(); i++) { - const auto start_checkpoint = checkpoints_.at(i - 1); - const auto goal_checkpoint = checkpoints_.at(i); + for (std::size_t i = 1; i < check_points.size(); i++) { + const auto start_check_point = check_points.at(i - 1); + const auto goal_check_point = check_points.at(i); lanelet::ConstLanelets path_lanelets; if (!route_handler_.planPathLaneletsBetweenCheckpoints( - start_checkpoint.pose, goal_checkpoint.pose, &path_lanelets)) { + start_check_point, goal_check_point, &path_lanelets)) { return route_msg; } // create local route sections @@ -286,25 +287,30 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::plan_route return route_msg; } - refine_goal_height(route_sections); + const auto refined_goal_pose = refine_goal_height(route_sections, goal_pose); route_msg.header.stamp = this->now(); route_msg.header.frame_id = map_frame_; route_msg.segments = route_sections; - route_msg.goal_pose = goal_pose_.pose; + route_msg.goal_pose = refined_goal_pose; - RCLCPP_DEBUG(get_logger(), "Goal Pose Z : %lf", goal_pose_.pose.position.z); + RCLCPP_DEBUG(get_logger(), "Goal Pose Z : %lf", refined_goal_pose.position.z); return route_msg; } -void MissionPlannerLanelet2::refine_goal_height(const RouteSections & route_sections) +geometry_msgs::msg::Pose MissionPlannerLanelet2::refine_goal_height( + const RouteSections & route_sections, const geometry_msgs::msg::Pose & goal_pose) const { const auto goal_lane_id = route_sections.back().preferred_primitive_id; - lanelet::Lanelet goal_lanelet = lanelet_map_ptr_->laneletLayer.get(goal_lane_id); - const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose_.pose.position); - double goal_height = project_goal_to_map(goal_lanelet, goal_lanelet_pt); - goal_pose_.pose.position.z = goal_height; - checkpoints_.back().pose.position.z = goal_height; + const lanelet::Lanelet goal_lanelet = lanelet_map_ptr_->laneletLayer.get(goal_lane_id); + const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose.position); + + const double goal_height = project_goal_to_map(goal_lanelet, goal_lanelet_pt); + + geometry_msgs::msg::Pose refined_goal_pose = goal_pose; + refined_goal_pose.position.z = goal_height; + + return refined_goal_pose; } } // namespace mission_planner diff --git a/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp b/planning/mission_planner/src/utility_functions.cpp similarity index 97% rename from planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp rename to planning/mission_planner/src/utility_functions.cpp index e0ffe51d188fd..efdb41a66032c 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp +++ b/planning/mission_planner/src/utility_functions.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utility_functions.hpp" +#include "mission_planner/utility_functions.hpp" #include From 6e5c69a6aec77a0a3405611795a2fd3e3e9f3c82 Mon Sep 17 00:00:00 2001 From: Ambroise Vincent Date: Fri, 30 Sep 2022 01:23:34 +0100 Subject: [PATCH 33/38] feat(lidar_apollo_segmentation_tvm): port package (#1181) Port apollo_lidar_segmentation from Autoware.Auto, changing the name to align with the TierIV package using TensorRT. Issue-Id: SCM-4493 Change-Id: If2bd78d473a0044d06fa42658766ffe9177a59a2 Signed-off-by: Luca Foschiani Signed-off-by: Ambroise Vincent Signed-off-by: Luca Foschiani Signed-off-by: Ambroise Vincent Co-authored-by: Luca Foschiani Co-authored-by: Xinyu Wang <93699235+angry-crab@users.noreply.github.com> --- .../CMakeLists.txt | 75 ++++ .../design/lidar-segmentation-design.md | 29 ++ .../cluster2d.hpp | 180 +++++++++ .../disjoint_set.hpp | 84 ++++ .../feature_generator.hpp | 71 ++++ .../feature_map.hpp | 87 +++++ .../lidar_apollo_segmentation_tvm.hpp | 198 ++++++++++ .../log_table.hpp | 35 ++ .../lidar_apollo_segmentation_tvm/util.hpp | 66 ++++ .../visibility_control.hpp | 37 ++ .../lidar_apollo_segmentation_tvm/package.xml | 36 ++ .../src/cluster2d.cpp | 364 ++++++++++++++++++ .../src/feature_generator.cpp | 116 ++++++ .../src/feature_map.cpp | 184 +++++++++ .../src/lidar_apollo_segmentation_tvm.cpp | 197 ++++++++++ .../src/log_table.cpp | 56 +++ .../test/main.cpp | 96 +++++ .../CMakeLists.txt | 51 +++ .../design/lidar-segmentation-nodes-design.md | 78 ++++ .../lidar_apollo_segmentation_tvm_node.hpp | 54 +++ .../visibility_control.hpp | 37 ++ ...ar_apollo_segmentation_tvm_nodes.launch.py | 51 +++ ...r_apollo_segmentation_tvm_nodes.launch.xml | 13 + .../package.xml | 28 ++ .../param/test.param.yaml | 13 + .../lidar_apollo_segmentation_tvm_node.cpp | 72 ++++ .../test/launch.test.py | 61 +++ 27 files changed, 2369 insertions(+) create mode 100644 perception/lidar_apollo_segmentation_tvm/CMakeLists.txt create mode 100644 perception/lidar_apollo_segmentation_tvm/design/lidar-segmentation-design.md create mode 100644 perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp create mode 100644 perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/disjoint_set.hpp create mode 100644 perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp create mode 100644 perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp create mode 100644 perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp create mode 100644 perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp create mode 100644 perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp create mode 100644 perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/visibility_control.hpp create mode 100755 perception/lidar_apollo_segmentation_tvm/package.xml create mode 100644 perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp create mode 100644 perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp create mode 100644 perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp create mode 100644 perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp create mode 100644 perception/lidar_apollo_segmentation_tvm/src/log_table.cpp create mode 100644 perception/lidar_apollo_segmentation_tvm/test/main.cpp create mode 100644 perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt create mode 100644 perception/lidar_apollo_segmentation_tvm_nodes/design/lidar-segmentation-nodes-design.md create mode 100644 perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp create mode 100644 perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/visibility_control.hpp create mode 100644 perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py create mode 100644 perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml create mode 100755 perception/lidar_apollo_segmentation_tvm_nodes/package.xml create mode 100644 perception/lidar_apollo_segmentation_tvm_nodes/param/test.param.yaml create mode 100644 perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp create mode 100644 perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py diff --git a/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt b/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt new file mode 100644 index 0000000000000..ac3c4bd870769 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt @@ -0,0 +1,75 @@ +# Copyright 2021-2022 Arm Ltd. +# +# 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. + +cmake_minimum_required(VERSION 3.14) +project(lidar_apollo_segmentation_tvm) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(tvm_runtime_DIR ${tvm_vendor_DIR}) +find_package(tvm_runtime CONFIG REQUIRED) +find_package(PCL 1.10 REQUIRED) + +# Get target backend +set(${PROJECT_NAME}_BACKEND llvm CACHE STRING "${PROJECT_NAME} neural network backend") + +# Test if files exist. The result is set in baidu_cnn_FOUND +autoware_check_neural_network(baidu_cnn "${${PROJECT_NAME}_BACKEND}") +if(baidu_cnn_FOUND) + # Library + ament_auto_add_library(${PROJECT_NAME} SHARED + include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp + include/lidar_apollo_segmentation_tvm/cluster2d.hpp + include/lidar_apollo_segmentation_tvm/disjoint_set.hpp + include/lidar_apollo_segmentation_tvm/feature_generator.hpp + include/lidar_apollo_segmentation_tvm/feature_map.hpp + include/lidar_apollo_segmentation_tvm/log_table.hpp + include/lidar_apollo_segmentation_tvm/util.hpp + src/lidar_apollo_segmentation_tvm.cpp + src/cluster2d.cpp + src/feature_generator.cpp + src/feature_map.cpp + src/log_table.cpp + ) + + target_compile_options(${PROJECT_NAME} PRIVATE "-Wno-sign-conversion" "-Wno-conversion") + + target_compile_definitions(${PROJECT_NAME} PRIVATE NETWORKS_BACKEND=${${PROJECT_NAME}_BACKEND}) + + # Set includes as "SYSTEM" to avoid compiler errors on their headers. + target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${PCL_INCLUDE_DIRS}" + "${tvm_vendor_INCLUDE_DIRS}" + ) + + target_link_libraries(${PROJECT_NAME} + ${PCL_LIBRARIES} + ${tf2_ros_LIBRARIES} + ${tvm_runtime_LIBRARIES} + ) + + if(BUILD_TESTING) + # gtest + set(LIDAR_APOLLO_SEGMENTATION_TVM_GTEST lidar_apollo_segmentation_tvm_gtest) + ament_add_gtest(${LIDAR_APOLLO_SEGMENTATION_TVM_GTEST} test/main.cpp TIMEOUT 120) + target_include_directories(${LIDAR_APOLLO_SEGMENTATION_TVM_GTEST} PRIVATE "include") + target_link_libraries(${LIDAR_APOLLO_SEGMENTATION_TVM_GTEST} ${PROJECT_NAME}) + endif() + + ament_export_include_directories(${PCL_INCLUDE_DIRS}) + ament_auto_package() +else() + message(WARNING "Neural network not found, skipping package.") +endif() diff --git a/perception/lidar_apollo_segmentation_tvm/design/lidar-segmentation-design.md b/perception/lidar_apollo_segmentation_tvm/design/lidar-segmentation-design.md new file mode 100644 index 0000000000000..ead0672ffe408 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/design/lidar-segmentation-design.md @@ -0,0 +1,29 @@ +# lidar_apollo_segmentation_tvm {#lidar-apollo-segmentation-tvm-design} + +## Design + +### Usage {#lidar-apollo-segmentation-tvm-design-usage} + +#### Neural network + +This package will not build without a neural network for its inference. +The network is provided by the neural_networks_provider package. +See its design page for more information on how to enable downloading pre-compiled networks (by setting the `DOWNLOAD_ARTIFACTS` cmake variable), or how to handle user-compiled networks. + +#### Backend + +The backend used for the inference can be selected by setting the `lidar_apollo_segmentation_tvm_BACKEND` cmake variable. +The current available options are `llvm` for a CPU backend, and `vulkan` for a GPU backend. +It defaults to `llvm`. + +### Convolutional Neural Networks (CNN) Segmentation + +See the [original design](https://github.com/ApolloAuto/apollo/blob/3422a62ce932cb1c0c269922a0f1aa59a290b733/docs/specs/3d_obstacle_perception.md#convolutional-neural-networks-cnn-segmentation) by Apollo. +The paragraph of interest goes up to, but excluding, the "MinBox Builder" paragraph. +This package instead relies on further processing by a dedicated shape estimator. + +Note: the parameters described in the original design have been modified and are out of date. + +## Reference + +Lidar segmentation is based off a core algorithm by [Apollo](https://github.com/ApolloAuto/apollo/blob/master/docs/specs/3d_obstacle_perception.md), with modifications from [TierIV] () for the TVM backend. diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp new file mode 100644 index 0000000000000..5c4216edece9d --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp @@ -0,0 +1,180 @@ +// Copyright 2017-2022 Arm Ltd., TierIV, Autoware Foundation, The Apollo Authors +// +// 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 LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_ +#define LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +using autoware::common::types::bool8_t; +using autoware::common::types::char8_t; +using autoware::common::types::float32_t; + +/// \brief Internal obstacle classification categories. +enum class MetaType { + META_UNKNOWN, + META_SMALLMOT, + META_BIGMOT, + META_NONMOT, + META_PEDESTRIAN, + MAX_META_TYPE +}; + +/// \brief Internal obstacle representation. +struct Obstacle +{ + std::vector grids; + pcl::PointCloud::Ptr cloud_ptr; + float32_t score; + float32_t height; + float32_t heading; + MetaType meta_type; + std::vector meta_type_probs; + + Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(MetaType::META_UNKNOWN) + { + cloud_ptr.reset(new pcl::PointCloud); + meta_type_probs.assign(static_cast(MetaType::MAX_META_TYPE), 0.0); + } +}; + +/// \brief Handle the ouput of the CNN-based prediction by obtaining information on individual +/// cells. +class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D +{ +public: + /// \brief Constructor + /// \param[in] rows The number of rows in the cluster. + /// \param[in] cols The number of columns in the cluster. + /// \param[in] range Scaling factor. + explicit Cluster2D(int32_t rows, int32_t cols, float32_t range); + + /// \brief Construct a directed graph and search the connected components for candidate object + /// clusters. + /// \param[in] inferred_data Prediction information from the neural network inference. + /// \param[in] pc_ptr Input point cloud. + /// \param[in] valid_indices Indices of the points to consider in the point cloud. + /// \param[in] objectness_thresh Threshold for filtering out non-object cells. + /// \param[in] use_all_grids_for_clustering + void cluster( + const float32_t * inferred_data, const pcl::PointCloud::ConstPtr & pc_ptr, + const pcl::PointIndices & valid_indices, float32_t objectness_thresh, + bool8_t use_all_grids_for_clustering); + + /// \brief Populate the fields of obstacles_ elements. + /// \param[in] inferred_data Prediction information from the neural network inference. + void filter(const float32_t * inferred_data); + + /// \brief Assign a classification type to the obstacles_ elements. + /// \param[in] inferred_data Prediction information from the neural network inference. + void classify(const float32_t * inferred_data); + + /// \brief Remove the candidate clusters that don't meet the parameters' requirements. + /// \param[in] confidence_thresh The detection confidence score threshold. + /// \param[in] height_thresh If it is non-negative, the points that are higher than the predicted + /// object height by height_thresh are filtered out. + /// \param[in] min_pts_num The candidate clusters with less than min_pts_num points are removed. + /// \return The detected objects. + std::shared_ptr getObjects( + float32_t confidence_thresh, float32_t height_thresh, int32_t min_pts_num); + + /// \brief Transform an obstacle from the internal representation to the external one. + /// \param[in] in_obstacle + /// \return Output obstacle. + tier4_perception_msgs::msg::DetectedObjectWithFeature obstacleToObject( + const Obstacle & in_obstacle) const; + +private: + const int32_t rows_; + const int32_t cols_; + const int32_t siz_; + const float32_t range_; + const float32_t scale_; + const float32_t inv_res_x_; + const float32_t inv_res_y_; + std::vector point2grid_; + std::vector obstacles_; + std::vector id_img_; + + pcl::PointCloud::ConstPtr pc_ptr_; + const std::vector * valid_indices_in_pc_ = nullptr; + + /// \brief Node of a directed graph. + struct Node + { + Node * center_node; + Node * parent; + char8_t node_rank; + char8_t traversed; + bool8_t is_center; + bool8_t is_object; + int32_t point_num; + int32_t obstacle_id; + + Node() + { + center_node = nullptr; + parent = nullptr; + node_rank = 0; + traversed = 0; + is_center = false; + is_object = false; + point_num = 0; + obstacle_id = -1; + } + }; + + /// \brief Check whether a signed row and column values are valid array indices. + inline bool IsValidRowCol(int32_t row, int32_t col) const + { + return IsValidRow(row) && IsValidCol(col); + } + + /// \brief Check whether a signed row value is a valid array index. + inline bool IsValidRow(int32_t row) const { return row >= 0 && row < rows_; } + + /// \brief Check whether a signed column value is a valid array index. + inline bool IsValidCol(int32_t col) const { return col >= 0 && col < cols_; } + + /// \brief Transform a row and column coordinate to a linear grid index. + inline int32_t RowCol2Grid(int32_t row, int32_t col) const { return row * cols_ + col; } + + /// \brief Traverse the directed graph until visiting a node. + /// \param[in] x Node to visit. + void traverse(Node * x) const; +}; +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware +#endif // LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/disjoint_set.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/disjoint_set.hpp new file mode 100644 index 0000000000000..b86243a7e37e9 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/disjoint_set.hpp @@ -0,0 +1,84 @@ +// Copyright 2017-2022 Arm Ltd., The Apollo Authors +// +// 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 LIDAR_APOLLO_SEGMENTATION_TVM__DISJOINT_SET_HPP_ +#define LIDAR_APOLLO_SEGMENTATION_TVM__DISJOINT_SET_HPP_ + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +/// \brief Add a new element in a new set. +/// \param[inout] x The element to be added. +template +void DisjointSetMakeSet(T * x) +{ + x->parent = x; + x->node_rank = 0; +} + +/// \brief Recursively follow the chain of parent pointers from the input until reaching a root. +/// \param[inout] x The element which root is looked for. +/// \return The root of the set containing x. +template +T * DisjointSetFindRecursive(T * x) +{ + if (x->parent != x) { + x->parent = DisjointSetFindRecursive(x->parent); + } + return x->parent; +} + +/// \brief Find the root of the set x belongs to. +/// \param[inout] x The set element. +/// \return The root of the set containing x. +template +T * DisjointSetFind(T * x) +{ + T * y = x->parent; + if (y == x || y->parent == y) { + return y; + } + T * root = DisjointSetFindRecursive(y->parent); + x->parent = root; + y->parent = root; + return root; +} + +/// \brief Replace the set containing x and the set containing y with their union. +/// \param[inout] x An element of a first set. +/// \param[inout] y An element of a second set. +template +void DisjointSetUnion(T * x, T * y) +{ + x = DisjointSetFind(x); + y = DisjointSetFind(y); + if (x == y) { + return; + } + if (x->node_rank < y->node_rank) { + x->parent = y; + } else if (y->node_rank < x->node_rank) { + y->parent = x; + } else { + y->parent = x; + x->node_rank++; + } +} +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware +#endif // LIDAR_APOLLO_SEGMENTATION_TVM__DISJOINT_SET_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp new file mode 100644 index 0000000000000..ee880e8151be9 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp @@ -0,0 +1,71 @@ +// Copyright 2020-2022 Arm Ltd., TierIV +// +// 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 LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_ +#define LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include + +#include + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +using autoware::common::types::bool8_t; +using autoware::common::types::float32_t; + +/// \brief A FeatureMap generator based on channel feature information. +class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL FeatureGenerator +{ +private: + const bool8_t use_intensity_feature_; + const bool8_t use_constant_feature_; + const float32_t min_height_; + const float32_t max_height_; + std::shared_ptr map_ptr_; + +public: + /// \brief Constructor + /// \param[in] width The number of cells in X (column) axis of the 2D grid. + /// \param[in] height The number of cells in Y (row) axis of the 2D grid. + /// \param[in] range The range of the 2D grid. + /// \param[in] use_intensity_feature Enable input channel intensity feature. + /// \param[in] use_constant_feature Enable input channel constant feature. + /// \param[in] min_height The minimum height. + /// \param[in] max_height The maximum height. + explicit FeatureGenerator( + int32_t width, int32_t height, int32_t range, bool8_t use_intensity_feature, + bool8_t use_constant_feature, float32_t min_height, float32_t max_height); + + /// \brief Generate a FeatureMap based on the configured features of this object. + /// \param[in] pc_ptr Pointcloud used to populate the generated FeatureMap. + /// \return A shared pointer to the generated FeatureMap. + std::shared_ptr generate( + const pcl::PointCloud::ConstPtr & pc_ptr); +}; +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware +#endif // LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp new file mode 100644 index 0000000000000..f8525176a5675 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp @@ -0,0 +1,87 @@ +// Copyright 2020-2022 Arm Ltd., TierIV +// +// 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 LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_ +#define LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_ + +#include + +#include +#include + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +using autoware::common::types::float32_t; + +/// \brief Abstract interface for FeatureMap. +struct FeatureMapInterface +{ +public: + const int32_t channels; + const int32_t width; + const int32_t height; + const int32_t range; + float32_t * max_height_data; // channel 0 + float32_t * mean_height_data; // channel 1 + float32_t * count_data; // channel 2 + float32_t * direction_data; // channel 3 + float32_t * top_intensity_data; // channel 4 + float32_t * mean_intensity_data; // channel 5 + float32_t * distance_data; // channel 6 + float32_t * nonempty_data; // channel 7 + std::vector map_data; + virtual void initializeMap(std::vector & map) = 0; + virtual void resetMap(std::vector & map) = 0; + explicit FeatureMapInterface(int32_t _channels, int32_t _width, int32_t _height, int32_t _range); +}; + +/// \brief FeatureMap with no extra feature channels. +struct FeatureMap : public FeatureMapInterface +{ + explicit FeatureMap(int32_t width, int32_t height, int32_t range); + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; +}; + +/// \brief FeatureMap with an intensity feature channel. +struct FeatureMapWithIntensity : public FeatureMapInterface +{ + explicit FeatureMapWithIntensity(int32_t width, int32_t height, int32_t range); + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; +}; + +/// \brief FeatureMap with a constant feature channel. +struct FeatureMapWithConstant : public FeatureMapInterface +{ + explicit FeatureMapWithConstant(int32_t width, int32_t height, int32_t range); + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; +}; + +/// \brief FeatureMap with constant and intensity feature channels. +struct FeatureMapWithConstantAndIntensity : public FeatureMapInterface +{ + explicit FeatureMapWithConstantAndIntensity(int32_t width, int32_t height, int32_t range); + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; +}; +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware +#endif // LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp new file mode 100644 index 0000000000000..af8fc6eaf1b70 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp @@ -0,0 +1,198 @@ +// Copyright 2020-2022 Arm Ltd., TierIV +// +// 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 LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_ +#define LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +using autoware::common::types::bool8_t; +using autoware::common::types::float32_t; +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tvm_utility::pipeline::TVMArrayContainer; +using tvm_utility::pipeline::TVMArrayContainerVector; + +/// \brief Pre-precessing step of the TVM pipeline. +class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPreProcessor +: public tvm_utility::pipeline::PreProcessor::ConstPtr> +{ +public: + /// \brief Constructor. + /// \param[in] config The TVM configuration. + /// \param[in] range The range of the 2D grid. + /// \param[in] use_intensity_feature Enable input channel intensity feature. + /// \param[in] use_constant_feature Enable input channel constant feature. + /// \param[in] min_height The minimum height. + /// \param[in] max_height The maximum height. + explicit ApolloLidarSegmentationPreProcessor( + const tvm_utility::pipeline::InferenceEngineTVMConfig & config, int32_t range, + bool8_t use_intensity_feature, bool8_t use_constant_feature, float32_t min_height, + float32_t max_height); + + /// \brief Transfer the input data to a TVM array. + /// \param[in] pc_ptr Input pointcloud. + /// \return A TVM array containing the pointcloud data. + /// \throw std::runtime_error If the features are incorrectly configured. + TVMArrayContainerVector schedule(const pcl::PointCloud::ConstPtr & pc_ptr); + +private: + const int64_t input_channels; + const int64_t input_width; + const int64_t input_height; + const int64_t datatype_bytes; + const std::shared_ptr feature_generator; + TVMArrayContainer output; +}; + +/// \brief Post-precessing step of the TVM pipeline. +class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPostProcessor +: public tvm_utility::pipeline::PostProcessor> +{ +public: + /// \brief Constructor. + /// \param[in] config The TVM configuration. + /// \param[in] pc_ptr Pointcloud containing the initial input information to be matched against + /// the result of the inference. + /// \param[in] range The range of the 2D grid. + /// \param[in] objectness_thresh The threshold of objectness for filtering out non-object cells. + /// \param[in] score_threshold The detection confidence score threshold for filtering out the + /// candidate clusters. + /// \param[in] height_thresh If it is non-negative, the points that are higher than the predicted + /// object height by height_thresh are filtered out. + /// \param[in] min_pts_num The candidate clusters with less than min_pts_num points are removed. + explicit ApolloLidarSegmentationPostProcessor( + const tvm_utility::pipeline::InferenceEngineTVMConfig & config, + const pcl::PointCloud::ConstPtr & pc_ptr, int32_t range, + float32_t objectness_thresh, float32_t score_threshold, float32_t height_thresh, + int32_t min_pts_num); + + /// \brief Copy the inference result. + /// \param[in] input The result of the inference engine. + /// \return The inferred data. + std::shared_ptr schedule(const TVMArrayContainerVector & input); + +private: + const int64_t output_channels; + const int64_t output_width; + const int64_t output_height; + const int64_t datatype_bytes; + const float32_t objectness_thresh_; + const float32_t score_threshold_; + const float32_t height_thresh_; + const int32_t min_pts_num_; + const std::shared_ptr inferred_data; + const pcl::PointCloud::ConstPtr pc_ptr_; + const std::shared_ptr cluster2d_; +}; + +/// \brief Handle the neural network inference over the input point cloud. +class LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC ApolloLidarSegmentation +{ +public: + /// \brief Constructor + /// \param[in] range The range of the 2D grid. + /// \param[in] score_threshold The detection confidence score threshold for filtering out the + /// candidate clusters in the post-processing step. + /// \param[in] use_intensity_feature Enable input channel intensity feature. + /// \param[in] use_constant_feature Enable input channel constant feature. + /// \param[in] z_offset The offset to translate up the input pointcloud before the inference. + /// \param[in] min_height The minimum height. + /// \param[in] max_height The maximum height. + /// \param[in] objectness_thresh The threshold of objectness for filtering out non-object cells in + /// the obstacle clustering step. + /// \param[in] min_pts_num In the post-processing step, the candidate clusters with less than + /// min_pts_num points are removed. + /// \param[in] height_thresh If it is non-negative, the points that are higher than the predicted + /// object height by height_thresh are filtered out in the + /// post-processing step. + explicit ApolloLidarSegmentation( + int32_t range, float32_t score_threshold, bool8_t use_intensity_feature, + bool8_t use_constant_feature, float32_t z_offset, float32_t min_height, float32_t max_height, + float32_t objectness_thresh, int32_t min_pts_num, float32_t height_thresh); + + /// \brief Detect obstacles. + /// \param[in] input Input pointcloud. + /// \return Detected obstacles. + /// \throw tf2::TransformException If the pointcloud transformation fails. + /// \throw std::runtime_error If the features are incorrectly configured. + std::shared_ptr detectDynamicObjects( + const sensor_msgs::msg::PointCloud2 & input); + +private: + const int32_t range_; + const float32_t score_threshold_; + const float32_t z_offset_; + const float32_t objectness_thresh_; + const int32_t min_pts_num_; + const float32_t height_thresh_; + const pcl::PointCloud::Ptr pcl_pointcloud_ptr_; + + // Pipeline + using PrePT = ApolloLidarSegmentationPreProcessor; + using IET = tvm_utility::pipeline::InferenceEngineTVM; + using PostPT = ApolloLidarSegmentationPostProcessor; + + const std::shared_ptr PreP; + const std::shared_ptr IE; + const std::shared_ptr PostP; + + const std::shared_ptr> pipeline; + + /// \brief Move up the input pointcloud by z_offset_ and transform the pointcloud to target_frame_ + /// if needed. + /// \param[in] input + /// \param[out] transformed_cloud + /// \throw tf2::TransformException If the pointcloud transformation fails. + void LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL transformCloud( + const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud, + float32_t z_offset); + + rclcpp::Clock::SharedPtr clock_ = std::make_shared(RCL_ROS_TIME); + std::unique_ptr tf_buffer_ = std::make_unique(clock_); + + const std::string target_frame_ = "base_link"; +}; +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware +#endif // LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp new file mode 100644 index 0000000000000..7121e893795de --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp @@ -0,0 +1,35 @@ +// Copyright 2020-2022 Arm Ltd., TierIV +// +// 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 LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_ +#define LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_ + +#include + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +using autoware::common::types::float32_t; + +/// \brief Use a lookup table to compute the natural logarithm of 1+num. +/// \param[in] num +/// \return ln(1+num) +float32_t calcApproximateLog(float32_t num); +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware +#endif // LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp new file mode 100644 index 0000000000000..1b138f15a7ba5 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp @@ -0,0 +1,66 @@ +// Copyright 2017-2022 Arm Ltd., Autoware Foundation, The Apollo Authors +// +// 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 LIDAR_APOLLO_SEGMENTATION_TVM__UTIL_HPP_ +#define LIDAR_APOLLO_SEGMENTATION_TVM__UTIL_HPP_ + +#include + +#include +#include + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +using autoware::common::types::float32_t; + +/// \brief Project a point from a pointcloud to a 2D map. +/// \param[in] val Coordinate of the point in the pointcloud. +/// \param[in] ori Diameter of area containing the pointcloud. +/// \param[in] scale Scaling factor from pointcloud size to grid size. +/// \return The grid in which the point is. +inline int32_t F2I(float32_t val, float32_t ori, float32_t scale) +{ + return static_cast(std::floor((ori - val) * scale)); +} + +/// \brief Transform a pointcloud scale to a pixel scale. +/// \param[in] in_pc Coordinate of the point in the pointcloud. +/// \param[in] in_range Range of the pointcloud. +/// \param[in] out_size Size of the grid. +/// \return The distance to the point in pixel scale. +inline int32_t Pc2Pixel(float32_t in_pc, float32_t in_range, float32_t out_size) +{ + float32_t inv_res = 0.5f * out_size / in_range; + return static_cast(std::floor((in_range - in_pc) * inv_res)); +} + +/// \brief Transform a pixel scale to a pointcloud scale. +/// \param[in] in_pixel Coordinate of the cell in the grid. +/// \param[in] in_size Size of the grid. +/// \param[in] out_range Range of the pointcloud. +/// \return The distance to the cell in pointcloud scale. +inline float32_t Pixel2Pc(int32_t in_pixel, float32_t in_size, float32_t out_range) +{ + float32_t res = 2.0f * out_range / in_size; + return out_range - (static_cast(in_pixel) + 0.5f) * res; +} +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware + +#endif // LIDAR_APOLLO_SEGMENTATION_TVM__UTIL_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/visibility_control.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/visibility_control.hpp new file mode 100644 index 0000000000000..9696e7983c2ad --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2021-2022 Arm Ltd. +// +// 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 LIDAR_APOLLO_SEGMENTATION_TVM__VISIBILITY_CONTROL_HPP_ +#define LIDAR_APOLLO_SEGMENTATION_TVM__VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) +#if defined(LIDAR_APOLLO_SEGMENTATION_TVM_BUILDING_DLL) || \ + defined(LIDAR_APOLLO_SEGMENTATION_TVM_EXPORTS) +#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __declspec(dllexport) +#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL +#else +#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __declspec(dllimport) +#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL +#endif +#elif defined(__linux__) +#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __attribute__((visibility("default"))) +#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __attribute__((visibility("default"))) +#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL __attribute__((visibility("hidden"))) +#else +#error "Unsupported Build Configuration" +#endif + +#endif // LIDAR_APOLLO_SEGMENTATION_TVM__VISIBILITY_CONTROL_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/package.xml b/perception/lidar_apollo_segmentation_tvm/package.xml new file mode 100755 index 0000000000000..31dec39befbff --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/package.xml @@ -0,0 +1,36 @@ + + + + lidar_apollo_segmentation_tvm + 0.1.0 + lidar_apollo_segmentation_tvm + Ambroise Vincent + Apache 2.0 + + ament_cmake_auto + + autoware_cmake + libpcl-all-dev + + autoware_auto_common + autoware_auto_geometry + autoware_auto_perception_msgs + geometry_msgs + pcl_conversions + rclcpp + sensor_msgs + tf2_eigen + tf2_ros + tier4_perception_msgs + tvm_utility + tvm_vendor + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + libpcl-all-dev + + + ament_cmake + + diff --git a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp new file mode 100644 index 0000000000000..f0a6990a4db98 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp @@ -0,0 +1,364 @@ +// Copyright 2017-2022 Arm Ltd., TierIV, Autoware Foundation, The Apollo Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include +#include + +using Point = geometry_msgs::msg::Point32; +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +geometry_msgs::msg::Quaternion getQuaternionFromRPY(const double r, const double p, const double y) +{ + tf2::Quaternion q; + q.setRPY(r, p, y); + return tf2::toMsg(q); +} + +Cluster2D::Cluster2D(const int32_t rows, const int32_t cols, const float32_t range) +: rows_(rows), + cols_(cols), + siz_(rows * cols), + range_(range), + scale_(0.5f * static_cast(rows) / range), + inv_res_x_(0.5f * static_cast(cols) / range), + inv_res_y_(0.5f * static_cast(rows) / range) +{ + point2grid_.clear(); + id_img_.assign(siz_, -1); + pc_ptr_.reset(); + valid_indices_in_pc_ = nullptr; +} + +void Cluster2D::traverse(Node * x) const +{ + std::vector p; + p.clear(); + + while (x->traversed == 0) { + p.push_back(x); + x->traversed = 2; + x = x->center_node; + } + if (x->traversed == 2) { + for (int i = static_cast(p.size()) - 1; i >= 0 && p[i] != x; i--) { + p[i]->is_center = true; + } + x->is_center = true; + } + for (size_t i = 0; i < p.size(); i++) { + Node * y = p[i]; + y->traversed = 1; + y->parent = x->parent; + } +} + +void Cluster2D::cluster( + const float32_t * inferred_data, const pcl::PointCloud::ConstPtr & pc_ptr, + const pcl::PointIndices & valid_indices, float32_t objectness_thresh, + bool use_all_grids_for_clustering) +{ + const float32_t * category_pt_data = inferred_data; + const float32_t * instance_pt_x_data = inferred_data + siz_; + const float32_t * instance_pt_y_data = inferred_data + siz_ * 2; + + pc_ptr_ = pc_ptr; + + std::vector> nodes(rows_, std::vector(cols_, Node())); + + valid_indices_in_pc_ = &(valid_indices.indices); + point2grid_.assign(valid_indices_in_pc_->size(), -1); + + for (size_t i = 0; i < valid_indices_in_pc_->size(); ++i) { + int32_t point_id = valid_indices_in_pc_->at(i); + const auto & point = pc_ptr_->points[point_id]; + // * the coordinates of x and y have been exchanged in feature generation + // step, + // so we swap them back here. + int32_t pos_x = F2I(point.y, range_, inv_res_x_); // col + int32_t pos_y = F2I(point.x, range_, inv_res_y_); // row + if (IsValidRowCol(pos_y, pos_x)) { + point2grid_[i] = RowCol2Grid(pos_y, pos_x); + nodes[pos_y][pos_x].point_num++; + } + } + + for (int32_t row = 0; row < rows_; ++row) { + for (int32_t col = 0; col < cols_; ++col) { + int32_t grid = RowCol2Grid(row, col); + Node * node = &nodes[row][col]; + DisjointSetMakeSet(node); + node->is_object = (use_all_grids_for_clustering || nodes[row][col].point_num > 0) && + (*(category_pt_data + grid) >= objectness_thresh); + int32_t center_row = + row + static_cast(std::round(instance_pt_x_data[grid] * scale_)); + int32_t center_col = + col + static_cast(std::round(instance_pt_y_data[grid] * scale_)); + center_row = std::min(std::max(center_row, 0), rows_ - 1); + center_col = std::min(std::max(center_col, 0), cols_ - 1); + node->center_node = &nodes[center_row][center_col]; + } + } + + for (int32_t row = 0; row < rows_; ++row) { + for (int32_t col = 0; col < cols_; ++col) { + Node * node = &nodes[row][col]; + if (node->is_object && node->traversed == 0) { + traverse(node); + } + } + } + + for (int32_t row = 0; row < rows_; ++row) { + for (int32_t col = 0; col < cols_; ++col) { + Node * node = &nodes[row][col]; + if (!node->is_center) { + continue; + } + for (int32_t row2 = row - 1; row2 <= row + 1; ++row2) { + for (int32_t col2 = col - 1; col2 <= col + 1; ++col2) { + if ((row2 == row || col2 == col) && IsValidRowCol(row2, col2)) { + Node * node2 = &nodes[row2][col2]; + if (node2->is_center) { + DisjointSetUnion(node, node2); + } + } + } + } + } + } + + int32_t count_obstacles = 0; + obstacles_.clear(); + id_img_.assign(siz_, -1); + for (int32_t row = 0; row < rows_; ++row) { + for (int32_t col = 0; col < cols_; ++col) { + Node * node = &nodes[row][col]; + if (!node->is_object) { + continue; + } + Node * root = DisjointSetFind(node); + if (root->obstacle_id < 0) { + root->obstacle_id = count_obstacles++; + obstacles_.push_back(Obstacle()); + } + int32_t grid = RowCol2Grid(row, col); + id_img_[grid] = root->obstacle_id; + obstacles_[root->obstacle_id].grids.push_back(grid); + } + } + filter(inferred_data); + classify(inferred_data); +} + +void Cluster2D::filter(const float32_t * inferred_data) +{ + const float32_t * confidence_pt_data = inferred_data + siz_ * 3; + const float32_t * heading_pt_x_data = inferred_data + siz_ * 9; + const float32_t * heading_pt_y_data = inferred_data + siz_ * 10; + const float32_t * height_pt_data = inferred_data + siz_ * 11; + + for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { + Obstacle * obs = &obstacles_[obstacle_id]; + float64_t score = 0.0; + float64_t height = 0.0; + float64_t vec_x = 0.0; + float64_t vec_y = 0.0; + for (int32_t grid : obs->grids) { + score += static_cast(confidence_pt_data[grid]); + height += static_cast(height_pt_data[grid]); + vec_x += heading_pt_x_data[grid]; + vec_y += heading_pt_y_data[grid]; + } + obs->score = static_cast(score / static_cast(obs->grids.size())); + obs->height = static_cast(height / static_cast(obs->grids.size())); + obs->heading = static_cast(std::atan2(vec_y, vec_x) * 0.5); + obs->cloud_ptr.reset(new pcl::PointCloud); + } +} + +void Cluster2D::classify(const float32_t * inferred_data) +{ + const float32_t * classify_pt_data = inferred_data + siz_ * 4; + int num_classes = static_cast(MetaType::MAX_META_TYPE); + for (size_t obs_id = 0; obs_id < obstacles_.size(); obs_id++) { + Obstacle * obs = &obstacles_[obs_id]; + + for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) { + int32_t grid = obs->grids[grid_id]; + for (int k = 0; k < num_classes; k++) { + obs->meta_type_probs[k] += classify_pt_data[k * siz_ + grid]; + } + } + int meta_type_id = 0; + for (int k = 0; k < num_classes; k++) { + obs->meta_type_probs[k] /= static_cast(obs->grids.size()); + if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) { + meta_type_id = k; + } + } + obs->meta_type = static_cast(meta_type_id); + } +} + +tier4_perception_msgs::msg::DetectedObjectWithFeature Cluster2D::obstacleToObject( + const Obstacle & in_obstacle) const +{ + using autoware_auto_perception_msgs::msg::DetectedObjectKinematics; + using autoware_auto_perception_msgs::msg::ObjectClassification; + + tier4_perception_msgs::msg::DetectedObjectWithFeature resulting_object; + + resulting_object.object.classification.emplace_back( + autoware_auto_perception_msgs::build() + .label(ObjectClassification::UNKNOWN) + .probability(in_obstacle.score)); + if (in_obstacle.meta_type == MetaType::META_PEDESTRIAN) { + resulting_object.object.classification.front().label = ObjectClassification::PEDESTRIAN; + } else if (in_obstacle.meta_type == MetaType::META_NONMOT) { + resulting_object.object.classification.front().label = ObjectClassification::MOTORCYCLE; + } else if (in_obstacle.meta_type == MetaType::META_SMALLMOT) { + resulting_object.object.classification.front().label = ObjectClassification::CAR; + } else if (in_obstacle.meta_type == MetaType::META_BIGMOT) { + resulting_object.object.classification.front().label = ObjectClassification::BUS; + } else { + resulting_object.object.classification.front().label = ObjectClassification::UNKNOWN; + } + + pcl::PointXYZ min_point; + pcl::PointXYZ max_point; + for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end(); + ++pit) { + if (pit->x < min_point.x) { + min_point.x = pit->x; + } + if (pit->y < min_point.y) { + min_point.y = pit->y; + } + if (pit->z < min_point.z) { + min_point.z = pit->z; + } + if (pit->x > max_point.x) { + max_point.x = pit->x; + } + if (pit->y > max_point.y) { + max_point.y = pit->y; + } + if (pit->z > max_point.z) { + max_point.z = pit->z; + } + } + + // cluster and ground filtering + pcl::PointCloud cluster; + const float min_height = min_point.z + ((max_point.z - min_point.z) * 0.1f); + for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end(); + ++pit) { + if (min_height < pit->z) { + cluster.points.push_back(*pit); + } + } + min_point.z = 0.0; + max_point.z = 0.0; + for (auto pit = cluster.points.begin(); pit != cluster.points.end(); ++pit) { + if (pit->z < min_point.z) { + min_point.z = pit->z; + } + if (pit->z > max_point.z) { + max_point.z = pit->z; + } + } + sensor_msgs::msg::PointCloud2 ros_pc; + pcl::toROSMsg(cluster, ros_pc); + resulting_object.feature.cluster = ros_pc; + + // position + const float height = max_point.z - min_point.z; + const float length = max_point.x - min_point.x; + const float width = max_point.y - min_point.y; + resulting_object.object.kinematics.pose_with_covariance.pose.position.x = + min_point.x + length / 2; + resulting_object.object.kinematics.pose_with_covariance.pose.position.y = min_point.y + width / 2; + resulting_object.object.kinematics.pose_with_covariance.pose.position.z = + min_point.z + height / 2; + + resulting_object.object.kinematics.pose_with_covariance.pose.orientation = + getQuaternionFromRPY(0.0, 0.0, in_obstacle.heading); + resulting_object.object.kinematics.orientation_availability = + DetectedObjectKinematics::SIGN_UNKNOWN; + + return resulting_object; +} + +std::shared_ptr Cluster2D::getObjects( + const float32_t confidence_thresh, const float32_t height_thresh, const int32_t min_pts_num) +{ + auto object_array = std::make_shared(); + + for (size_t i = 0; i < point2grid_.size(); ++i) { + int32_t grid = point2grid_[i]; + if (grid < 0) { + continue; + } + + int32_t obstacle_id = id_img_[grid]; + + int32_t point_id = valid_indices_in_pc_->at(i); + + if (obstacle_id >= 0 && obstacles_[obstacle_id].score >= confidence_thresh) { + if ( + height_thresh < 0 || + pc_ptr_->points[point_id].z <= obstacles_[obstacle_id].height + height_thresh) { + obstacles_[obstacle_id].cloud_ptr->push_back(pc_ptr_->points[point_id]); + } + } + } + + for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { + Obstacle * obs = &obstacles_[obstacle_id]; + if (static_cast(obs->cloud_ptr->size()) < min_pts_num) { + continue; + } + tier4_perception_msgs::msg::DetectedObjectWithFeature out_obj = obstacleToObject(*obs); + object_array->feature_objects.push_back(out_obj); + } + + return object_array; +} +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp b/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp new file mode 100644 index 0000000000000..c103af1b40646 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp @@ -0,0 +1,116 @@ +// Copyright 2020-2022 Arm Ltd., TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include + +using autoware::common::types::bool8_t; +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +namespace +{ +inline float32_t normalizeIntensity(float32_t intensity) { return intensity / 255; } +} // namespace + +FeatureGenerator::FeatureGenerator( + const int32_t width, const int32_t height, const int32_t range, + const bool8_t use_intensity_feature, const bool8_t use_constant_feature, + const float32_t min_height, const float32_t max_height) +: use_intensity_feature_(use_intensity_feature), + use_constant_feature_(use_constant_feature), + min_height_(min_height), + max_height_(max_height) +{ + // select feature map type + if (use_constant_feature && use_intensity_feature) { + map_ptr_ = std::make_shared(width, height, range); + } else if (use_constant_feature) { + map_ptr_ = std::make_shared(width, height, range); + } else if (use_intensity_feature) { + map_ptr_ = std::make_shared(width, height, range); + } else { + map_ptr_ = std::make_shared(width, height, range); + } + map_ptr_->initializeMap(map_ptr_->map_data); +} + +std::shared_ptr FeatureGenerator::generate( + const pcl::PointCloud::ConstPtr & pc_ptr) +{ + const float64_t epsilon = 1e-6; + map_ptr_->resetMap(map_ptr_->map_data); + + const int32_t size = map_ptr_->height * map_ptr_->width; + + const float32_t inv_res_x = 0.5f * map_ptr_->width / map_ptr_->range; + const float32_t inv_res_y = 0.5f * map_ptr_->height / map_ptr_->range; + + for (size_t i = 0; i < pc_ptr->points.size(); ++i) { + if (pc_ptr->points[i].z <= min_height_ || max_height_ <= pc_ptr->points[i].z) { + continue; + } + + // x on grid + const int32_t pos_x = static_cast( + std::floor((static_cast(map_ptr_->range) - pc_ptr->points[i].y) * inv_res_x)); + // y on grid + const int32_t pos_y = static_cast( + std::floor((static_cast(map_ptr_->range) - pc_ptr->points[i].x) * inv_res_y)); + if (pos_x < 0 || map_ptr_->width <= pos_x || pos_y < 0 || map_ptr_->height <= pos_y) { + continue; + } + + const int32_t idx = pos_y * map_ptr_->width + pos_x; + + if (map_ptr_->max_height_data[idx] < pc_ptr->points[i].z) { + map_ptr_->max_height_data[idx] = pc_ptr->points[i].z; + if (map_ptr_->top_intensity_data != nullptr) { + map_ptr_->top_intensity_data[idx] = normalizeIntensity(pc_ptr->points[i].intensity); + } + } + map_ptr_->mean_height_data[idx] += pc_ptr->points[i].z; + if (map_ptr_->mean_intensity_data != nullptr) { + map_ptr_->mean_intensity_data[idx] += normalizeIntensity(pc_ptr->points[i].intensity); + } + map_ptr_->count_data[idx] += 1.0f; + } + + for (int32_t i = 0; i < size; ++i) { + if (static_cast(map_ptr_->count_data[i]) < epsilon) { + map_ptr_->max_height_data[i] = 0.0f; + } else { + map_ptr_->mean_height_data[i] /= map_ptr_->count_data[i]; + if (map_ptr_->mean_intensity_data != nullptr) { + map_ptr_->mean_intensity_data[i] /= map_ptr_->count_data[i]; + } + map_ptr_->nonempty_data[i] = 1.0f; + } + map_ptr_->count_data[i] = calcApproximateLog(map_ptr_->count_data[i]); + } + return map_ptr_; +} +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp b/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp new file mode 100644 index 0000000000000..959561965f657 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp @@ -0,0 +1,184 @@ +// Copyright 2020-2022 Arm Ltd., TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include + +using autoware::common::types::float32_t; + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +FeatureMapInterface::FeatureMapInterface( + const int32_t _channels, const int32_t _width, const int32_t _height, const int32_t _range) +: channels(_channels), + width(_width), + height(_height), + range(_range), + max_height_data(nullptr), + mean_height_data(nullptr), + count_data(nullptr), + direction_data(nullptr), + top_intensity_data(nullptr), + mean_intensity_data(nullptr), + distance_data(nullptr), + nonempty_data(nullptr) +{ + map_data.resize(width * height * channels); +} + +FeatureMap::FeatureMap(const int32_t width, const int32_t height, const int32_t range) +: FeatureMapInterface(4, width, height, range) +{ + max_height_data = &(map_data[0]) + width * height * 0; + mean_height_data = &(map_data[0]) + width * height * 1; + count_data = &(map_data[0]) + width * height * 2; + nonempty_data = &(map_data[0]) + width * height * 3; +} +void FeatureMap::initializeMap(std::vector & map) { (void)map; } +void FeatureMap::resetMap(std::vector & map) +{ + const int32_t size = width * height; + (void)map; + for (int32_t i = 0; i < size; ++i) { + max_height_data[i] = -5.0f; + mean_height_data[i] = 0.0f; + count_data[i] = 0.0f; + nonempty_data[i] = 0.0f; + } +} + +FeatureMapWithIntensity::FeatureMapWithIntensity( + const int32_t width, const int32_t height, const int32_t range) +: FeatureMapInterface(6, width, height, range) +{ + max_height_data = &(map_data[0]) + width * height * 0; + mean_height_data = &(map_data[0]) + width * height * 1; + count_data = &(map_data[0]) + width * height * 2; + top_intensity_data = &(map_data[0]) + width * height * 3; + mean_intensity_data = &(map_data[0]) + width * height * 4; + nonempty_data = &(map_data[0]) + width * height * 5; +} +void FeatureMapWithIntensity::initializeMap(std::vector & map) { (void)map; } +void FeatureMapWithIntensity::resetMap(std::vector & map) +{ + const int32_t size = width * height; + (void)map; + for (int32_t i = 0; i < size; ++i) { + max_height_data[i] = -5.0f; + mean_height_data[i] = 0.0f; + count_data[i] = 0.0f; + top_intensity_data[i] = 0.0f; + mean_intensity_data[i] = 0.0f; + nonempty_data[i] = 0.0f; + } +} + +FeatureMapWithConstant::FeatureMapWithConstant( + const int32_t width, const int32_t height, const int32_t range) +: FeatureMapInterface(6, width, height, range) +{ + max_height_data = &(map_data[0]) + width * height * 0; + mean_height_data = &(map_data[0]) + width * height * 1; + count_data = &(map_data[0]) + width * height * 2; + direction_data = &(map_data[0]) + width * height * 3; + distance_data = &(map_data[0]) + width * height * 4; + nonempty_data = &(map_data[0]) + width * height * 5; +} +void FeatureMapWithConstant::initializeMap(std::vector & map) +{ + (void)map; + for (int32_t row = 0; row < height; ++row) { + for (int32_t col = 0; col < width; ++col) { + int32_t idx = row * width + col; + // * row <-> x, column <-> y + // return the distance from the car to center of the grid. + // Pc means point cloud = real world scale. so transform pixel scale to + // real world scale + float32_t center_x = Pixel2Pc(row, height, range); + float32_t center_y = Pixel2Pc(col, width, range); + // normalization. -0.5~0.5 + direction_data[idx] = std::atan2(center_y, center_x) / static_cast(2.0 * M_PI); + distance_data[idx] = std::hypot(center_x, center_y) / 60.0f - 0.5f; + } + } +} + +void FeatureMapWithConstant::resetMap(std::vector & map) +{ + const int32_t size = width * height; + (void)map; + for (int32_t i = 0; i < size; ++i) { + max_height_data[i] = -5.0f; + mean_height_data[i] = 0.0f; + count_data[i] = 0.0f; + nonempty_data[i] = 0.0f; + } +} + +FeatureMapWithConstantAndIntensity::FeatureMapWithConstantAndIntensity( + const int32_t width, const int32_t height, const int32_t range) +: FeatureMapInterface(8, width, height, range) +{ + max_height_data = &(map_data[0]) + width * height * 0; + mean_height_data = &(map_data[0]) + width * height * 1; + count_data = &(map_data[0]) + width * height * 2; + direction_data = &(map_data[0]) + width * height * 3; + top_intensity_data = &(map_data[0]) + width * height * 4; + mean_intensity_data = &(map_data[0]) + width * height * 5; + distance_data = &(map_data[0]) + width * height * 6; + nonempty_data = &(map_data[0]) + width * height * 7; +} +void FeatureMapWithConstantAndIntensity::initializeMap(std::vector & map) +{ + (void)map; + for (int32_t row = 0; row < height; ++row) { + for (int32_t col = 0; col < width; ++col) { + int32_t idx = row * width + col; + // * row <-> x, column <-> y + // return the distance from the car to center of the grid. + // Pc means point cloud = real world scale. so transform pixel scale to + // real world scale + float32_t center_x = Pixel2Pc(row, height, range); + float32_t center_y = Pixel2Pc(col, width, range); + // normalization. -0.5~0.5 + direction_data[idx] = std::atan2(center_y, center_x) / static_cast(2.0 * M_PI); + distance_data[idx] = std::hypot(center_x, center_y) / 60.0f - 0.5f; + } + } +} + +void FeatureMapWithConstantAndIntensity::resetMap(std::vector & map) +{ + const int32_t size = width * height; + (void)map; + for (int32_t i = 0; i < size; ++i) { + max_height_data[i] = -5.0f; + mean_height_data[i] = 0.0f; + count_data[i] = 0.0f; + top_intensity_data[i] = 0.0f; + mean_intensity_data[i] = 0.0f; + nonempty_data[i] = 0.0f; + } +} +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp new file mode 100644 index 0000000000000..134f955127c30 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -0,0 +1,197 @@ +// Copyright 2020-2022 Arm Ltd., TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include +#include + +using autoware::common::types::bool8_t; +using autoware::common::types::float32_t; +using model_zoo::perception::lidar_obstacle_detection::baidu_cnn::onnx_bcnn::config; + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +ApolloLidarSegmentationPreProcessor::ApolloLidarSegmentationPreProcessor( + const tvm_utility::pipeline::InferenceEngineTVMConfig & config, int32_t range, + bool8_t use_intensity_feature, bool8_t use_constant_feature, float32_t min_height, + float32_t max_height) +: input_channels(config.network_inputs[0].second[1]), + input_width(config.network_inputs[0].second[2]), + input_height(config.network_inputs[0].second[3]), + datatype_bytes(config.tvm_dtype_bits / 8), + feature_generator(std::make_shared( + input_width, input_height, range, use_intensity_feature, use_constant_feature, min_height, + max_height)) +{ + // Allocate input variable + std::vector shape_x{1, input_channels, input_width, input_height}; + TVMArrayContainer x{ + shape_x, + config.tvm_dtype_code, + config.tvm_dtype_bits, + config.tvm_dtype_lanes, + config.tvm_device_type, + config.tvm_device_id}; + output = x; +} + +TVMArrayContainerVector ApolloLidarSegmentationPreProcessor::schedule( + const pcl::PointCloud::ConstPtr & pc_ptr) +{ + // generate feature map + std::shared_ptr feature_map_ptr = feature_generator->generate(pc_ptr); + + if (feature_map_ptr->channels < input_channels) { + throw std::runtime_error("schedule: incorrect feature configuration"); + } + + TVMArrayCopyFromBytes( + output.getArray(), feature_map_ptr->map_data.data(), + input_channels * input_height * input_width * datatype_bytes); + + return {output}; +} + +ApolloLidarSegmentationPostProcessor::ApolloLidarSegmentationPostProcessor( + const tvm_utility::pipeline::InferenceEngineTVMConfig & config, + const pcl::PointCloud::ConstPtr & pc_ptr, int32_t range, + float32_t objectness_thresh, float32_t score_threshold, float32_t height_thresh, + int32_t min_pts_num) +: output_channels(config.network_outputs[0].second[1]), + output_width(config.network_outputs[0].second[2]), + output_height(config.network_outputs[0].second[3]), + datatype_bytes(config.tvm_dtype_bits / 8), + objectness_thresh_(objectness_thresh), + score_threshold_(score_threshold), + height_thresh_(height_thresh), + min_pts_num_(min_pts_num), + inferred_data(std::shared_ptr( + new float32_t[output_channels * output_width * output_height], + std::default_delete())), + pc_ptr_(pc_ptr), + cluster2d_(std::make_shared(output_width, output_height, range)) +{ +} + +std::shared_ptr ApolloLidarSegmentationPostProcessor::schedule( + const TVMArrayContainerVector & input) +{ + pcl::PointIndices valid_idx; + valid_idx.indices.resize(pc_ptr_->size()); + std::iota(valid_idx.indices.begin(), valid_idx.indices.end(), 0); + cluster2d_->cluster( + static_cast(input[0].getArray()->data), pc_ptr_, valid_idx, objectness_thresh_, + true /*use all grids for clustering*/); + auto object_array = cluster2d_->getObjects(score_threshold_, height_thresh_, min_pts_num_); + + return object_array; +} + +ApolloLidarSegmentation::ApolloLidarSegmentation( + int32_t range, float32_t score_threshold, bool8_t use_intensity_feature, + bool8_t use_constant_feature, float32_t z_offset, float32_t min_height, float32_t max_height, + float32_t objectness_thresh, int32_t min_pts_num, float32_t height_thresh) +: range_(range), + score_threshold_(score_threshold), + z_offset_(z_offset), + objectness_thresh_(objectness_thresh), + min_pts_num_(min_pts_num), + height_thresh_(height_thresh), + pcl_pointcloud_ptr_(new pcl::PointCloud), + PreP(std::make_shared( + config, range, use_intensity_feature, use_constant_feature, min_height, max_height)), + IE(std::make_shared(config)), + PostP(std::make_shared( + config, pcl_pointcloud_ptr_, range, objectness_thresh, score_threshold, height_thresh, + min_pts_num)), + pipeline( + std::make_shared>(*PreP, *IE, *PostP)) +{ +} + +void ApolloLidarSegmentation::transformCloud( + const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud, + float32_t z_offset) +{ + if (target_frame_ == input.header.frame_id && z_offset == 0) { + transformed_cloud = input; + } else { + pcl::PointCloud in_cluster, transformed_cloud_cluster; + pcl::fromROSMsg(input, in_cluster); + + // transform pointcloud to target_frame + if (target_frame_ != input.header.frame_id) { + geometry_msgs::msg::TransformStamped transform_stamped; + builtin_interfaces::msg::Time time_stamp = input.header.stamp; + const tf2::TimePoint time_point = tf2::TimePoint( + std::chrono::seconds(time_stamp.sec) + std::chrono::nanoseconds(time_stamp.nanosec)); + transform_stamped = + tf_buffer_->lookupTransform(target_frame_, input.header.frame_id, time_point); + Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl::transformPointCloud(in_cluster, transformed_cloud_cluster, affine_matrix); + transformed_cloud_cluster.header.frame_id = target_frame_; + } else { + transformed_cloud_cluster = in_cluster; + } + + // move pointcloud z_offset in z axis + if (z_offset != 0) { + Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); + Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); + pcl::transformPointCloud( + transformed_cloud_cluster, transformed_cloud_cluster, z_up_transform); + } + + pcl::toROSMsg(transformed_cloud_cluster, transformed_cloud); + } +} + +std::shared_ptr ApolloLidarSegmentation::detectDynamicObjects( + const sensor_msgs::msg::PointCloud2 & input) +{ + // move up pointcloud z_offset in z axis + sensor_msgs::msg::PointCloud2 transformed_cloud; + ApolloLidarSegmentation::transformCloud(input, transformed_cloud, z_offset_); + // convert from ros to pcl + pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_ptr_); + + // inference pipeline + auto output = pipeline->schedule(pcl_pointcloud_ptr_); + for (auto & feature_object : output->feature_objects) { + feature_object.feature.cluster.header = input.header; + } + output->header = input.header; + + // move down pointcloud z_offset in z axis + for (std::size_t i = 0; i < output->feature_objects.size(); i++) { + sensor_msgs::msg::PointCloud2 transformed_cloud; + transformCloud(output->feature_objects.at(i).feature.cluster, transformed_cloud, -z_offset_); + output->feature_objects.at(i).feature.cluster = transformed_cloud; + } + + return output; +} +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp b/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp new file mode 100644 index 0000000000000..3389f753364bf --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp @@ -0,0 +1,56 @@ +// Copyright 2020-2022 Arm Ltd., TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include + +using autoware::common::types::float32_t; + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm +{ +struct LogTable +{ + std::vector data; + LogTable() + { + data.resize(256 * 10); + for (size_t i = 0; i < data.size(); ++i) { + data[i] = std::log1p(static_cast(i) / 10); + } + } +}; + +static LogTable log_table; + +float32_t calcApproximateLog(float32_t num) +{ + if (num >= 0) { + size_t integer_num = static_cast(num * 10.0f); + if (integer_num < log_table.data.size()) { + return log_table.data[integer_num]; + } + } + return std::log(1.0f + num); +} +} // namespace lidar_apollo_segmentation_tvm +} // namespace perception +} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/test/main.cpp b/perception/lidar_apollo_segmentation_tvm/test/main.cpp new file mode 100644 index 0000000000000..0306cddf0b8ed --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm/test/main.cpp @@ -0,0 +1,96 @@ +// Copyright 2021-2022 Arm Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include + +#include +#include +#include +#include + +using autoware::common::types::float32_t; +using autoware::common::types::uchar8_t; +using autoware::perception::lidar_apollo_segmentation_tvm::ApolloLidarSegmentation; + +void test_segmentation(bool use_intensity_feature, bool use_constant_feature, bool expect_throw) +{ + // Instantiate the pipeline + const int width = 1; + const int height = 10000; + const int range = 70; + const float score_threshold = 0.8f; + const float z_offset = 1.0f; + const float32_t min_height = -5.0f; + const float32_t max_height = 5.0f; + const float32_t objectness_thresh = 0.5f; + const int32_t min_pts_num = 3; + const float32_t height_thresh = 0.5f; + ApolloLidarSegmentation segmentation( + range, score_threshold, use_intensity_feature, use_constant_feature, z_offset, min_height, + max_height, objectness_thresh, min_pts_num, height_thresh); + + std::random_device rd; + std::mt19937 gen(42); + std::uniform_real_distribution dis(-50.0, 50.0); + std::vector v(width * height * sizeof(float32_t) * 4); + for (size_t i = 0; i < width * height * 4; i++) { + reinterpret_cast(v.data())[i] = dis(gen); + } + + sensor_msgs::msg::PointCloud2 input{}; + input.header.frame_id = "base_link"; + input.fields.resize(4U); + input.fields[0U].name = "x"; + input.fields[1U].name = "y"; + input.fields[2U].name = "z"; + input.fields[3U].name = "intensity"; + for (uint32_t idx = 0U; idx < 4U; ++idx) { + input.fields[idx].offset = static_cast(idx * sizeof(float32_t)); + input.fields[idx].datatype = sensor_msgs::msg::PointField::FLOAT32; + input.fields[idx].count = 1U; + input.point_step += static_cast(sizeof(float32_t)); + } + input.height = static_cast(height); + input.width = static_cast(width); + input.is_bigendian = false; + input.row_step = input.point_step * input.width; + input.is_dense = false; + input.data = v; + + std::shared_ptr output; + bool has_thrown = false; + try { + output = segmentation.detectDynamicObjects(input); + } catch (const std::exception & e) { + has_thrown = true; + } + EXPECT_EQ(expect_throw, has_thrown); +} + +// Test configuration matching the default-provided network. +TEST(lidar_apollo_segmentation_tvm, sanity) { test_segmentation(true, false, false); } + +// Test that configuring ApolloLidarSegmentation with too few channels results in an error. +TEST(lidar_apollo_segmentation_tvm, runtime_error) { test_segmentation(false, false, true); } + +// Other test configurations to increase code coverage. +TEST(lidar_apollo_segmentation_tvm, others) +{ + test_segmentation(false, true, false); + test_segmentation(true, true, false); +} diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt b/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt new file mode 100644 index 0000000000000..8a97a1ef05d5a --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt @@ -0,0 +1,51 @@ +# Copyright 2021-2022 Arm Ltd. +# +# 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. + +cmake_minimum_required(VERSION 3.14) +project(lidar_apollo_segmentation_tvm_nodes) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Check that a neural network was available for lidar_apollo_segmentation_tvm to be built. +if(lidar_apollo_segmentation_tvm_FOUND) + # Set lidar_apollo_segmentation_tvm includes as "SYSTEM" to ignore compiler errors on PCL headers + include_directories(SYSTEM "${lidar_apollo_segmentation_tvm_INCLUDE_DIRS}") + + # Library + ament_auto_add_library(${PROJECT_NAME} SHARED + include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp + src/lidar_apollo_segmentation_tvm_node.cpp + ) + target_link_libraries(${PROJECT_NAME} ${lidar_apollo_segmentation_tvm_LIBRARIES}) + + rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::perception::lidar_apollo_segmentation_tvm_nodes::ApolloLidarSegmentationNode" + EXECUTABLE ${PROJECT_NAME}_exe + ) + + if(BUILD_TESTING) + add_ros_test( + test/launch.test.py + TIMEOUT "30" + ) + endif() + + ament_auto_package(INSTALL_TO_SHARE + launch + param + ) +else() + message(WARNING "lidar_apollo_segmentation_tvm not found, skipping package.") +endif() diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/design/lidar-segmentation-nodes-design.md b/perception/lidar_apollo_segmentation_tvm_nodes/design/lidar-segmentation-nodes-design.md new file mode 100644 index 0000000000000..4a2d91e466517 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/design/lidar-segmentation-nodes-design.md @@ -0,0 +1,78 @@ +# lidar_apollo_segmentation_tvm_nodes {#lidar-apollo-segmentation-tvm-nodes-design} + +## Purpose / Use cases + +An alternative to Euclidean clustering. +This node detects and labels foreground obstacles (e.g. cars, motorcycles, pedestrians) from a point +cloud, using a neural network. + +## Design + +See the design of the algorithm in the core (lidar_apollo_segmentation_tvm) package's design documents. + +### Usage + +`lidar_apollo_segmentation_tvm` and `lidar_apollo_segmentation_tvm_nodes` will not build without a neural network. +See the lidar_apollo_segmentation_tvm usage for more information. + +### Assumptions / Known limits + +The original node from Apollo has a Region Of Interest (ROI) filter. +This has the benefit of working with a filtered point cloud that includes only the points inside the +ROI (i.e., the drivable road and junction areas) with most of the background obstacles removed (such +as buildings and trees around the road region). +Not having this filter may negatively impact performance. + +### Inputs / Outputs / API + +#### Inputs + +The input are non-ground points as a PointCloud2 message from the sensor_msgs package. + +#### Outputs + +The output is a [DetectedObjectsWithFeature](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_perception_msgs/msg/object_recognition/DetectedObjectsWithFeature.msg). + +#### Parameters + +| Parameter | Type | Description | Default | +| ----------------------- | ------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------- | +| `range` | _int_ | The range of the 2D grid with respect to the origin (the LiDAR sensor). Unit: meter | `70` | +| `score_threshold` | _float_ | The detection confidence score threshold for filtering out the candidate clusters in the post-processing step. | `0.8` | +| `use_intensity_feature` | _bool_ | Enable input channel intensity feature. | `true` | +| `use_constant_feature` | _bool_ | Enable input channel constant feature. | `false` | +| `z_offset` | _float_ | The offset to translate up the input pointcloud before the inference. Unit: meter | `0.0` | +| `min_height` | _float_ | The minimum height with respect to the origin (the LiDAR sensor). Unit: meter | `-5.0` | +| `max_height` | _float_ | The maximum height with respect to the origin (the LiDAR sensor). Unit: meter | `5.0` | +| `objectness_thresh` | _float_ | The threshold of objectness for filtering out non-object cells in the obstacle clustering step. | `0.5` | +| `min_pts_num` | _int_ | In the post-processing step, the candidate clusters with less than min_pts_num points are removed. | `3` | +| `height_thresh` | _float_ | If it is non-negative, the points that are higher than the predicted object height by height_thresh are filtered out in the post-processing step. Unit: meter | `0.5` | + +### Error detection and handling + +Abort and warn when the input frame can't be converted to `base_link`. + +### Security considerations + +Both the input and output are controlled by the same actor, so the following security concerns are +out-of-scope: + +- Spoofing +- Tampering + +Leaking data to another actor would require a flaw in TVM or the host operating system that allows +arbitrary memory to be read, a significant security flaw in itself. +This is also true for an external actor operating the pipeline early: only the object that initiated +the pipeline can run the methods to receive its output. + +A Denial-of-Service attack could make the target hardware unusable for other pipelines but would +require being able to run code on the CPU, which would already allow a more severe Denial-of-Service +attack. + +No elevation of privilege is required for this package. + +## Future extensions / Unimplemented parts + +## Related issues + +- #226: Autoware.Auto Neural Networks Inference Architecture Design diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp b/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp new file mode 100644 index 0000000000000..eb05ca28c3a39 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp @@ -0,0 +1,54 @@ +// Copyright 2020-2022 Arm Ltd., TierIV +// +// 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 LIDAR_APOLLO_SEGMENTATION_TVM_NODES__LIDAR_APOLLO_SEGMENTATION_TVM_NODE_HPP_ +#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES__LIDAR_APOLLO_SEGMENTATION_TVM_NODE_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm_nodes +{ +/// \brief Object detection node based on neural network inference. +class LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC ApolloLidarSegmentationNode : public rclcpp::Node +{ +private: + const rclcpp::Subscription::SharedPtr m_cloud_sub_ptr; + const rclcpp::Publisher::SharedPtr + m_detected_pub_ptr; + const std::shared_ptr m_detector_ptr; + /// \brief Main callback function. + void LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL + pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr & msg); + +public: + /// \brief Constructor + /// \param options Additional options to control creation of the node. + explicit ApolloLidarSegmentationNode(const rclcpp::NodeOptions & options); +}; +} // namespace lidar_apollo_segmentation_tvm_nodes +} // namespace perception +} // namespace autoware +#endif // LIDAR_APOLLO_SEGMENTATION_TVM_NODES__LIDAR_APOLLO_SEGMENTATION_TVM_NODE_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/visibility_control.hpp b/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/visibility_control.hpp new file mode 100644 index 0000000000000..0521737a96ed1 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2021-2022 Arm Ltd. +// +// 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 LIDAR_APOLLO_SEGMENTATION_TVM_NODES__VISIBILITY_CONTROL_HPP_ +#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES__VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) +#if defined(LIDAR_APOLLO_SEGMENTATION_TVM_NODES_BUILDING_DLL) || \ + defined(LIDAR_APOLLO_SEGMENTATION_TVM_NODES_EXPORTS) +#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __declspec(dllexport) +#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL +#else +#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __declspec(dllimport) +#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL +#endif +#elif defined(__linux__) +#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __attribute__((visibility("default"))) +#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __attribute__((visibility("default"))) +#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL __attribute__((visibility("hidden"))) +#else +#error "Unsupported Build Configuration" +#endif + +#endif // LIDAR_APOLLO_SEGMENTATION_TVM_NODES__VISIBILITY_CONTROL_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py new file mode 100644 index 0000000000000..6abf95dce7789 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py @@ -0,0 +1,51 @@ +# Copyright 2021-2022 the Autoware Foundation +# +# 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. + +"""Launch lidar segmentation node.""" + +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +import yaml + + +def generate_launch_description(): + param_file = os.path.join( + get_package_share_directory("lidar_apollo_segmentation_tvm_nodes"), "param/test.param.yaml" + ) + with open(param_file, "r") as f: + lidar_apollo_segmentation_tvm_node_params = yaml.safe_load(f)["/**"]["ros__parameters"] + + arguments = [ + DeclareLaunchArgument("input/pointcloud", default_value="/sensing/lidar/pointcloud"), + DeclareLaunchArgument("output/objects", default_value="labeled_clusters"), + ] + + # lidar segmentation node execution definition. + lidar_apollo_segmentation_tvm_node_runner = Node( + package="lidar_apollo_segmentation_tvm_nodes", + executable="lidar_apollo_segmentation_tvm_nodes_exe", + remappings=[ + ("points_in", LaunchConfiguration("input/pointcloud")), + ("objects_out", LaunchConfiguration("output/objects")), + ], + parameters=[lidar_apollo_segmentation_tvm_node_params], + output="screen", + ) + + return launch.LaunchDescription(arguments + [lidar_apollo_segmentation_tvm_node_runner]) diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml new file mode 100644 index 0000000000000..624e25c7d4ba6 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/package.xml b/perception/lidar_apollo_segmentation_tvm_nodes/package.xml new file mode 100755 index 0000000000000..4c6047a105d7c --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/package.xml @@ -0,0 +1,28 @@ + + + + lidar_apollo_segmentation_tvm_nodes + 0.1.0 + lidar_apollo_segmentation_tvm_nodes + Ambroise Vincent + Apache 2.0 + + ament_cmake + ament_cmake_auto + + autoware_cmake + + autoware_auto_common + lidar_apollo_segmentation_tvm + rclcpp + rclcpp_components + sensor_msgs + + ament_lint_auto + autoware_lint_common + ros_testing + + + ament_cmake + + diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/param/test.param.yaml b/perception/lidar_apollo_segmentation_tvm_nodes/param/test.param.yaml new file mode 100644 index 0000000000000..cf5e895288717 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/param/test.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + # The range of the 2D grid with respect to the origin. + range: 70 + # The detection confidence score threshold for filtering out the candidate + # clusters in the post-processing step. + score_threshold: 0.8 + # Enable input channel intensity feature. + use_intensity_feature: true + # Enable input channel constant feature. + use_constant_feature: false + # Vertical translation of the pointcloud before inference. + z_offset: 0.0 diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp b/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp new file mode 100644 index 0000000000000..e6b6cd0025409 --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp @@ -0,0 +1,72 @@ +// Copyright 2020-2022 Arm Ltd., TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +using autoware::common::types::bool8_t; +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using autoware::perception::lidar_apollo_segmentation_tvm::ApolloLidarSegmentation; + +namespace autoware +{ +namespace perception +{ +namespace lidar_apollo_segmentation_tvm_nodes +{ + +ApolloLidarSegmentationNode::ApolloLidarSegmentationNode(const rclcpp::NodeOptions & options) +: Node("lidar_apollo_segmentation_tvm", options), + m_cloud_sub_ptr{create_subscription( + "points_in", rclcpp::SensorDataQoS().keep_last(1), + [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { pointCloudCallback(msg); })}, + m_detected_pub_ptr{create_publisher( + "objects_out", rclcpp::QoS{1})}, + m_detector_ptr{std::make_shared( + declare_parameter("range", rclcpp::ParameterValue{70}).get(), + declare_parameter("score_threshold", rclcpp::ParameterValue{0.8}).get(), + declare_parameter("use_intensity_feature", rclcpp::ParameterValue{true}).get(), + declare_parameter("use_constant_feature", rclcpp::ParameterValue{false}).get(), + declare_parameter("z_offset", rclcpp::ParameterValue{0.0}).get(), + declare_parameter("min_height", rclcpp::ParameterValue{-5.0}).get(), + declare_parameter("max_height", rclcpp::ParameterValue{5.0}).get(), + declare_parameter("objectness_thresh", rclcpp::ParameterValue{0.5}).get(), + declare_parameter("min_pts_num", rclcpp::ParameterValue{3}).get(), + declare_parameter("height_thresh", rclcpp::ParameterValue{0.5}).get())} +{ +} + +void ApolloLidarSegmentationNode::pointCloudCallback( + const sensor_msgs::msg::PointCloud2::SharedPtr & msg) +{ + std::shared_ptr output_msg; + try { + output_msg = m_detector_ptr->detectDynamicObjects(*msg); + } catch (const std::exception & e) { + RCLCPP_WARN(get_logger(), e.what()); + return; + } + m_detected_pub_ptr->publish(*output_msg); +} +} // namespace lidar_apollo_segmentation_tvm_nodes +} // namespace perception +} // namespace autoware + +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::perception::lidar_apollo_segmentation_tvm_nodes::ApolloLidarSegmentationNode) diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py b/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py new file mode 100644 index 0000000000000..f657270865c7e --- /dev/null +++ b/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py @@ -0,0 +1,61 @@ +# Copyright 2021-2022 Arm Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch_testing +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + + lidar_apollo_segmentation_tvm = Node( + package="lidar_apollo_segmentation_tvm_nodes", + executable="lidar_apollo_segmentation_tvm_nodes_exe", + name="lidar_apollo_segmentation_tvm_nodes", + namespace="benchmark", + output="screen", + parameters=[ + os.path.join( + get_package_share_directory("lidar_apollo_segmentation_tvm_nodes"), + "param/test.param.yaml", + ) + ], + ) + + context = {"lidar_apollo_segmentation_tvm": lidar_apollo_segmentation_tvm} + + launch_description = LaunchDescription( + [ + lidar_apollo_segmentation_tvm, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + return launch_description, context + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_output, proc_info, lidar_apollo_segmentation_tvm): + # Check that process exits with code -2 or -15 + launch_testing.asserts.assertExitCodes( + proc_info, [-2, -15], process=lidar_apollo_segmentation_tvm + ) From 794840db40fef02b9950f279aa767aa9d061be58 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Fri, 30 Sep 2022 10:33:10 +0900 Subject: [PATCH 34/38] refactor(imu_corrector): refactor covariance index (#1878) Signed-off-by: scepter914 Signed-off-by: scepter914 --- sensing/imu_corrector/package.xml | 1 + sensing/imu_corrector/src/imu_corrector_core.cpp | 9 ++++++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index 420f9b66ee94f..25ab1f75340ce 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -15,6 +15,7 @@ rclcpp rclcpp_components sensor_msgs + tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index 79a44fc2ca12a..d19b0da7d9387 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -14,6 +14,8 @@ #include "imu_corrector/imu_corrector_core.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + namespace imu_corrector { ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & node_options) @@ -42,11 +44,12 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m imu_msg.angular_velocity.y -= angular_velocity_offset_y_; imu_msg.angular_velocity.z -= angular_velocity_offset_z_; - imu_msg.angular_velocity_covariance[0 * 3 + 0] = + using IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + imu_msg.angular_velocity_covariance[IDX::X_X] = angular_velocity_stddev_xx_ * angular_velocity_stddev_xx_; - imu_msg.angular_velocity_covariance[1 * 3 + 1] = + imu_msg.angular_velocity_covariance[IDX::Y_Y] = angular_velocity_stddev_yy_ * angular_velocity_stddev_yy_; - imu_msg.angular_velocity_covariance[2 * 3 + 2] = + imu_msg.angular_velocity_covariance[IDX::Z_Z] = angular_velocity_stddev_zz_ * angular_velocity_stddev_zz_; imu_pub_->publish(imu_msg); From 6a6532b625ac69c6e6c102ff9fb16cba0a56b0a5 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Fri, 30 Sep 2022 11:35:23 +0900 Subject: [PATCH 35/38] feat(tier4_perception_launch): add enable_fine_detection_option (#1991) * feat(tier4_perception_launch): add enable_fine_detection_option Signed-off-by: Shumpei Wakabayashi * chore: rename Signed-off-by: Shumpei Wakabayashi * ci(pre-commit): autofix Signed-off-by: Shumpei Wakabayashi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- launch/tier4_perception_launch/launch/perception.launch.xml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index ae6df48a412f2..c568ea60d9f06 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -35,6 +35,8 @@ /> + + @@ -129,7 +131,9 @@ - + + + From 144569628b51bc1704e912b586fa85fa50278fad Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Fri, 30 Sep 2022 17:38:15 +0900 Subject: [PATCH 36/38] fix(lidar_apollo_segmentation_tvm): always run ament_auto_package() in CMakeLists.txt (#1995) Signed-off-by: mitsudome-r Signed-off-by: mitsudome-r --- perception/lidar_apollo_segmentation_tvm/CMakeLists.txt | 1 + perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt b/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt index ac3c4bd870769..f665a22a6005b 100644 --- a/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt +++ b/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt @@ -72,4 +72,5 @@ if(baidu_cnn_FOUND) ament_auto_package() else() message(WARNING "Neural network not found, skipping package.") + ament_auto_package() endif() diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt b/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt index 8a97a1ef05d5a..202cac6ad328a 100644 --- a/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt +++ b/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt @@ -19,7 +19,8 @@ find_package(autoware_cmake REQUIRED) autoware_package() # Check that a neural network was available for lidar_apollo_segmentation_tvm to be built. -if(lidar_apollo_segmentation_tvm_FOUND) +autoware_check_neural_network(baidu_cnn "${${lidar_apollo_segmentation_tvm}_BACKEND}") +if(baidu_cnn_FOUND) # Set lidar_apollo_segmentation_tvm includes as "SYSTEM" to ignore compiler errors on PCL headers include_directories(SYSTEM "${lidar_apollo_segmentation_tvm_INCLUDE_DIRS}") @@ -48,4 +49,5 @@ if(lidar_apollo_segmentation_tvm_FOUND) ) else() message(WARNING "lidar_apollo_segmentation_tvm not found, skipping package.") + ament_auto_package() endif() From 8dae2a24bb8f1a1ef6b38177e71462287b0ff20e Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Sat, 1 Oct 2022 15:17:16 +0900 Subject: [PATCH 37/38] refactor(simple_planning_simulator): refactor covariance index (#1972) Signed-off-by: scepter914 Signed-off-by: scepter914 --- .../simple_planning_simulator_core.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index ad013e1846f8a..95c387ce6bcf5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -19,6 +19,7 @@ #include "motion_common/motion_common.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -286,8 +287,9 @@ void SimplePlanningSimulator::on_timer() // add estimate covariance { - current_odometry_.pose.covariance[0 * 6 + 0] = x_stddev_; - current_odometry_.pose.covariance[1 * 6 + 1] = y_stddev_; + using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + current_odometry_.pose.covariance[COV_IDX::X_X] = x_stddev_; + current_odometry_.pose.covariance[COV_IDX::Y_Y] = y_stddev_; } // publish vehicle state @@ -527,13 +529,14 @@ void SimplePlanningSimulator::publish_acceleration() msg.header.stamp = get_clock()->now(); msg.accel.accel.linear.x = vehicle_model_ptr_->getAx(); + using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; constexpr auto COV = 0.001; - msg.accel.covariance.at(6 * 0 + 0) = COV; // linear x - msg.accel.covariance.at(6 * 1 + 1) = COV; // linear y - msg.accel.covariance.at(6 * 2 + 2) = COV; // linear z - msg.accel.covariance.at(6 * 3 + 3) = COV; // angular x - msg.accel.covariance.at(6 * 4 + 4) = COV; // angular y - msg.accel.covariance.at(6 * 5 + 5) = COV; // angular z + msg.accel.covariance.at(COV_IDX::X_X) = COV; // linear x + msg.accel.covariance.at(COV_IDX::Y_Y) = COV; // linear y + msg.accel.covariance.at(COV_IDX::Z_Z) = COV; // linear z + msg.accel.covariance.at(COV_IDX::ROLL_ROLL) = COV; // angular x + msg.accel.covariance.at(COV_IDX::PITCH_PITCH) = COV; // angular y + msg.accel.covariance.at(COV_IDX::YAW_YAW) = COV; // angular z pub_acc_->publish(msg); } From e89afb9387cdb3a35667c44a59b59ad23e565990 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Sun, 2 Oct 2022 23:00:58 +0900 Subject: [PATCH 38/38] chore: sync files (#2001) Signed-off-by: GitHub Signed-off-by: GitHub Co-authored-by: kenji-miyake --- .github/workflows/cancel-previous-workflows.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml index aeb34c6a03dcf..b28a4ec0bbcfc 100644 --- a/.github/workflows/cancel-previous-workflows.yaml +++ b/.github/workflows/cancel-previous-workflows.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Cancel previous runs - uses: styfle/cancel-workflow-action@0.10.0 + uses: styfle/cancel-workflow-action@0.10.1 with: workflow_id: all all_but_latest: true