diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index f7d0d06f9310b..2a83c9b508ec4 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -28,6 +28,7 @@ common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @ common/signal_processing/** ali.boyali@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners common/tensorrt_common/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners +common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_autoware_utils/** kenji.miyake@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -36,7 +37,7 @@ common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp @autowarefoundation/ common/tier4_debug_tools/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -45,6 +46,7 @@ common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp @autowarefoundation/ common/time_utils/** christopherj.ho@gmail.com @autowarefoundation/autoware-global-codeowners common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp @autowarefoundation/autoware-global-codeowners +control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@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 @autowarefoundation/autoware-global-codeowners control/external_cmd_selector/** fumiya.watanabe@tier4.jp kenji.miyake@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 @autowarefoundation/autoware-global-codeowners control/joy_controller/** fumiya.watanabe@tier4.jp kenji.miyake@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 @autowarefoundation/autoware-global-codeowners @@ -58,16 +60,18 @@ control/shift_decider/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-g control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners +evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners evaluator/localization_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners +evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_localization_launch/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners +launch/tier4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_map_launch/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners 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 @autowarefoundation/autoware-global-codeowners launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_simulator_launch/** keisuke.shima@tier4.jp @autowarefoundation/autoware-global-codeowners +launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_system_launch/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners localization/ekf_localizer/** koji.minoda@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -83,6 +87,7 @@ map/map_height_fitter/** isamu.takagi@tier4.jp yamato.ando@tier4.jp @autowarefou map/map_loader/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp @autowarefoundation/autoware-global-codeowners map/map_tf_generator/** azumi.suzuki@tier4.jp @autowarefoundation/autoware-global-codeowners map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/bytetrack/** manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners perception/compare_map_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -112,9 +117,10 @@ perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp @autowarefoundation/auto perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners perception/traffic_light_classifier/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/traffic_light_selector/** isamu.takagi@tier4.jp @autowarefoundation/autoware-global-codeowners perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners perception/traffic_light_visualization/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -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 @autowarefoundation/autoware-global-codeowners +planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@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 @autowarefoundation/autoware-global-codeowners 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 takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -122,12 +128,12 @@ planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/mission_planner/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/obstacle_avoidance_planner/** takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/obstacle_stop_planner/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/planning_evaluator/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/planning_test_utils/** kyoichi.sugahara@tier4.jp @autowarefoundation/autoware-global-codeowners planning/planning_validator/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-global-codeowners planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index c114dac59e325..3c0f0862c92ab 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -77,7 +77,7 @@ jobs: - name: Get modified files id: get-modified-files - uses: tj-actions/changed-files@v34 + uses: tj-actions/changed-files@v35 with: files: | **/*.cpp diff --git a/.github/workflows/clang-tidy-pr-comments-manually.yaml b/.github/workflows/clang-tidy-pr-comments-manually.yaml index f593f034cdf6a..9bccd972becde 100644 --- a/.github/workflows/clang-tidy-pr-comments-manually.yaml +++ b/.github/workflows/clang-tidy-pr-comments-manually.yaml @@ -30,9 +30,9 @@ jobs: if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} id: set-variables run: | - echo ::set-output name=pr-id::"$(cat /tmp/clang-tidy-result/pr-id.txt)" - echo ::set-output name=pr-head-repo::"$(cat /tmp/clang-tidy-result/pr-head-repo.txt)" - echo ::set-output name=pr-head-ref::"$(cat /tmp/clang-tidy-result/pr-head-ref.txt)" + echo "pr-id=$(cat /tmp/clang-tidy-result/pr-id.txt)" >> $GITHUB_OUTPUT + echo "pr-head-repo=$(cat /tmp/clang-tidy-result/pr-head-repo.txt)" >> $GITHUB_OUTPUT + echo "pr-head-ref=$(cat /tmp/clang-tidy-result/pr-head-ref.txt)" >> $GITHUB_OUTPUT - name: Check out PR head if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml index 75849bd960170..baaa0fb8e7744 100644 --- a/.github/workflows/clang-tidy-pr-comments.yaml +++ b/.github/workflows/clang-tidy-pr-comments.yaml @@ -31,9 +31,9 @@ jobs: if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} id: set-variables run: | - echo ::set-output name=pr-id::"$(cat /tmp/clang-tidy-result/pr-id.txt)" - echo ::set-output name=pr-head-repo::"$(cat /tmp/clang-tidy-result/pr-head-repo.txt)" - echo ::set-output name=pr-head-ref::"$(cat /tmp/clang-tidy-result/pr-head-ref.txt)" + echo "pr-id=$(cat /tmp/clang-tidy-result/pr-id.txt)" >> $GITHUB_OUTPUT + echo "pr-head-repo=$(cat /tmp/clang-tidy-result/pr-head-repo.txt)" >> $GITHUB_OUTPUT + echo "pr-head-ref=$(cat /tmp/clang-tidy-result/pr-head-ref.txt)" >> $GITHUB_OUTPUT - name: Check out PR head if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml index 19e1e9c12e57c..33677fc1b762c 100644 --- a/.github/workflows/github-release.yaml +++ b/.github/workflows/github-release.yaml @@ -26,8 +26,8 @@ jobs: REF_NAME="${{ github.ref_name }}" fi - echo ::set-output name=ref-name::"$REF_NAME" - echo ::set-output name=tag-name::"${REF_NAME#beta/}" + echo "ref-name=$REF_NAME" >> $GITHUB_OUTPUT + echo "tag-name=${REF_NAME#beta/}" >> $GITHUB_OUTPUT" - name: Check out repository uses: actions/checkout@v3 @@ -39,7 +39,7 @@ jobs: id: set-target-name run: | if [[ "${{ steps.set-tag-name.outputs.ref-name }}" =~ "beta/" ]]; then - echo ::set-output name=target-name::"${{ steps.set-tag-name.outputs.ref-name }}" + echo "target-name=${{ steps.set-tag-name.outputs.ref-name }}" >> $GITHUB_OUTPUT fi - name: Create a local tag for beta branches @@ -62,7 +62,7 @@ jobs: verb=edit fi - echo ::set-output name=verb::"$verb" + echo "verb=$verb" >> $GITHUB_OUTPUT env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml index e0019e10d5210..caa92d0812ab0 100644 --- a/.pre-commit-config-optional.yaml +++ b/.pre-commit-config-optional.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/tcort/markdown-link-check - rev: v3.10.3 + rev: v3.11.0 hooks: - id: markdown-link-check args: [--config=.markdown-link-check.json] diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5e57d3b6c0998..da14e953ba1c1 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -24,12 +24,12 @@ repos: args: [-c, .markdownlint.yaml, --fix] - repo: https://github.com/pre-commit/mirrors-prettier - rev: v3.0.0-alpha.4 + rev: v3.0.0-alpha.6 hooks: - id: prettier - repo: https://github.com/adrienverge/yamllint - rev: v1.29.0 + rev: v1.30.0 hooks: - id: yamllint @@ -49,7 +49,7 @@ repos: - id: shellcheck - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.6.0-1 + rev: v3.6.0-2 hooks: - id: shfmt args: [-w, -s, -i=4] @@ -66,7 +66,7 @@ repos: args: [--line-length=100] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v15.0.7 + rev: v16.0.0 hooks: - id: clang-format types_or: [c++, c, cuda] diff --git a/common/autoware_testing/package.xml b/common/autoware_testing/package.xml index d895d85122ae8..30a27bb9435b1 100644 --- a/common/autoware_testing/package.xml +++ b/common/autoware_testing/package.xml @@ -12,6 +12,8 @@ autoware_cmake + ros_testing + ament_cmake_core ament_copyright ament_flake8 diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 821507ca9a6a8..dbdfc251164b4 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -273,7 +273,7 @@ boost::optional findNearestIndex( for (size_t i = 0; i < points.size(); ++i) { const auto squared_dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose); - if (squared_dist > max_squared_dist) { + if (squared_dist > max_squared_dist || squared_dist >= min_squared_dist) { continue; } @@ -283,10 +283,6 @@ boost::optional findNearestIndex( continue; } - if (squared_dist >= min_squared_dist) { - continue; - } - min_squared_dist = squared_dist; min_idx = i; is_nearest_found = true; @@ -1536,6 +1532,82 @@ boost::optional calcDistanceToForwardStopPoint( return std::max(0.0, closest_stop_dist); } + +// NOTE: Points after forward length from the point will be cropped +// forward_length is assumed to be positive. +template +T cropForwardPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length) +{ + if (points.empty()) { + return T{}; + } + + double sum_length = + -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (forward_length < sum_length) { + const size_t end_idx = i; + return T{points.begin(), points.begin() + end_idx}; + } + } + + return points; +} + +// NOTE: Points before backward length from the point will be cropped +// backward_length is assumed to be positive. +template +T cropBackwardPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length) +{ + if (points.empty()) { + return T{}; + } + + double sum_length = + -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + for (int i = target_seg_idx; 0 < i; --i) { + sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (sum_length < -backward_length) { + const size_t begin_idx = i; + return T{points.begin() + begin_idx, points.end()}; + } + } + + return points; +} + +template +T cropPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length) +{ + if (points.empty()) { + return T{}; + } + + // NOTE: Cropping forward must be done first in order to keep target_seg_idx. + const auto cropped_forward_points = + cropForwardPoints(points, target_pos, target_seg_idx, forward_length); + + const size_t modified_target_seg_idx = + std::min(target_seg_idx, cropped_forward_points.size() - 2); + const auto cropped_points = cropBackwardPoints( + cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); + + if (cropped_points.size() < 2) { + RCLCPP_ERROR( + rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), + ". Return original points since cropped_points size is less than 2."); + return points; + } + + return cropped_points; +} } // namespace motion_utils #endif // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 26d7e2081203b..66ce3ea3e697c 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -4543,3 +4543,96 @@ TEST(trajectory, removeOverlapPoints) EXPECT_TRUE(removed_traj.empty()); } } + +TEST(trajectory, cropForwardPoints) +{ + using motion_utils::cropForwardPoints; + + const auto traj = generateTestTrajectory(10, 1.0, 1.0); + + { // Normal case + const auto cropped_traj_points = + cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.5, 1.5, 0.0), 1, 4.8); + EXPECT_EQ(cropped_traj_points.size(), static_cast(7)); + } + + { // Forward length is longer than points arc length. + const auto cropped_traj_points = + cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.5, 1.5, 0.0), 1, 10.0); + EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); + } + + { // Point is on the previous segment + const auto cropped_traj_points = + cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.0, 1.0, 0.0), 0, 2.5); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } + + { // Point is on the next segment + const auto cropped_traj_points = + cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.0, 1.0, 0.0), 1, 2.5); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } +} + +TEST(trajectory, cropBackwardPoints) +{ + using motion_utils::cropBackwardPoints; + + const auto traj = generateTestTrajectory(10, 1.0, 1.0); + + { // Normal case + const auto cropped_traj_points = + cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.5, 8.5, 0.0), 8, 4.8); + EXPECT_EQ(cropped_traj_points.size(), static_cast(6)); + } + + { // Backward length is longer than points arc length. + const auto cropped_traj_points = + cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.5, 8.5, 0.0), 8, 10.0); + EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); + } + + { // Point is on the previous segment + const auto cropped_traj_points = + cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.0, 8.0, 0.0), 7, 2.5); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } + + { // Point is on the next segment + const auto cropped_traj_points = + cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.0, 8.0, 0.0), 8, 2.5); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } +} + +TEST(trajectory, cropPoints) +{ + using motion_utils::cropPoints; + + const auto traj = generateTestTrajectory(10, 1.0, 1.0); + + { // Normal case + const auto cropped_traj_points = + cropPoints(traj.points, tier4_autoware_utils::createPoint(3.5, 3.5, 0.0), 3, 2.3, 1.2); + EXPECT_EQ(cropped_traj_points.size(), static_cast(3)); + } + + { // Length is longer than points arc length. + const auto cropped_traj_points = + cropPoints(traj.points, tier4_autoware_utils::createPoint(3.5, 3.5, 0.0), 3, 10.0, 10.0); + EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); + } + + { // Point is on the previous segment + const auto cropped_traj_points = + cropPoints(traj.points, tier4_autoware_utils::createPoint(3.0, 3.0, 0.0), 2, 2.2, 1.9); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } + + { // Point is on the next segment + const auto cropped_traj_points = + cropPoints(traj.points, tier4_autoware_utils::createPoint(3.0, 3.0, 0.0), 3, 2.2, 1.9); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } +} diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index 2daa837ffd073..a08ba8874feb7 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -45,6 +45,12 @@ std::string getModuleName(const uint8_t module_type) case Module::EXT_REQUEST_LANE_CHANGE_RIGHT: { return "ext_request_lane_change_right"; } + case Module::AVOIDANCE_BY_LC_LEFT: { + return "avoidance_by_lane_change_left"; + } + case Module::AVOIDANCE_BY_LC_RIGHT: { + return "avoidance_by_lane_change_right"; + } case Module::AVOIDANCE_LEFT: { return "avoidance_left"; } @@ -87,9 +93,10 @@ bool isPathChangeModule(const uint8_t module_type) if ( module_type == Module::LANE_CHANGE_LEFT || module_type == Module::LANE_CHANGE_RIGHT || module_type == Module::EXT_REQUEST_LANE_CHANGE_LEFT || - module_type == Module::EXT_REQUEST_LANE_CHANGE_RIGHT || module_type == Module::AVOIDANCE_LEFT || - module_type == Module::AVOIDANCE_RIGHT || module_type == Module::PULL_OVER || - module_type == Module::PULL_OUT) { + module_type == Module::EXT_REQUEST_LANE_CHANGE_RIGHT || + module_type == Module::AVOIDANCE_BY_LC_LEFT || module_type == Module::AVOIDANCE_BY_LC_RIGHT || + module_type == Module::AVOIDANCE_LEFT || module_type == Module::AVOIDANCE_RIGHT || + module_type == Module::PULL_OVER || module_type == Module::PULL_OUT) { return true; } return false; @@ -98,7 +105,7 @@ bool isPathChangeModule(const uint8_t module_type) RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent) { // TODO(tanaka): replace this magic number to Module::SIZE - const size_t module_size = 14; + const size_t module_size = 18; auto_modes_.reserve(module_size); auto * v_layout = new QVBoxLayout; auto vertical_header = new QHeaderView(Qt::Vertical); diff --git a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..c6d4e30061626 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_automatic_goal_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/automatic_goal_sender.cpp + src/automatic_goal_append_tool.cpp + src/automatic_goal_panel.cpp +) + +ament_auto_add_executable(automatic_goal_sender + src/automatic_goal_sender.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 + launch + icons + plugins +) diff --git a/common/tier4_automatic_goal_rviz_plugin/README.md b/common/tier4_automatic_goal_rviz_plugin/README.md new file mode 100644 index 0000000000000..48fb577375714 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/README.md @@ -0,0 +1,80 @@ +# tier4_automatic_goal_rviz_plugin + +## Purpose + +1. Defining a `GoalsList` by adding goals using `RvizTool` (Pose on the map). + +2. Automatic execution of the created `GoalsList` from the selected goal - it can be stopped and restarted. + +3. Looping the current `GoalsList`. + +4. Saving achieved goals to a file. + +5. Plan the route to one (single) selected goal and starting that route - it can be stopped and restarted. + +6. Remove any goal from the list or clear the current route. + +7. Save the current `GoalsList` to a file and load the list from the file. + +8. The application enables/disables access to options depending on the current state. + +9. The saved `GoalsList` can be executed without using a plugin - using a node `automatic_goal_sender`. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------------------- | ------------------------------------------------- | ------------------------------------------------ | +| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | The topic represents the state of operation mode | +| `/api/routing/state` | `autoware_adapi_v1_msgs::msg::RouteState` | The topic represents the state of route | +| `/rviz2/automatic_goal/goal` | `geometry_msgs::msgs::PoseStamped` | The topic for adding goals to GoalsList | + +### Output + +| Name | Type | Description | +| ------------------------------------------ | -------------------------------------------------- | -------------------------------------------------- | +| `/api/operation_mode/change_to_autonomous` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to autonomous | +| `/api/operation_mode/change_to_stop` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to stop | +| `/api/routing/set_route_points` | `autoware_adapi_v1_msgs::srv::SetRoutePoints` | The service to set route | +| `/api/routing/clear_route` | `autoware_adapi_v1_msgs::srv::ClearRoute` | The service to clear route state | +| `/rviz2/automatic_goal/markers` | `visualization_msgs::msg::MarkerArray` | The topic to visualize goals as rviz markers | + +## HowToUse + +1. Start rviz and select panels/Add new panel. + + ![select_panel](./images/select_panels.png) + +2. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalPanel` and press OK. + +3. Select Add a new tool. + + ![select_tool](./images/select_tool.png) + +4. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalTool` and press OK. + +5. Add goals visualization as markers to `Displays`. + + ![markers](./images/markers.png) + +6. Append goals to the `GoalsList` to be achieved using `2D Append Goal` - in such a way that routes can be planned. + +7. Start sequential planning and goal achievement by clicking `Send goals automatically` + + ![panel](./images/panel.png) + +8. You can save `GoalsList` by clicking `Save to file`. + +9. After saving, you can run the `GoalsList` without using a plugin also: + - example: `ros2 launch tier4_automatic_goal_rviz_plugin automatic_goal_sender.launch.xml goals_list_file_path:="/tmp/goals_list.yaml" goals_achieved_dir_path:="/tmp/"` + - `goals_list_file_path` - is the path to the saved `GoalsList` file to be loaded + - `goals_achieved_dir_path` - is the path to the directory where the file `goals_achieved.log` will be created and the achieved goals will be written to it + +### Hints + +If the application (Engagement) goes into `ERROR` mode (usually returns to `EDITING` later), it means that one of the services returned a calling error (`code!=0`). +In this situation, check the terminal output for more information. + +- Often it is enough to try again. +- Sometimes a clearing of the current route is required before retrying. diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/markers.png b/common/tier4_automatic_goal_rviz_plugin/images/markers.png new file mode 100644 index 0000000000000..8dd4d9d02bef4 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/markers.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/panel.png b/common/tier4_automatic_goal_rviz_plugin/images/panel.png new file mode 100644 index 0000000000000..1800202ea9f57 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/panel.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png b/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png new file mode 100644 index 0000000000000..61dd9e1d7a1b3 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png b/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png new file mode 100644 index 0000000000000..a6ddc6d24c575 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml b/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml new file mode 100644 index 0000000000000..a9af89c078348 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/common/tier4_automatic_goal_rviz_plugin/package.xml b/common/tier4_automatic_goal_rviz_plugin/package.xml new file mode 100644 index 0000000000000..11d263e09c789 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/package.xml @@ -0,0 +1,35 @@ + + + + tier4_automatic_goal_rviz_plugin + 0.0.0 + The autoware automatic goal rviz plugin package + Shumpei Wakabayashi + Dawid Moszyński + Apache License 2.0 + Dawid Moszyński + + ament_cmake_auto + autoware_cmake + autoware_adapi_v1_msgs + geometry_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + rviz_default_plugins + tf2 + tf2_geometry_msgs + visualization_msgs + yaml-cpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..e7d5224550868 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,12 @@ + + + AutowareAutomaticGoalPanel + + + AutowareAutomaticGoalTool + + diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp new file mode 100644 index 0000000000000..43fc0edcccf84 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "automatic_goal_append_tool.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +#include + +namespace rviz_plugins +{ +AutowareAutomaticGoalTool::AutowareAutomaticGoalTool() +{ + shortcut_key_ = 'c'; + + pose_topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "~/automatic_goal/goal", "The topic on which to publish automatic_goal.", + getPropertyContainer(), SLOT(updateTopic()), this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void AutowareAutomaticGoalTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D AppendGoal"); + updateTopic(); +} + +void AutowareAutomaticGoalTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pose_pub_ = raw_node->create_publisher( + pose_topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void AutowareAutomaticGoalTool::onPoseSet(double x, double y, double theta) +{ + // pose + std::string fixed_frame = context_->getFixedFrame().toStdString(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = fixed_frame; + pose.header.stamp = clock_->now(); + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = position_z_->getFloat(); + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + pose.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("AutowareAutomaticGoalTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", + x, y, position_z_->getFloat(), theta, fixed_frame.c_str()); + pose_pub_->publish(pose); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticGoalTool, rviz_common::Tool) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp new file mode 100644 index 0000000000000..6fc98cee9afa1 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp @@ -0,0 +1,91 @@ +// Copyright 2023 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef AUTOMATIC_GOAL_APPEND_TOOL_HPP_ +#define AUTOMATIC_GOAL_APPEND_TOOL_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class AutowareAutomaticGoalTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + AutowareAutomaticGoalTool(); + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double theta) override; + +private Q_SLOTS: + void updateTopic(); + +private: // NOLINT for Qt + rclcpp::Clock::SharedPtr clock_{nullptr}; + rclcpp::Publisher::SharedPtr pose_pub_{nullptr}; + + rviz_common::properties::StringProperty * pose_topic_property_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_x_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_y_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_theta_{nullptr}; + rviz_common::properties::FloatProperty * position_z_{nullptr}; +}; + +} // namespace rviz_plugins + +#endif // AUTOMATIC_GOAL_APPEND_TOOL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp new file mode 100644 index 0000000000000..e04228a8f7b88 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp @@ -0,0 +1,484 @@ +// +// Copyright 2023 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 "automatic_goal_panel.hpp" + +namespace rviz_plugins +{ +AutowareAutomaticGoalPanel::AutowareAutomaticGoalPanel(QWidget * parent) +: rviz_common::Panel(parent) +{ + qt_timer_ = new QTimer(this); + connect( + qt_timer_, &QTimer::timeout, this, &AutowareAutomaticGoalPanel::updateAutoExecutionTimerTick); + + auto * h_layout = new QHBoxLayout(this); + auto * v_layout = new QVBoxLayout(this); + h_layout->addWidget(makeGoalsListGroup()); + v_layout->addWidget(makeEngagementGroup()); + v_layout->addWidget(makeRoutingGroup()); + h_layout->addLayout(v_layout); + setLayout(h_layout); +} + +// Layouts +QGroupBox * AutowareAutomaticGoalPanel::makeGoalsListGroup() +{ + auto * group = new QGroupBox("GoalsList", this); + auto * grid = new QGridLayout(group); + + load_file_btn_ptr_ = new QPushButton("Load from file", group); + connect(load_file_btn_ptr_, SIGNAL(clicked()), SLOT(onClickLoadListFromFile())); + grid->addWidget(load_file_btn_ptr_, 0, 0); + + save_file_btn_ptr_ = new QPushButton("Save to file", group); + connect(save_file_btn_ptr_, SIGNAL(clicked()), SLOT(onClickSaveListToFile())); + grid->addWidget(save_file_btn_ptr_, 1, 0); + + goals_list_widget_ptr_ = new QListWidget(group); + goals_list_widget_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(goals_list_widget_ptr_, 2, 0); + + remove_selected_goal_btn_ptr_ = new QPushButton("Remove selected", group); + connect(remove_selected_goal_btn_ptr_, SIGNAL(clicked()), SLOT(onClickRemove())); + grid->addWidget(remove_selected_goal_btn_ptr_, 3, 0); + + loop_list_btn_ptr_ = new QPushButton("Loop list", group); + loop_list_btn_ptr_->setCheckable(true); + connect(loop_list_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleLoopList(bool))); + grid->addWidget(loop_list_btn_ptr_, 4, 0); + + goals_achieved_btn_ptr_ = new QPushButton("Saving achieved goals to file", group); + goals_achieved_btn_ptr_->setCheckable(true); + connect(goals_achieved_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleSaveGoalsAchievement(bool))); + grid->addWidget(goals_achieved_btn_ptr_, 5, 0); + + group->setLayout(grid); + return group; +} + +QGroupBox * AutowareAutomaticGoalPanel::makeRoutingGroup() +{ + auto * group = new QGroupBox("Routing", this); + auto * grid = new QGridLayout(group); + + routing_label_ptr_ = new QLabel("INIT", group); + routing_label_ptr_->setMinimumSize(100, 25); + routing_label_ptr_->setAlignment(Qt::AlignCenter); + routing_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(routing_label_ptr_, 0, 0); + + clear_route_btn_ptr_ = new QPushButton("Clear planned route", group); + connect(clear_route_btn_ptr_, &QPushButton::clicked, [this]() { onClickClearRoute(); }); + grid->addWidget(clear_route_btn_ptr_, 1, 0); + group->setLayout(grid); + + group->setLayout(grid); + return group; +} + +QGroupBox * AutowareAutomaticGoalPanel::makeEngagementGroup() +{ + auto * group = new QGroupBox("Engagement", this); + auto * grid = new QGridLayout(group); + + engagement_label_ptr_ = new QLabel("INITIALIZING", group); + engagement_label_ptr_->setMinimumSize(100, 25); + engagement_label_ptr_->setAlignment(Qt::AlignCenter); + engagement_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(engagement_label_ptr_, 0, 0); + + automatic_mode_btn_ptr_ = new QPushButton("Send goals automatically", group); + automatic_mode_btn_ptr_->setCheckable(true); + + connect(automatic_mode_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleAutoMode(bool))); + grid->addWidget(automatic_mode_btn_ptr_, 1, 0); + + plan_btn_ptr_ = new QPushButton("Plan to selected goal", group); + connect(plan_btn_ptr_, &QPushButton::clicked, [this] { onClickPlan(); }); + grid->addWidget(plan_btn_ptr_, 2, 0); + + start_btn_ptr_ = new QPushButton("Start current plan", group); + connect(start_btn_ptr_, &QPushButton::clicked, [this] { onClickStart(); }); + grid->addWidget(start_btn_ptr_, 3, 0); + + stop_btn_ptr_ = new QPushButton("Stop current plan", group); + connect(stop_btn_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); + grid->addWidget(stop_btn_ptr_, 4, 0); + group->setLayout(grid); + + group->setLayout(grid); + return group; +} + +void AutowareAutomaticGoalPanel::showMessageBox(const QString & string) +{ + QMessageBox msg_box(this); + msg_box.setText(string); + msg_box.exec(); +} + +// Slots +void AutowareAutomaticGoalPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + pub_marker_ = raw_node_->create_publisher("~/automatic_goal/markers", 0); + sub_append_goal_ = raw_node_->create_subscription( + "~/automatic_goal/goal", 5, + std::bind(&AutowareAutomaticGoalPanel::onAppendGoal, this, std::placeholders::_1)); + initCommunication(raw_node_.get()); +} + +void AutowareAutomaticGoalPanel::onToggleLoopList(bool checked) +{ + is_loop_list_on_ = checked; + updateGUI(); +} + +void AutowareAutomaticGoalPanel::onToggleSaveGoalsAchievement(bool checked) +{ + if (checked) { + QString file_name = QFileDialog::getSaveFileName( + this, tr("Save File with GoalsList"), "/tmp/goals_achieved.log", + tr("Achieved goals (*.log)")); + goals_achieved_file_path_ = file_name.toStdString(); + } else { + goals_achieved_file_path_ = ""; + } + updateGUI(); +} + +void AutowareAutomaticGoalPanel::onToggleAutoMode(bool checked) +{ + if (checked && goals_list_widget_ptr_->selectedItems().count() != 1) { + showMessageBox("Select the first goal in GoalsList"); + automatic_mode_btn_ptr_->setChecked(false); + } else { + if (checked) current_goal_ = goals_list_widget_ptr_->currentRow(); + is_automatic_mode_on_ = checked; + is_automatic_mode_on_ ? qt_timer_->start(1000) : qt_timer_->stop(); + onClickClearRoute(); // here will be set State::AUTO_NEXT or State::EDITING; + } +} + +void AutowareAutomaticGoalPanel::onClickPlan() +{ + if (goals_list_widget_ptr_->selectedItems().count() != 1) { + showMessageBox("Select a goal in GoalsList"); + return; + } + + if (callPlanToGoalIndex(cli_set_route_, goals_list_widget_ptr_->currentRow())) { + state_ = State::PLANNING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickStart() +{ + if (callService(cli_change_to_autonomous_)) { + state_ = State::STARTING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickStop() +{ + // if ERROR is set then the ego is already stopped + if (state_ == State::ERROR) { + state_ = State::STOPPED; + updateGUI(); + } else if (callService(cli_change_to_stop_)) { + state_ = State::STOPPING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickClearRoute() +{ + if (callService(cli_clear_route_)) { + state_ = State::CLEARING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickRemove() +{ + if (static_cast(goals_list_widget_ptr_->currentRow()) < goals_list_.size()) + goals_list_.erase(goals_list_.begin() + goals_list_widget_ptr_->currentRow()); + resetAchievedGoals(); + updateGUI(); + updateGoalsList(); +} + +void AutowareAutomaticGoalPanel::onClickLoadListFromFile() +{ + QString file_name = QFileDialog::getOpenFileName( + this, tr("Open File with GoalsList"), "/tmp", tr("Goal lists (*.yaml)")); + if (file_name.count() > 0) loadGoalsList(file_name.toStdString()); +} + +void AutowareAutomaticGoalPanel::onClickSaveListToFile() +{ + if (!goals_list_.empty()) { + QString file_name = QFileDialog::getSaveFileName( + this, tr("Save File with GoalsList"), "/tmp/goals_list.yaml", tr("Goal lists (*.yaml)")); + if (file_name.count() > 0) saveGoalsList(file_name.toStdString()); + } +} + +// Inputs +void AutowareAutomaticGoalPanel::onAppendGoal(const PoseStamped::ConstSharedPtr pose) +{ + if (state_ == State::EDITING) { + goals_list_.push_back(pose); + updateGoalsList(); + updateGUI(); + } +} + +// Override +void AutowareAutomaticGoalPanel::onCallResult() { updateGUI(); } +void AutowareAutomaticGoalPanel::onGoalListUpdated() +{ + goals_list_widget_ptr_->clear(); + for (auto const & goal : goals_achieved_) { + auto * item = + new QListWidgetItem(QString::fromStdString(goal.second.first), goals_list_widget_ptr_); + goals_list_widget_ptr_->addItem(item); + updateGoalIcon(goals_list_widget_ptr_->count() - 1, QColor("lightGray")); + } + publishMarkers(); +} +void AutowareAutomaticGoalPanel::onOperationModeUpdated(const OperationModeState::ConstSharedPtr) +{ + updateGUI(); +} +void AutowareAutomaticGoalPanel::onRouteUpdated(const RouteState::ConstSharedPtr msg) +{ + std::pair style; + if (msg->state == RouteState::UNSET) + style = std::make_pair("UNSET", "background-color: #FFFF00;"); // yellow + else if (msg->state == RouteState::SET) + style = std::make_pair("SET", "background-color: #00FF00;"); // green + else if (msg->state == RouteState::ARRIVED) + style = std::make_pair("ARRIVED", "background-color: #FFA500;"); // orange + else if (msg->state == RouteState::CHANGING) + style = std::make_pair("CHANGING", "background-color: #FFFF00;"); // yellow + else + style = std::make_pair("UNKNOWN", "background-color: #FF0000;"); // red + + updateLabel(routing_label_ptr_, style.first, style.second); + updateGUI(); +} + +void AutowareAutomaticGoalPanel::updateAutoExecutionTimerTick() +{ + if (is_automatic_mode_on_) { + if (state_ == State::AUTO_NEXT) { + // end if loop is off + if (current_goal_ >= goals_list_.size() && !is_loop_list_on_) { + disableAutomaticMode(); + return; + } + // plan to next goal + current_goal_ = current_goal_ % goals_list_.size(); + if (callPlanToGoalIndex(cli_set_route_, current_goal_)) { + state_ = State::PLANNING; + updateGUI(); + } + } else if (state_ == State::PLANNED) { + updateGoalIcon(current_goal_, QColor("yellow")); + onClickStart(); + } else if (state_ == State::ARRIVED) { + goals_achieved_[current_goal_].second++; + updateAchievedGoalsFile(current_goal_); + updateGoalIcon(current_goal_++, QColor("green")); + onClickClearRoute(); // will be set AUTO_NEXT as next state_ + } else if (state_ == State::STOPPED || state_ == State::ERROR) { + disableAutomaticMode(); + } + } +} + +// Visual updates +void AutowareAutomaticGoalPanel::updateGUI() +{ + deactivateButton(automatic_mode_btn_ptr_); + deactivateButton(remove_selected_goal_btn_ptr_); + deactivateButton(clear_route_btn_ptr_); + deactivateButton(plan_btn_ptr_); + deactivateButton(start_btn_ptr_); + deactivateButton(stop_btn_ptr_); + deactivateButton(load_file_btn_ptr_); + deactivateButton(save_file_btn_ptr_); + deactivateButton(loop_list_btn_ptr_); + deactivateButton(goals_achieved_btn_ptr_); + + std::pair style; + switch (state_) { + case State::EDITING: + activateButton(load_file_btn_ptr_); + if (!goals_list_.empty()) { + activateButton(goals_achieved_btn_ptr_); + activateButton(plan_btn_ptr_); + activateButton(remove_selected_goal_btn_ptr_); + activateButton(automatic_mode_btn_ptr_); + activateButton(save_file_btn_ptr_); + activateButton(loop_list_btn_ptr_); + } + style = std::make_pair("EDITING", "background-color: #FFFF00;"); + break; + case State::PLANNED: + activateButton(start_btn_ptr_); + activateButton(clear_route_btn_ptr_); + activateButton(save_file_btn_ptr_); + style = std::make_pair("PLANNED", "background-color: #00FF00;"); + break; + case State::STARTED: + activateButton(stop_btn_ptr_); + activateButton(save_file_btn_ptr_); + style = std::make_pair("STARTED", "background-color: #00FF00;"); + break; + case State::STOPPED: + activateButton(start_btn_ptr_); + activateButton(automatic_mode_btn_ptr_); + activateButton(clear_route_btn_ptr_); + activateButton(save_file_btn_ptr_); + style = std::make_pair("STOPPED", "background-color: #00FF00;"); + break; + case State::ARRIVED: + if (!is_automatic_mode_on_) onClickClearRoute(); // will be set EDITING as next state_ + break; + case State::CLEARED: + is_automatic_mode_on_ ? state_ = State::AUTO_NEXT : state_ = State::EDITING; + updateGUI(); + break; + case State::ERROR: + activateButton(stop_btn_ptr_); + if (!goals_list_.empty()) activateButton(save_file_btn_ptr_); + style = std::make_pair("ERROR", "background-color: #FF0000;"); + break; + case State::PLANNING: + activateButton(clear_route_btn_ptr_); + style = std::make_pair("PLANNING", "background-color: #FFA500;"); + break; + case State::STARTING: + style = std::make_pair("STARTING", "background-color: #FFA500;"); + break; + case State::STOPPING: + style = std::make_pair("STOPPING", "background-color: #FFA500;"); + break; + case State::CLEARING: + style = std::make_pair("CLEARING", "background-color: #FFA500;"); + break; + default: + break; + } + + automatic_mode_btn_ptr_->setStyleSheet(""); + loop_list_btn_ptr_->setStyleSheet(""); + goals_achieved_btn_ptr_->setStyleSheet(""); + if (is_automatic_mode_on_) automatic_mode_btn_ptr_->setStyleSheet("background-color: green"); + if (is_loop_list_on_) loop_list_btn_ptr_->setStyleSheet("background-color: green"); + if (!goals_achieved_file_path_.empty()) + goals_achieved_btn_ptr_->setStyleSheet("background-color: green"); + + updateLabel(engagement_label_ptr_, style.first, style.second); +} + +void AutowareAutomaticGoalPanel::updateGoalIcon(const unsigned goal_index, const QColor & color) +{ + QPixmap pixmap(24, 24); + pixmap.fill(color); + QPainter painter(&pixmap); + painter.setPen(QColor("black")); + painter.setFont(QFont("fixed-width", 10)); + QString text = QString::number(goals_achieved_[goal_index].second); + painter.drawText(QRect(0, 0, 24, 24), Qt::AlignCenter, text); + QIcon icon(pixmap); + goals_list_widget_ptr_->item(static_cast(goal_index))->setIcon(icon); +} + +void AutowareAutomaticGoalPanel::publishMarkers() +{ + MarkerArray text_array; + MarkerArray arrow_array; + // Clear existing + visualization_msgs::msg::Marker marker; + marker.header.stamp = rclcpp::Time(); + marker.ns = "names"; + marker.id = 0; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + text_array.markers.push_back(marker); + marker.ns = "poses"; + arrow_array.markers.push_back(marker); + pub_marker_->publish(text_array); + pub_marker_->publish(arrow_array); + text_array.markers.clear(); + arrow_array.markers.clear(); + // Publish current + for (unsigned i = 0; i < goals_list_.size(); i++) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.ns = "names"; + marker.id = static_cast(i); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = goals_list_[i]->pose; + marker.lifetime = rclcpp::Duration(0, 0); + marker.scale.x = 1.6; + marker.scale.y = 1.6; + marker.scale.z = 1.6; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.color.a = 1.0; + marker.frame_locked = false; + marker.text = "G" + std::to_string(i); + text_array.markers.push_back(marker); + marker.ns = "poses"; + marker.scale.y = 0.2; + marker.scale.z = 0.2; + marker.type = visualization_msgs::msg::Marker::ARROW; + arrow_array.markers.push_back(marker); + } + pub_marker_->publish(text_array); + pub_marker_->publish(arrow_array); +} + +// File +void AutowareAutomaticGoalPanel::saveGoalsList(const std::string & file_path) +{ + YAML::Node node; + for (unsigned i = 0; i < goals_list_.size(); ++i) { + node[i]["position_x"] = goals_list_[i]->pose.position.x; + node[i]["position_y"] = goals_list_[i]->pose.position.y; + node[i]["position_z"] = goals_list_[i]->pose.position.z; + node[i]["orientation_x"] = goals_list_[i]->pose.orientation.x; + node[i]["orientation_y"] = goals_list_[i]->pose.orientation.y; + node[i]["orientation_z"] = goals_list_[i]->pose.orientation.z; + node[i]["orientation_w"] = goals_list_[i]->pose.orientation.w; + } + std::ofstream file_out(file_path); + file_out << node; + file_out.close(); +} + +} // namespace rviz_plugins +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticGoalPanel, rviz_common::Panel) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp new file mode 100644 index 0000000000000..0ec9ca530f074 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp @@ -0,0 +1,147 @@ +// +// Copyright 2023 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 AUTOMATIC_GOAL_PANEL_HPP_ +#define AUTOMATIC_GOAL_PANEL_HPP_ + +#include "automatic_goal_sender.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +class AutowareAutomaticGoalPanel : public rviz_common::Panel, + public automatic_goal::AutowareAutomaticGoalSender +{ + using State = automatic_goal::AutomaticGoalState; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + using RouteState = autoware_adapi_v1_msgs::msg::RouteState; + using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + Q_OBJECT + +public: + explicit AutowareAutomaticGoalPanel(QWidget * parent = nullptr); + void onInitialize() override; + +public Q_SLOTS: // NOLINT for Qt + void onToggleLoopList(bool checked); + void onToggleAutoMode(bool checked); + void onToggleSaveGoalsAchievement(bool checked); + void onClickPlan(); + void onClickStart(); + void onClickStop(); + void onClickClearRoute(); + void onClickRemove(); + void onClickLoadListFromFile(); + void onClickSaveListToFile(); + +private: + // Override + void updateAutoExecutionTimerTick() override; + void onRouteUpdated(const RouteState::ConstSharedPtr msg) override; + void onOperationModeUpdated(const OperationModeState::ConstSharedPtr msg) override; + void onCallResult() override; + void onGoalListUpdated() override; + + // Inputs + void onAppendGoal(const PoseStamped::ConstSharedPtr pose); + + // Visual updates + void updateGUI(); + void updateGoalIcon(const unsigned goal_index, const QColor & color); + void publishMarkers(); + void showMessageBox(const QString & string); + void disableAutomaticMode() { automatic_mode_btn_ptr_->setChecked(false); } + static void activateButton(QAbstractButton * btn) { btn->setEnabled(true); } + static void deactivateButton(QAbstractButton * btn) { btn->setEnabled(false); } + static void updateLabel(QLabel * label, const QString & text, const QString & style_sheet) + { + label->setText(text); + label->setStyleSheet(style_sheet); + } + // File + void saveGoalsList(const std::string & file); + + // Pub/Sub + rclcpp::Publisher::SharedPtr pub_marker_{nullptr}; + rclcpp::Subscription::SharedPtr sub_append_goal_{nullptr}; + + // Containers + rclcpp::Node::SharedPtr raw_node_{nullptr}; + bool is_automatic_mode_on_{false}; + bool is_loop_list_on_{false}; + + // QT Containers + QGroupBox * makeGoalsListGroup(); + QGroupBox * makeRoutingGroup(); + QGroupBox * makeEngagementGroup(); + QTimer * qt_timer_{nullptr}; + QListWidget * goals_list_widget_ptr_{nullptr}; + QLabel * routing_label_ptr_{nullptr}; + QLabel * operation_mode_label_ptr_{nullptr}; + QLabel * engagement_label_ptr_{nullptr}; + QPushButton * loop_list_btn_ptr_{nullptr}; + QPushButton * goals_achieved_btn_ptr_{nullptr}; + QPushButton * load_file_btn_ptr_{nullptr}; + QPushButton * save_file_btn_ptr_{nullptr}; + QPushButton * automatic_mode_btn_ptr_{nullptr}; + QPushButton * remove_selected_goal_btn_ptr_{nullptr}; + QPushButton * clear_route_btn_ptr_{nullptr}; + QPushButton * plan_btn_ptr_{nullptr}; + QPushButton * start_btn_ptr_{nullptr}; + QPushButton * stop_btn_ptr_{nullptr}; +}; +} // namespace rviz_plugins + +#endif // AUTOMATIC_GOAL_PANEL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp new file mode 100644 index 0000000000000..51f7716863465 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp @@ -0,0 +1,227 @@ +// Copyright 2016 Open Source Robotics Foundation, 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 "automatic_goal_sender.hpp" + +namespace automatic_goal +{ +AutowareAutomaticGoalSender::AutowareAutomaticGoalSender() : Node("automatic_goal_sender") {} + +void AutowareAutomaticGoalSender::init() +{ + loadParams(this); + initCommunication(this); + loadGoalsList(goals_list_file_path_); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&AutowareAutomaticGoalSender::updateAutoExecutionTimerTick, this)); + + // Print info + RCLCPP_INFO_STREAM(get_logger(), "GoalsList has been loaded from: " << goals_list_file_path_); + for (auto const & goal : goals_achieved_) + RCLCPP_INFO_STREAM(get_logger(), "Loaded: " << goal.second.first); + RCLCPP_INFO_STREAM( + get_logger(), "Achieved goals will be saved in: " << goals_achieved_file_path_); +} + +void AutowareAutomaticGoalSender::loadParams(rclcpp::Node * node) +{ + namespace fs = std::filesystem; + node->declare_parameter("goals_list_file_path", ""); + node->declare_parameter("goals_achieved_dir_path", ""); + // goals_list + goals_list_file_path_ = node->get_parameter("goals_list_file_path").as_string(); + if (!fs::exists(goals_list_file_path_) || !fs::is_regular_file(goals_list_file_path_)) + throw std::invalid_argument( + "goals_list_file_path parameter - file path is invalid: " + goals_list_file_path_); + // goals_achieved + goals_achieved_file_path_ = node->get_parameter("goals_achieved_dir_path").as_string(); + if (!fs::exists(goals_achieved_file_path_) || fs::is_regular_file(goals_achieved_file_path_)) + throw std::invalid_argument( + "goals_achieved_dir_path - directory path is invalid: " + goals_achieved_file_path_); + goals_achieved_file_path_ += "goals_achieved.log"; +} + +void AutowareAutomaticGoalSender::initCommunication(rclcpp::Node * node) +{ + // Executing + sub_operation_mode_ = node->create_subscription( + "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), + std::bind(&AutowareAutomaticGoalSender::onOperationMode, this, std::placeholders::_1)); + + cli_change_to_autonomous_ = node->create_client( + "/api/operation_mode/change_to_autonomous", rmw_qos_profile_services_default); + + cli_change_to_stop_ = node->create_client( + "/api/operation_mode/change_to_stop", rmw_qos_profile_services_default); + + // Planning + sub_route_ = node->create_subscription( + "/api/routing/state", rclcpp::QoS{1}.transient_local(), + std::bind(&AutowareAutomaticGoalSender::onRoute, this, std::placeholders::_1)); + + cli_clear_route_ = + node->create_client("/api/routing/clear_route", rmw_qos_profile_services_default); + + cli_set_route_ = node->create_client( + "/api/routing/set_route_points", rmw_qos_profile_services_default); +} + +// Sub +void AutowareAutomaticGoalSender::onRoute(const RouteState::ConstSharedPtr msg) +{ + if (msg->state == RouteState::UNSET && state_ == State::CLEARING) + state_ = State::CLEARED; + else if (msg->state == RouteState::SET && state_ == State::PLANNING) + state_ = State::PLANNED; + else if (msg->state == RouteState::ARRIVED && state_ == State::STARTED) + state_ = State::ARRIVED; + onRouteUpdated(msg); +} + +void AutowareAutomaticGoalSender::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + if (msg->mode == OperationModeState::STOP && state_ == State::INITIALIZING) + state_ = State::EDITING; + else if (msg->mode == OperationModeState::STOP && state_ == State::STOPPING) + state_ = State::STOPPED; + else if (msg->mode == OperationModeState::AUTONOMOUS && state_ == State::STARTING) + state_ = State::STARTED; + onOperationModeUpdated(msg); +} + +// Update +void AutowareAutomaticGoalSender::updateGoalsList() +{ + unsigned i = 0; + for (const auto & goal : goals_list_) { + std::stringstream ss; + ss << std::fixed << std::setprecision(2); + tf2::Quaternion tf2_quat; + tf2::convert(goal->pose.orientation, tf2_quat); + ss << "G" << i << " (" << goal->pose.position.x << ", "; + ss << goal->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; + goals_achieved_.insert({i++, std::make_pair(ss.str(), 0)}); + } + onGoalListUpdated(); +} + +void AutowareAutomaticGoalSender::updateAutoExecutionTimerTick() +{ + auto goal = goals_achieved_[current_goal_].first; + + if (state_ == State::INITIALIZING) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 3000, "Initializing... waiting for OperationModeState::STOP"); + + } else if (state_ == State::EDITING) { // skip the editing step by default + state_ = State::AUTO_NEXT; + + } else if (state_ == State::AUTO_NEXT) { // plan to next goal + RCLCPP_INFO_STREAM(get_logger(), goal << ": Goal set as the next. Planning in progress..."); + if (callPlanToGoalIndex(cli_set_route_, current_goal_)) state_ = State::PLANNING; + + } else if (state_ == State::PLANNED) { // start plan to next goal + RCLCPP_INFO_STREAM(get_logger(), goal << ": Route has been planned. Route starting..."); + if (callService(cli_change_to_autonomous_)) state_ = State::STARTING; + + } else if (state_ == State::STARTED) { + RCLCPP_INFO_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, goal << ": Driving to the goal."); + + } else if (state_ == State::ARRIVED) { // clear plan after achieving next goal, goal++ + RCLCPP_INFO_STREAM( + get_logger(), goal << ": Goal has been ACHIEVED " << ++goals_achieved_[current_goal_].second + << " times. Route clearing..."); + updateAchievedGoalsFile(current_goal_); + if (callService(cli_clear_route_)) state_ = State::CLEARING; + + } else if (state_ == State::CLEARED) { + RCLCPP_INFO_STREAM(get_logger(), goal << ": Route has been cleared."); + current_goal_++; + current_goal_ = current_goal_ % goals_list_.size(); + state_ = State::AUTO_NEXT; + + } else if (state_ == State::STOPPED) { + RCLCPP_WARN_STREAM( + get_logger(), goal << ": The execution has been stopped unexpectedly! Restarting..."); + if (callService(cli_change_to_autonomous_)) state_ = State::STARTING; + + } else if (state_ == State::ERROR) { + timer_->cancel(); + throw std::runtime_error(goal + ": Error encountered during execution!"); + } +} + +// File +void AutowareAutomaticGoalSender::loadGoalsList(const std::string & file_path) +{ + YAML::Node node = YAML::LoadFile(file_path); + goals_list_.clear(); + for (auto && goal : node) { + std::shared_ptr pose = std::make_shared(); + pose->header.frame_id = "map"; + pose->header.stamp = rclcpp::Time(); + pose->pose.position.x = goal["position_x"].as(); + pose->pose.position.y = goal["position_y"].as(); + pose->pose.position.z = goal["position_z"].as(); + pose->pose.orientation.x = goal["orientation_x"].as(); + pose->pose.orientation.y = goal["orientation_y"].as(); + pose->pose.orientation.z = goal["orientation_z"].as(); + pose->pose.orientation.w = goal["orientation_w"].as(); + goals_list_.push_back(pose); + } + resetAchievedGoals(); + updateGoalsList(); +} + +void AutowareAutomaticGoalSender::updateAchievedGoalsFile(const unsigned goal_index) +{ + if (!goals_achieved_file_path_.empty()) { + std::ofstream out(goals_achieved_file_path_, std::fstream::app); + std::stringstream ss; + ss << "[" << getTimestamp() << "] Achieved: " << goals_achieved_[goal_index].first; + ss << ", Current number of achievements: " << goals_achieved_[goal_index].second << "\n"; + out << ss.str(); + out.close(); + } +} + +void AutowareAutomaticGoalSender::resetAchievedGoals() +{ + goals_achieved_.clear(); + if (!goals_achieved_file_path_.empty()) { + std::ofstream out(goals_achieved_file_path_, std::fstream::app); + out << "[" << getTimestamp() + << "] GoalsList was loaded from a file or a goal was removed - counters have been reset\n"; + out.close(); + } +} +} // namespace automatic_goal + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node{nullptr}; + try { + node = std::make_shared(); + node->init(); + } catch (const std::exception & e) { + fprintf(stderr, "%s Exiting...\n", e.what()); + return 1; + } + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp new file mode 100644 index 0000000000000..aacdada352811 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp @@ -0,0 +1,174 @@ +// Copyright 2016 Open Source Robotics Foundation, 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 AUTOMATIC_GOAL_SENDER_HPP_ +#define AUTOMATIC_GOAL_SENDER_HPP_ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace automatic_goal +{ +enum class AutomaticGoalState { + INITIALIZING, + EDITING, + PLANNING, + PLANNED, + STARTING, + STARTED, + ARRIVED, + AUTO_NEXT, + STOPPING, + STOPPED, + CLEARING, + CLEARED, + ERROR, +}; + +class AutowareAutomaticGoalSender : public rclcpp::Node +{ + using State = AutomaticGoalState; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + using RouteState = autoware_adapi_v1_msgs::msg::RouteState; + using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + +public: + AutowareAutomaticGoalSender(); + void init(); + +protected: + void initCommunication(rclcpp::Node * node); + // Calls + bool callPlanToGoalIndex( + const rclcpp::Client::SharedPtr client, const unsigned goal_index) + { + if (!client->service_is_ready()) { + RCLCPP_WARN(get_logger(), "SetRoutePoints client is unavailable"); + return false; + } + + auto req = std::make_shared(); + req->header = goals_list_.at(goal_index)->header; + req->goal = goals_list_.at(goal_index)->pose; + client->async_send_request( + req, [this](typename rclcpp::Client::SharedFuture result) { + if (result.get()->status.code != 0) state_ = State::ERROR; + printCallResult(result); + onCallResult(); + }); + return true; + } + template + bool callService(const typename rclcpp::Client::SharedPtr client) + { + if (!client->service_is_ready()) { + RCLCPP_WARN(get_logger(), "Client is unavailable"); + return false; + } + + auto req = std::make_shared(); + client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { + if (result.get()->status.code != 0) state_ = State::ERROR; + printCallResult(result); + onCallResult(); + }); + return true; + } + template + void printCallResult(typename rclcpp::Client::SharedFuture result) + { + if (result.get()->status.code != 0) { + RCLCPP_ERROR( + get_logger(), "Service type \"%s\" status: %d, %s", typeid(T).name(), + result.get()->status.code, result.get()->status.message.c_str()); + } else { + RCLCPP_DEBUG( + get_logger(), "Service type \"%s\" status: %d, %s", typeid(T).name(), + result.get()->status.code, result.get()->status.message.c_str()); + } + } + + // Update + void updateGoalsList(); + virtual void updateAutoExecutionTimerTick(); + + // File + void loadGoalsList(const std::string & file_path); + void updateAchievedGoalsFile(const unsigned goal_index); + void resetAchievedGoals(); + static std::string getTimestamp() + { + char buffer[128]; + std::time_t now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::strftime(&buffer[0], sizeof(buffer), "%Y-%m-%d %H:%M:%S", std::localtime(&now)); + return std::string{buffer}; + } + + // Sub + void onRoute(const RouteState::ConstSharedPtr msg); + void onOperationMode(const OperationModeState::ConstSharedPtr msg); + + // Interface + virtual void onRouteUpdated(const RouteState::ConstSharedPtr) {} + virtual void onOperationModeUpdated(const OperationModeState::ConstSharedPtr) {} + virtual void onCallResult() {} + virtual void onGoalListUpdated() {} + + // Cli + rclcpp::Client::SharedPtr cli_change_to_autonomous_{nullptr}; + rclcpp::Client::SharedPtr cli_change_to_stop_{nullptr}; + rclcpp::Client::SharedPtr cli_clear_route_{nullptr}; + rclcpp::Client::SharedPtr cli_set_route_{nullptr}; + + // Containers + unsigned current_goal_{0}; + State state_{State::INITIALIZING}; + std::vector goals_list_{}; + std::map> goals_achieved_{}; + std::string goals_achieved_file_path_{}; + +private: + void loadParams(rclcpp::Node * node); + + // Sub + rclcpp::Subscription::SharedPtr sub_route_{nullptr}; + rclcpp::Subscription::SharedPtr sub_operation_mode_{nullptr}; + + // Containers + std::string goals_list_file_path_{}; + rclcpp::TimerBase::SharedPtr timer_{nullptr}; +}; +} // namespace automatic_goal +#endif // AUTOMATIC_GOAL_SENDER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp index 14026d47abdbb..99e3018962eb2 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -44,7 +44,7 @@ Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::DetectedObject & Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::TrackedObject & object); Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::PredictedObject & object); double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); - +Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset); } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp index c08fd6c22c357..2cdb385d19ad6 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -210,4 +210,37 @@ double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) throw std::logic_error("The shape type is not supported in tier4_autoware_utils."); } + +// NOTE: The number of vertices on the expanded polygon by boost::geometry::buffer +// is larger than the original one. +// This function fixes the issue. +Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset) +{ + // NOTE: input_polygon is supposed to have a duplicated point. + const size_t num_points = input_polygon.outer().size() - 1; + + Polygon2d expanded_polygon; + for (size_t i = 0; i < num_points; ++i) { + const auto & curr_p = input_polygon.outer().at(i); + const auto & next_p = input_polygon.outer().at(i + 1); + const auto & prev_p = + i == 0 ? input_polygon.outer().at(num_points - 1) : input_polygon.outer().at(i - 1); + + Eigen::Vector2d current_to_next(next_p.x() - curr_p.x(), next_p.y() - curr_p.y()); + Eigen::Vector2d current_to_prev(prev_p.x() - curr_p.x(), prev_p.y() - curr_p.y()); + current_to_next.normalize(); + current_to_prev.normalize(); + + const Eigen::Vector2d offset_vector = (-current_to_next - current_to_prev).normalized(); + const double theta = std::acos(offset_vector.dot(current_to_next)); + const double scaled_offset = offset / std::sin(theta); + const Eigen::Vector2d offset_point = + Eigen::Vector2d(curr_p.x(), curr_p.y()) + offset_vector * scaled_offset; + + expanded_polygon.outer().push_back(Point2d(offset_point.x(), offset_point.y())); + } + + boost::geometry::correct(expanded_polygon); + return expanded_polygon; +} } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp index 421e434c60e1e..3e028a481e380 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp @@ -259,3 +259,45 @@ TEST(boost_geometry, boost_getArea) EXPECT_DOUBLE_EQ(anti_clock_wise_area, x * y); } } + +TEST(boost_geometry, boost_expandPolygon) +{ + using tier4_autoware_utils::expandPolygon; + + { // box with a certain offset + Polygon2d box_poly{{{-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}}}; + const auto expanded_poly = expandPolygon(box_poly, 1.0); + + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).x(), -2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).y(), -2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).x(), -2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).y(), 2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).x(), 2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).y(), 2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).x(), 2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).y(), -2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).x(), -2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).y(), -2.0); + } + + { // box with no offset + Polygon2d box_poly{{{-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}}}; + const auto expanded_poly = expandPolygon(box_poly, 0.0); + + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).x(), -1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).y(), -1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).x(), -1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).y(), 1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).x(), 1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).y(), 1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).x(), 1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).y(), -1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).x(), -1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).y(), -1.0); + } + + { // empty polygon + Polygon2d empty_poly; + EXPECT_THROW(expandPolygon(empty_poly, 1.0), std::out_of_range); + } +} diff --git a/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png b/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png differ diff --git a/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml b/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml index 3e75efc966985..853a471c7c6e4 100644 --- a/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml @@ -11,6 +11,10 @@ type="rviz_plugins::BusInitialPoseTool" base_class_type="rviz_common::Tool"> + + diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp index 654683ea925eb..cf07eeed99b3a 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp @@ -238,8 +238,104 @@ Object BusInitialPoseTool::createObjectMsg() const return object; } +BikeInitialPoseTool::BikeInitialPoseTool() +{ + // short cut below cyclist + shortcut_key_ = 'c'; + + enable_interactive_property_ = new rviz_common::properties::BoolProperty( + "Interactive", false, "Enable/Disable interactive action by manipulating mouse.", + getPropertyContainer()); + property_frame_ = new rviz_common::properties::TfFrameProperty( + "Target Frame", rviz_common::properties::TfFrameProperty::FIXED_FRAME_STRING, + "The TF frame where the point cloud is output.", getPropertyContainer(), nullptr, true); + topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "/simulation/dummy_perception_publisher/object_info", + "The topic on which to publish dummy object info.", getPropertyContainer(), SLOT(updateTopic()), + this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.03, "X standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.03, "Y standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_z_ = new rviz_common::properties::FloatProperty( + "Z std deviation", 0.03, "Z standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", 5.0 * M_PI / 180.0, "Theta standard deviation for initial pose [rad]", + getPropertyContainer()); + length_ = new rviz_common::properties::FloatProperty( + "L vehicle length", 1.5, "L length for vehicle [m]", getPropertyContainer()); + width_ = new rviz_common::properties::FloatProperty( + "W vehicle width", 0.5, "W width for vehicle [m]", getPropertyContainer()); + height_ = new rviz_common::properties::FloatProperty( + "H vehicle height", 1.0, "H height for vehicle [m]", getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer()); + velocity_ = new rviz_common::properties::FloatProperty( + "Velocity", 0.0, "velocity [m/s]", getPropertyContainer()); + accel_ = new rviz_common::properties::FloatProperty( + "Acceleration", 0.0, "acceleration [m/s^2]", getPropertyContainer()); + max_velocity_ = new rviz_common::properties::FloatProperty( + "Max velocity", 33.3, "Max velocity [m/s]", getPropertyContainer()); + min_velocity_ = new rviz_common::properties::FloatProperty( + "Min velocity", -33.3, "Min velocity [m/s]", getPropertyContainer()); + label_ = new rviz_common::properties::EnumProperty( + "Type", "BICYCLE", "BICYCLE or MOTORCYCLE", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_z_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); + label_->addOption("BICYCLE", ObjectClassification::BICYCLE); + label_->addOption("MOTORCYCLE", ObjectClassification::MOTORCYCLE); +} + +void BikeInitialPoseTool::onInitialize() +{ + rviz_plugins::InteractiveObjectTool::onInitialize(); + setName("2D Dummy Bicycle"); + updateTopic(); +} + +Object BikeInitialPoseTool::createObjectMsg() const +{ + Object object{}; + std::string fixed_frame = context_->getFixedFrame().toStdString(); + + // header + object.header.frame_id = fixed_frame; + object.header.stamp = clock_->now(); + + // semantic + object.classification.label = label_->getOptionInt(); + object.classification.probability = 1.0; + + // shape + object.shape.type = Shape::BOUNDING_BOX; + object.shape.dimensions.x = length_->getFloat(); + object.shape.dimensions.y = width_->getFloat(); + object.shape.dimensions.z = height_->getFloat(); + + // initial state + object.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); + object.initial_state.pose_covariance.covariance[0] = + std_dev_x_->getFloat() * std_dev_x_->getFloat(); + object.initial_state.pose_covariance.covariance[7] = + std_dev_y_->getFloat() * std_dev_y_->getFloat(); + object.initial_state.pose_covariance.covariance[14] = + std_dev_z_->getFloat() * std_dev_z_->getFloat(); + object.initial_state.pose_covariance.covariance[35] = + std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); + + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(object.id.uuid.begin(), object.id.uuid.end(), bit_eng); + + return object; +} + } // end namespace rviz_plugins #include PLUGINLIB_EXPORT_CLASS(rviz_plugins::CarInitialPoseTool, rviz_common::Tool) PLUGINLIB_EXPORT_CLASS(rviz_plugins::BusInitialPoseTool, rviz_common::Tool) +PLUGINLIB_EXPORT_CLASS(rviz_plugins::BikeInitialPoseTool, rviz_common::Tool) diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp index 43afe7db95dcd..4fc6aeb71771d 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp @@ -73,6 +73,17 @@ class BusInitialPoseTool : public InteractiveObjectTool [[nodiscard]] Object createObjectMsg() const override; }; +class BikeInitialPoseTool : public InteractiveObjectTool +{ +public: + BikeInitialPoseTool(); + void onInitialize() override; + [[nodiscard]] Object createObjectMsg() const override; + +private: + rviz_common::properties::EnumProperty * label_; +}; + } // namespace rviz_plugins #endif // TOOLS__CAR_POSE_HPP_ diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp index 6f0b382eb0b58..fc40b08be822b 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include diff --git a/common/tier4_planning_rviz_plugin/include/path/display.hpp b/common/tier4_planning_rviz_plugin/include/path/display.hpp index 63788b8f7f9b3..d483cac062b04 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display.hpp @@ -84,19 +84,14 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay { public: AutowarePathWithDrivableAreaDisplay() + : property_drivable_area_view_{"View Drivable Area", true, "", this}, + property_drivable_area_alpha_{"Alpha", 0.999, "", &property_drivable_area_view_}, + property_drivable_area_color_{"Color", QColor(0, 148, 205), "", &property_drivable_area_view_}, + property_drivable_area_width_{"Width", 0.3f, "", &property_drivable_area_view_} { - property_drivable_area_view_ = new rviz_common::properties::BoolProperty( - "View Drivable Area", true, "", this, SLOT(updateVisualization())); - property_drivable_area_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 0.999, "", property_drivable_area_view_, SLOT(updateVisualization()), this); - property_drivable_area_alpha_->setMin(0.0); - property_drivable_area_alpha_->setMax(1.0); - property_drivable_area_color_ = new rviz_common::properties::ColorProperty( - "Color", QColor(0, 148, 205), "", property_drivable_area_view_, SLOT(updateVisualization()), - this); - property_drivable_area_width_ = new rviz_common::properties::FloatProperty( - "Width", 0.3f, "", property_drivable_area_view_, SLOT(updateVisualization()), this); - property_drivable_area_width_->setMin(0.001); + property_drivable_area_alpha_.setMin(0.0); + property_drivable_area_alpha_.setMax(1.0); + property_drivable_area_width_.setMin(0.001); } ~AutowarePathWithDrivableAreaDisplay() @@ -137,11 +132,11 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay return; } - if (property_drivable_area_view_->getBool()) { + if (property_drivable_area_view_.getBool()) { Ogre::ColourValue color = - rviz_common::properties::qtToOgre(property_drivable_area_color_->getColor()); - color.a = property_drivable_area_alpha_->getFloat(); - const auto line_width = property_drivable_area_width_->getFloat(); + rviz_common::properties::qtToOgre(property_drivable_area_color_.getColor()); + color.a = property_drivable_area_alpha_.getFloat(); + const auto line_width = property_drivable_area_width_.getFloat(); visualizeBound(msg_ptr->left_bound, color, line_width, left_bound_object_); visualizeBound(msg_ptr->right_bound, color, line_width, right_bound_object_); } @@ -151,10 +146,10 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay Ogre::ManualObject * left_bound_object_{nullptr}; Ogre::ManualObject * right_bound_object_{nullptr}; - rviz_common::properties::BoolProperty * property_drivable_area_view_; - rviz_common::properties::ColorProperty * property_drivable_area_color_; - rviz_common::properties::FloatProperty * property_drivable_area_alpha_; - rviz_common::properties::FloatProperty * property_drivable_area_width_; + rviz_common::properties::BoolProperty property_drivable_area_view_; + rviz_common::properties::FloatProperty property_drivable_area_alpha_; + rviz_common::properties::ColorProperty property_drivable_area_color_; + rviz_common::properties::FloatProperty property_drivable_area_width_; }; class AutowarePathWithLaneIdDisplay diff --git a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp index 88fbdc483f89c..9e3ea7ed3ab4d 100644 --- a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp +++ b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp @@ -28,7 +28,7 @@ void setFormatDate(QLabel * line, double time) char buffer[128]; auto seconds = static_cast(time); strftime(buffer, sizeof(buffer), "%Y-%m-%d-%H-%M-%S", localtime(&seconds)); - line->setText(QString("- ") + QString(buffer) + QString(".mp4")); + line->setText(QString("- ") + QString(buffer)); } AutowareScreenCapturePanel::AutowareScreenCapturePanel(QWidget * parent) @@ -104,7 +104,8 @@ void AutowareScreenCapturePanel::onRateChanged() {} void AutowareScreenCapturePanel::onClickScreenCapture() { - const std::string time_text = "capture/" + ros_time_label_->text().toStdString(); + const std::string time_text = + "capture/" + file_name_prefix_->text().toStdString() + ros_time_label_->text().toStdString(); getDisplayContext()->getViewManager()->getRenderPanel()->getRenderWindow()->captureScreenShot( time_text + ".png"); } diff --git a/control/autonomous_emergency_braking/CMakeLists.txt b/control/autonomous_emergency_braking/CMakeLists.txt new file mode 100644 index 0000000000000..53a629fafa0cc --- /dev/null +++ b/control/autonomous_emergency_braking/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.14) +project(autonomous_emergency_braking) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED) + +include_directories( + include + SYSTEM + ${PCL_INCLUDE_DIRS} +) + +set(AEB_NODE ${PROJECT_NAME}_node) +ament_auto_add_library(${AEB_NODE} SHARED + src/node.cpp +) + +rclcpp_components_register_node(${AEB_NODE} + PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB" + EXECUTABLE ${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/autonomous_emergency_braking/README.md b/control/autonomous_emergency_braking/README.md new file mode 100644 index 0000000000000..d1cd80625f2ac --- /dev/null +++ b/control/autonomous_emergency_braking/README.md @@ -0,0 +1,112 @@ +# Autonomous Emergency Braking (AEB) + +## Purpose / Role + +`autonomous_emergency_braking` is a module that prevents collisions with obstacles on the predicted path created by a control module or sensor values estimated from the control module. + +### Assumptions + +This module has following assumptions. + +- It is used when driving at low speeds (about 15 km/h). + +- The predicted path of the ego vehicle can be made from either the path created from sensors or the path created from a control module, or both. + +- The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses point cloud as obstacles. + +![aeb_range](./image/range.drawio.svg) + +### Limitations + +- AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud. + +- Longitudinal acceleration information obtained from sensors is not used due to the high amount of noise. + +- The accuracy of the predicted path created from sensor data depends on the accuracy of sensors attached to the ego vehicle. + +## Parameters + +| Name | Unit | Type | Description | Default value | +| :------------------------ | :----- | :----- | :-------------------------------------------------------------------------- | :------------ | +| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | +| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | +| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | +| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | +| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | +| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | +| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | +| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | +| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | +| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | +| prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | +| prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | +| aeb_hz | [-] | double | 　frequency at which AEB operates per second | 10 | + +## Inner-workings / Algorithms + +AEB has the following steps before it outputs the emergency stop signal. + +1. Activate AEB if necessary. + +2. Generate a predicted path of the ego vehicle. + +3. Get target obstacles from the input point cloud. + +4. Collision check with target obstacles. + +5. Send emergency stop signals to `/diagnostics`. + +We give more details of each section below. + +### 1. Activate AEB if necessary + +We do not activate AEB module if it satisfies the following conditions. + +- Ego vehicle is not in autonomous driving state + +- When the ego vehicle is not moving (Current Velocity is very low) + +### 2. Generate a predicted path of the ego vehicle + +AEB generates a predicted path based on current velocity and current angular velocity obtained from attached sensors. Note that if `use_imu_path` is `false`, it skips this step. This predicted path is generated as: + +$$ +x_{k+1} = x_k + v cos(\theta_k) dt \\ +y_{k+1} = y_k + v sin(\theta_k) dt \\ +\theta_{k+1} = \theta_k + \omega dt +$$ + +where $v$ and $\omega$ are current longitudinal velocity and angular velocity respectively. $dt$ is time interval that users can define in advance. + +### 3. Get target obstacles from the input point cloud + +After generating the ego predicted path, we select target obstacles from the input point cloud. This obstacle filtering has two major steps, which are rough filtering and rigorous filtering. + +#### Rough filtering + +In rough filtering step, we select target obstacle with simple filter. Create a search area up to a certain distance (default 5[m]) away from the predicted path of the ego vehicle and ignore the point cloud (obstacles) that are not within it. The image of the rough filtering is illustrated below. + +![rough_filtering](./image/obstacle_filtering_1.drawio.svg) + +#### Rigorous filtering + +After rough filtering, it performs a geometric collision check to determine whether the filtered obstacles actually have possibility to collide with the ego vehicle. In this check, the ego vehicle is represented as a rectangle, and the point cloud obstacles are represented as points. + +![rigorous_filtering](./image/obstacle_filtering_2.drawio.svg) + +### 4. Collision check with target obstacles + +In the fourth step, it checks the collision with filtered obstacles using RSS distance. RSS is formulated as: + +$$ +d = v_{ego}*t_{response} + v_{ego}^2/(2*a_{min}) - v_{obj}^2/(2*a_{obj_{min}}) + offset +$$ + +where $v_{ego}$ and $v_{obj}$ is current ego and obstacle velocity, $a_{min}$ and $a_{obj_{min}}$ is ego and object minimum acceleration (maximum deceleration), $t_{response}$ is response time of the ego vehicle to start deceleration. Therefore the distance from the ego vehicle to the obstacle is smaller than this RSS distance $d$, the ego vehicle send emergency stop signals. This is illustrated in the following picture. + +![rss_check](./image/rss_check.drawio.svg) + +### 5. Send emergency stop signals to `/diagnostics` + +If AEB detects collision with point cloud obstacles in the previous step, it sends emergency signal to `/diagnostics` in this step. Note that in order to enable emergency stop, it has to send ERROR level emergency. Moreover, AEB user should modify the setting file to keep the emergency level, otherwise Autoware does not hold the emergency state. diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml new file mode 100644 index 0000000000000..6019eaae503a0 --- /dev/null +++ b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + use_predicted_trajectory: true + use_imu_path: true + voxel_grid_x: 0.05 + voxel_grid_y: 0.05 + voxel_grid_z: 100000.0 + min_generated_path_length: 0.5 + expand_width: 0.1 + longitudinal_offset: 2.0 + t_response: 1.0 + a_ego_min: -3.0 + a_obj_min: -1.0 + prediction_time_horizon: 1.5 + prediction_time_interval: 0.1 + aeb_hz: 10.0 diff --git a/control/autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg b/control/autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg new file mode 100644 index 0000000000000..c1fa2e16ff7fb --- /dev/null +++ b/control/autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg @@ -0,0 +1,133 @@ + + + + + + + + + + + + + + + +
+
+
+ Ego Predicted Path +
+
+
+ + Ego Predicted Path + + + + + + + + + + + + + +
+
+
+ Pointcloud +
+
+
+ + Pointcloud + + + + + + + + +
+
+
+ Ignore point outside of the search area +
+
+
+ + Ignore point outside of the search area + + + + + + + + + + +
+
+
+ Search Area +
+
+
+ + Search Area + + + + + + + + +
+
+
+ Ignore point behind the ego vehicle +
+
+
+ + Ignore point behind the ego vehicle + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg b/control/autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg new file mode 100644 index 0000000000000..d0340892aa5e9 --- /dev/null +++ b/control/autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/autonomous_emergency_braking/image/range.drawio.svg b/control/autonomous_emergency_braking/image/range.drawio.svg new file mode 100644 index 0000000000000..d647d9060a67c --- /dev/null +++ b/control/autonomous_emergency_braking/image/range.drawio.svg @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Obstacles in this range might not be able to react +
+
+
+ + Obstacles in this range might not be able to rea... + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/autonomous_emergency_braking/image/rss_check.drawio.svg b/control/autonomous_emergency_braking/image/rss_check.drawio.svg new file mode 100644 index 0000000000000..71d6e6ff8932c --- /dev/null +++ b/control/autonomous_emergency_braking/image/rss_check.drawio.svg @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + +
+
+
+ obj_dist +
+
+
+ + obj_dist + + + + + + + + + + + +
+
+
+ + rss_dist +
+ +
+
+
+ + rss_dist + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp new file mode 100644 index 0000000000000..64a3079df106a --- /dev/null +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -0,0 +1,150 @@ +// Copyright 2023 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 AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#define AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion::control::autonomous_emergency_braking +{ + +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_system_msgs::msg::AutowareState; +using autoware_auto_vehicle_msgs::msg::VelocityReport; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::Imu; +using sensor_msgs::msg::PointCloud2; +using PointCloud = pcl::PointCloud; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using vehicle_info_util::VehicleInfo; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +using Path = std::vector; + +struct ObjectData +{ + geometry_msgs::msg::Point position; + double velocity; +}; + +class AEB : public rclcpp::Node +{ +public: + explicit AEB(const rclcpp::NodeOptions & node_options); + + // subscriber + rclcpp::Subscription::SharedPtr sub_point_cloud_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_predicted_traj_; + rclcpp::Subscription::SharedPtr sub_autoware_state_; + + // publisher + rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; + rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug + + // timer + rclcpp::TimerBase::SharedPtr timer_; + + // callback + void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); + void onVelocity(const VelocityReport::ConstSharedPtr input_msg); + void onImu(const Imu::ConstSharedPtr input_msg); + void onTimer(); + void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg); + void onAutowareState(const AutowareState::ConstSharedPtr input_msg); + + bool isDataReady(); + + // main function + void onCheckCollision(DiagnosticStatusWrapper & stat); + bool checkCollision(); + bool hasCollision( + const double current_v, const Path & ego_path, const std::vector & ego_polys); + + void generateEgoPath( + const double curr_v, const double curr_w, Path & path, std::vector & polygons); + void generateEgoPath( + const Trajectory & predicted_traj, Path & path, std::vector & polygons); + void createObjectData( + const Path & ego_path, const std::vector & ego_polys, + std::vector & objects); + + void addMarker( + const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, + const double color_r, const double color_g, const double color_b, const double color_a, + const std::string & path_ns, const std::string & poly_ns, MarkerArray & debug_markers); + + PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; + VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; + Imu::ConstSharedPtr imu_ptr_{nullptr}; + Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; + AutowareState::ConstSharedPtr autoware_state_{nullptr}; + + tf2_ros::Buffer tf_buffer_{get_clock()}; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + + // vehicle info + VehicleInfo vehicle_info_; + + // diag + Updater updater_{this}; + + // member variables + bool use_predicted_trajectory_; + bool use_imu_path_; + double voxel_grid_x_; + double voxel_grid_y_; + double voxel_grid_z_; + double min_generated_path_length_; + double expand_width_; + double longitudinal_offset_; + double t_response_; + double a_ego_min_; + double a_obj_min_; + double prediction_time_horizon_; + double prediction_time_interval_; +}; +} // namespace autoware::motion::control::autonomous_emergency_braking + +#endif // AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ diff --git a/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml b/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml new file mode 100644 index 0000000000000..cf6640ec6be52 --- /dev/null +++ b/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml new file mode 100644 index 0000000000000..e4e7da168b239 --- /dev/null +++ b/control/autonomous_emergency_braking/package.xml @@ -0,0 +1,43 @@ + + + + autonomous_emergency_braking + 0.1.0 + Autonomous Emergency Braking package as a ROS2 node + Yutaka Shimizu + Takamasa Horibe + Apache License 2.0 + + ament_cmake + + autoware_cmake + + autoware_auto_control_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + diagnostic_updater + geometry_msgs + motion_utils + nav_msgs + pcl_conversions + pcl_ros + pointcloud_preprocessor + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp new file mode 100644 index 0000000000000..92ede8b3a9c05 --- /dev/null +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -0,0 +1,495 @@ +// 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 "autonomous_emergency_braking/node.hpp" + +#include +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +namespace autoware::motion::control::autonomous_emergency_braking +{ +using diagnostic_msgs::msg::DiagnosticStatus; +namespace bg = boost::geometry; + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + bg::append(polygon.outer(), point); +} + +Polygon2d createPolygon( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) +{ + Polygon2d polygon; + + const double longitudinal_offset = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + expand_width; + const double rear_overhang = vehicle_info.rear_overhang_m; + + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); + + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); + + polygon = tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); + + Polygon2d hull_polygon; + bg::convex_hull(polygon, hull_polygon); + + return hull_polygon; +} + +AEB::AEB(const rclcpp::NodeOptions & node_options) +: Node("AEB", node_options), + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) +{ + // Subscribers + sub_point_cloud_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&AEB::onPointCloud, this, std::placeholders::_1)); + + sub_velocity_ = this->create_subscription( + "~/input/velocity", rclcpp::QoS{1}, std::bind(&AEB::onVelocity, this, std::placeholders::_1)); + + sub_imu_ = this->create_subscription( + "~/input/imu", rclcpp::QoS{1}, std::bind(&AEB::onImu, this, std::placeholders::_1)); + + sub_predicted_traj_ = this->create_subscription( + "~/input/predicted_trajectory", rclcpp::QoS{1}, + std::bind(&AEB::onPredictedTrajectory, this, std::placeholders::_1)); + + sub_autoware_state_ = this->create_subscription( + "/autoware/state", rclcpp::QoS{1}, + std::bind(&AEB::onAutowareState, this, std::placeholders::_1)); + + // Publisher + pub_obstacle_pointcloud_ = + this->create_publisher("~/debug/obstacle_pointcloud", 1); + debug_ego_path_publisher_ = this->create_publisher("~/debug/markers", 1); + + // Diagnostics + updater_.setHardwareID("autonomous_emergency_braking"); + updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision); + + // parameter + use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); + use_imu_path_ = declare_parameter("use_imu_path"); + voxel_grid_x_ = declare_parameter("voxel_grid_x"); + voxel_grid_y_ = declare_parameter("voxel_grid_y"); + voxel_grid_z_ = declare_parameter("voxel_grid_z"); + min_generated_path_length_ = declare_parameter("min_generated_path_length"); + expand_width_ = declare_parameter("expand_width"); + longitudinal_offset_ = declare_parameter("longitudinal_offset"); + t_response_ = declare_parameter("t_response"); + a_ego_min_ = declare_parameter("a_ego_min"); + a_obj_min_ = declare_parameter("a_obj_min"); + prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); + prediction_time_interval_ = declare_parameter("prediction_time_interval"); + + // start time + const double aeb_hz = declare_parameter("aeb_hz"); + const auto period_ns = rclcpp::Rate(aeb_hz).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&AEB::onTimer, this)); +} + +void AEB::onTimer() { updater_.force_update(); } + +void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg) +{ + current_velocity_ptr_ = input_msg; +} + +void AEB::onImu(const Imu::ConstSharedPtr input_msg) { imu_ptr_ = input_msg; } + +void AEB::onPredictedTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) +{ + predicted_traj_ptr_ = input_msg; +} + +void AEB::onAutowareState(const AutowareState::ConstSharedPtr input_msg) +{ + autoware_state_ = input_msg; +} + +void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) +{ + PointCloud::Ptr pointcloud_ptr(new PointCloud); + pcl::fromROSMsg(*input_msg, *pointcloud_ptr); + + if (input_msg->header.frame_id != "base_link") { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB]: Input point cloud frame is not base_link and it is " << input_msg->header.frame_id); + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", input_msg->header.frame_id, input_msg->header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *input_msg, transformed_points); + pcl::fromROSMsg(transformed_points, *pointcloud_ptr); + } + + pcl::VoxelGrid filter; + PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud); + filter.setInputCloud(pointcloud_ptr); + filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_); + filter.filter(*no_height_filtered_pointcloud_ptr); + + obstacle_ros_pointcloud_ptr_ = std::make_shared(); + pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); + obstacle_ros_pointcloud_ptr_->header = input_msg->header; + pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_); +} + +bool AEB::isDataReady() +{ + const auto missing = [this](const auto & name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name); + return false; + }; + + if (!current_velocity_ptr_) { + return missing("ego velocity"); + } + + if (!obstacle_ros_pointcloud_ptr_) { + return missing("object pointcloud"); + } + + if (use_imu_path_ && !imu_ptr_) { + return missing("imu"); + } + + if (use_predicted_trajectory_ && !predicted_traj_ptr_) { + return missing("control predicted trajectory"); + } + + if (!autoware_state_) { + return missing("autoware_state"); + } + + return true; +} + +void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) +{ + if (checkCollision()) { + const std::string error_msg = "[AEB]: Emergency Brake"; + const auto diag_level = DiagnosticStatus::ERROR; + stat.summary(diag_level, error_msg); + return; + } + + const std::string error_msg = "[AEB]: No Collision"; + const auto diag_level = DiagnosticStatus::OK; + stat.summary(diag_level, error_msg); +} + +bool AEB::checkCollision() +{ + // step1. check data + if (!isDataReady()) { + return false; + } + + // if not driving, disable aeb + if (autoware_state_->state != AutowareState::DRIVING) { + return false; + } + + // step2. create velocity data check if the vehicle stops or not + const double current_v = current_velocity_ptr_->longitudinal_velocity; + if (current_v < 0.1) { + return false; + } + + MarkerArray debug_markers; + + // step3. create ego path based on sensor data + Path ego_path; + std::vector ego_polys; + if (use_imu_path_) { + const double current_w = imu_ptr_->angular_velocity.z; + constexpr double color_r = 0.0 / 256.0; + constexpr double color_g = 148.0 / 256.0; + constexpr double color_b = 205.0 / 256.0; + constexpr double color_a = 0.999; + const auto current_time = get_clock()->now(); + generateEgoPath(current_v, current_w, ego_path, ego_polys); + addMarker( + current_time, ego_path, ego_polys, color_r, color_g, color_b, color_a, "ego_path", + "ego_polygons", debug_markers); + } + + // step4. transform predicted trajectory from control module + Path predicted_path; + std::vector predicted_polys; + if (use_predicted_trajectory_) { + const auto predicted_traj_ptr = predicted_traj_ptr_; + constexpr double color_r = 0.0; + constexpr double color_g = 100.0 / 256.0; + constexpr double color_b = 0.0; + constexpr double color_a = 0.999; + const auto current_time = predicted_traj_ptr->header.stamp; + generateEgoPath(*predicted_traj_ptr, predicted_path, predicted_polys); + addMarker( + current_time, predicted_path, predicted_polys, color_r, color_g, color_b, color_a, + "predicted_path", "predicted_polygons", debug_markers); + } + + // publish debug markers + debug_ego_path_publisher_->publish(debug_markers); + + return hasCollision(current_v, ego_path, ego_polys) || + hasCollision(current_v, predicted_path, predicted_polys); +} + +bool AEB::hasCollision( + const double current_v, const Path & ego_path, const std::vector & ego_polys) +{ + // check if the predicted path has valid number of points + if (ego_path.size() < 2 || ego_polys.empty()) { + return false; + } + + // step1. create object + std::vector objects; + createObjectData(ego_path, ego_polys, objects); + + // step2. calculate RSS + const auto current_p = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + const double & t = t_response_; + for (const auto & obj : objects) { + const double & obj_v = obj.velocity; + const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - + obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; + const double dist_ego_to_object = + motion_utils::calcSignedArcLength(ego_path, current_p, obj.position) - + vehicle_info_.max_longitudinal_offset_m; + if (dist_ego_to_object < rss_dist) { + // collision happens + return true; + } + } + + return false; +} + +void AEB::generateEgoPath( + const double curr_v, const double curr_w, Path & path, std::vector & polygons) +{ + double curr_x = 0.0; + double curr_y = 0.0; + double curr_yaw = 0.0; + geometry_msgs::msg::Pose ini_pose; + ini_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + ini_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + path.push_back(ini_pose); + + if (curr_v < 0.1) { + // if current velocity is too small, assume it stops at the same point + return; + } + + constexpr double epsilon = 1e-6; + const double & dt = prediction_time_interval_; + const double & horizon = prediction_time_horizon_; + for (double t = 0.0; t < horizon + epsilon; t += dt) { + curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; + curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; + curr_yaw = curr_yaw + curr_w * dt; + geometry_msgs::msg::Pose current_pose; + current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + continue; + } + path.push_back(current_pose); + } + + // If path is shorter than minimum path length + while (motion_utils::calcArcLength(path) < min_generated_path_length_) { + curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; + curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; + curr_yaw = curr_yaw + curr_w * dt; + geometry_msgs::msg::Pose current_pose; + current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + continue; + } + path.push_back(current_pose); + } + + // generate ego polygons + polygons.resize(path.size() - 1); + for (size_t i = 0; i < path.size() - 1; ++i) { + polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_); + } +} + +void AEB::generateEgoPath( + const Trajectory & predicted_traj, Path & path, + std::vector & polygons) +{ + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", predicted_traj.header.frame_id, predicted_traj.header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map"); + return; + } + + // create path + path.resize(predicted_traj.points.size()); + for (size_t i = 0; i < predicted_traj.points.size(); ++i) { + geometry_msgs::msg::Pose map_pose; + tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); + path.at(i) = map_pose; + } + + // create polygon + polygons.resize(path.size()); + for (size_t i = 0; i < path.size() - 1; ++i) { + polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_); + } +} + +void AEB::createObjectData( + const Path & ego_path, const std::vector & ego_polys, + std::vector & objects) +{ + PointCloud::Ptr obstacle_points_ptr(new PointCloud); + pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *obstacle_points_ptr); + for (const auto & point : obstacle_points_ptr->points) { + ObjectData obj; + obj.position = tier4_autoware_utils::createPoint(point.x, point.y, point.z); + obj.velocity = 0.0; + const Point2d obj_point(point.x, point.y); + const double lat_dist = motion_utils::calcLateralOffset(ego_path, obj.position); + if (lat_dist > 5.0) { + continue; + } + for (const auto & ego_poly : ego_polys) { + if (bg::within(obj_point, ego_poly)) { + objects.push_back(obj); + break; + } + } + } +} + +void AEB::addMarker( + const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, + const double color_r, const double color_g, const double color_b, const double color_a, + const std::string & path_ns, const std::string & poly_ns, MarkerArray & debug_markers) +{ + // transform to map + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "map", "base_link", current_time, rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map"); + return; + } + + auto path_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, path_ns, 0L, Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(0.2, 0.2, 0.2), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + path_marker.points.resize(path.size()); + for (size_t i = 0; i < path.size(); ++i) { + const auto & pose = path.at(i); + geometry_msgs::msg::Pose map_pose; + tf2::doTransform(pose, map_pose, transform_stamped); + path_marker.points.at(i) = map_pose.position; + } + debug_markers.markers.push_back(path_marker); + + auto polygon_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, poly_ns, 0, Marker::LINE_LIST, + tier4_autoware_utils::createMarkerScale(0.03, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + for (const auto & poly : polygons) { + for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { + const auto & boost_cp = poly.outer().at(dp_idx); + const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); + const auto curr_point = tier4_autoware_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); + const auto next_point = tier4_autoware_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); + + geometry_msgs::msg::Point map_curr_point; + geometry_msgs::msg::Point map_next_point; + tf2::doTransform(curr_point, map_curr_point, transform_stamped); + tf2::doTransform(next_point, map_next_point, transform_stamped); + polygon_marker.points.push_back(map_curr_point); + polygon_marker.points.push_back(map_next_point); + } + } + debug_markers.markers.push_back(polygon_marker); +} +} // namespace autoware::motion::control::autonomous_emergency_braking + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::autonomous_emergency_braking::AEB) diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 9605f53cde1d3..244e75210cec3 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -66,7 +66,6 @@ struct Param struct Input { - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose{}; nav_msgs::msg::Odometry::ConstSharedPtr current_odom{}; lanelet::LaneletMapPtr lanelet_map{}; LaneletRoute::ConstSharedPtr route{}; diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index 6d8f267384075..85fd73c63f544 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -61,7 +60,6 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber - tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; rclcpp::Subscription::SharedPtr sub_route_; @@ -69,7 +67,6 @@ class LaneDepartureCheckerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; // Data Buffer - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; nav_msgs::msg::Odometry::ConstSharedPtr current_odom_; lanelet::LaneletMapPtr lanelet_map_; lanelet::ConstLanelets shoulder_lanelets_; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 1d594180a638b..1a63fb3eb395b 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -100,7 +100,7 @@ Output LaneDepartureChecker::update(const Input & input) tier4_autoware_utils::StopWatch stop_watch; output.trajectory_deviation = calcTrajectoryDeviation( - *input.reference_trajectory, input.current_pose->pose, param_.ego_nearest_dist_threshold, + *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold, param_.ego_nearest_yaw_threshold); output.processing_time_map["calcTrajectoryDeviation"] = stop_watch.toc(true); diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index c60cf59de2407..a3508018e0869 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -183,9 +183,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o updater_.add("trajectory_deviation", this, &LaneDepartureCheckerNode::checkTrajectoryDeviation); - // Wait for first self pose - self_pose_listener_.waitForFirstPose(); - // Timer const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); timer_ = rclcpp::create_timer( @@ -221,11 +218,6 @@ void LaneDepartureCheckerNode::onPredictedTrajectory(const Trajectory::ConstShar bool LaneDepartureCheckerNode::isDataReady() { - if (!current_pose_) { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_pose..."); - return false; - } - if (!current_odom_) { RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_twist msg..."); return false; @@ -261,7 +253,7 @@ bool LaneDepartureCheckerNode::isDataTimeout() const auto now = this->now(); constexpr double th_pose_timeout = 1.0; - const auto pose_time_diff = rclcpp::Time(current_pose_->header.stamp) - now; + const auto pose_time_diff = rclcpp::Time(current_odom_->header.stamp) - now; if (pose_time_diff.seconds() > th_pose_timeout) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "pose is timeout..."); return true; @@ -293,8 +285,6 @@ void LaneDepartureCheckerNode::onTimer() tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("Total"); - current_pose_ = self_pose_listener_.getCurrentPose(); - if (!isDataReady()) { return; } @@ -337,7 +327,6 @@ void LaneDepartureCheckerNode::onTimer() processing_time_map["Node: getRouteLanelets"] = stop_watch.toc(true); input_.current_odom = current_odom_; - input_.current_pose = current_pose_; input_.lanelet_map = lanelet_map_; input_.route = route_; input_.route_lanelets = route_lanelets_; @@ -471,7 +460,7 @@ visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray visualization_msgs::msg::MarkerArray marker_array; - const auto base_link_z = current_pose_->pose.position.z; + const auto base_link_z = current_odom_->pose.pose.position.z; if (node_param_.visualize_lanelet) { // Route Lanelets diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index c18711d7a4ffa..323a23ab3e80a 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -15,8 +15,6 @@ #ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_ #define MPC_LATERAL_CONTROLLER__MPC_HPP_ -#include "geometry/common_2d.hpp" -#include "helper_functions/angle_utils.hpp" #include "mpc_lateral_controller/interpolate.hpp" #include "mpc_lateral_controller/lowpass_filter.hpp" #include "mpc_lateral_controller/mpc_trajectory.hpp" @@ -46,6 +44,12 @@ namespace autoware::motion::control::mpc_lateral_controller { +using autoware_auto_control_msgs::msg::AckermannLateralCommand; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_vehicle_msgs::msg::SteeringReport; +using geometry_msgs::msg::Pose; +using tier4_debug_msgs::msg::Float32MultiArrayStamped; + struct MPCParam { //!< @brief prediction horizon step @@ -112,7 +116,7 @@ struct MPCData { int nearest_idx; double nearest_time; - geometry_msgs::msg::Pose nearest_pose; + Pose nearest_pose; double steer; double predicted_steer; double lateral_err; @@ -147,15 +151,15 @@ class MPC //!< @brief vehicle model type for MPC std::string m_vehicle_model_type; //!< @brief vehicle model for MPC - std::shared_ptr m_vehicle_model_ptr; + std::shared_ptr m_vehicle_model_ptr; //!< @brief qp solver for MPC - std::shared_ptr m_qpsolver_ptr; + std::shared_ptr m_qpsolver_ptr; //!< @brief lowpass filter for steering command - mpc_lateral_controller::Butterworth2dFilter m_lpf_steering_cmd; + Butterworth2dFilter m_lpf_steering_cmd; //!< @brief lowpass filter for lateral error - mpc_lateral_controller::Butterworth2dFilter m_lpf_lateral_error; + Butterworth2dFilter m_lpf_lateral_error; //!< @brief lowpass filter for heading error - mpc_lateral_controller::Butterworth2dFilter m_lpf_yaw_error; + Butterworth2dFilter m_lpf_yaw_error; //!< @brief raw output computed two iterations ago double m_raw_steer_cmd_pprev = 0.0; @@ -170,7 +174,7 @@ class MPC //!< @brief shift is forward or not. bool m_is_forward_shift = true; //!< @brief buffer of sent command - std::vector m_ctrl_cmd_vec; + std::vector m_ctrl_cmd_vec; //!< @brief minimum prediction distance double m_min_prediction_length = 5.0; @@ -178,9 +182,8 @@ class MPC * @brief get variables for mpc calculation */ bool getData( - const mpc_lateral_controller::MPCTrajectory & traj, - const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, - const geometry_msgs::msg::Pose & current_pose, MPCData * data); + const MPCTrajectory & traj, const SteeringReport & current_steer, const Pose & current_pose, + MPCData * data); /** * @brief calculate predicted steering */ @@ -194,10 +197,7 @@ class MPC * @brief set the reference trajectory to follow */ void storeSteerCmd(const double steer); - /** - * @brief reset previous result of MPC - */ - void resetPrevResult(const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer); + /** * @brief set initial condition for mpc * @param [in] data mpc data @@ -210,14 +210,13 @@ class MPC * @param [out] x updated state at delayed_time */ bool updateStateForDelayCompensation( - const mpc_lateral_controller::MPCTrajectory & traj, const double & start_time, - Eigen::VectorXd * x); + const MPCTrajectory & traj, const double & start_time, Eigen::VectorXd * x); /** * @brief generate MPC matrix with trajectory and vehicle model * @param [in] reference_trajectory used for linearization around reference trajectory */ MPCMatrix generateMPCMatrix( - const mpc_lateral_controller::MPCTrajectory & reference_trajectory, const double prediction_dt); + const MPCTrajectory & reference_trajectory, const double prediction_dt); /** * @brief generate MPC matrix with trajectory and vehicle model * @param [in] mpc_matrix parameters matrix to use for optimization @@ -232,22 +231,19 @@ class MPC * @brief resample trajectory with mpc resampling time */ bool resampleMPCTrajectoryByTime( - const double start_time, const double prediction_dt, - const mpc_lateral_controller::MPCTrajectory & input, - mpc_lateral_controller::MPCTrajectory * output) const; + const double start_time, const double prediction_dt, const MPCTrajectory & input, + MPCTrajectory * output) const; /** * @brief apply velocity dynamics filter with v0 from closest index */ - mpc_lateral_controller::MPCTrajectory applyVelocityDynamicsFilter( - const mpc_lateral_controller::MPCTrajectory & trajectory, - const geometry_msgs::msg::Pose & current_pose, const double v0) const; + MPCTrajectory applyVelocityDynamicsFilter( + const MPCTrajectory & trajectory, const Pose & current_pose, const double v0) const; /** * @brief get prediction delta time of mpc. * If trajectory length is shorter than min_prediction length, adjust delta time. */ double getPredictionDeltaTime( - const double start_time, const mpc_lateral_controller::MPCTrajectory & input, - const geometry_msgs::msg::Pose & current_pose) const; + const double start_time, const MPCTrajectory & input, const Pose & current_pose) const; /** * @brief add weights related to lateral_jerk, steering_rate, steering_acc into R */ @@ -342,7 +338,7 @@ class MPC public: //!< @brief reference trajectory to be followed - mpc_lateral_controller::MPCTrajectory m_ref_traj; + MPCTrajectory m_ref_traj; //!< @brief MPC design parameter MPCParam m_param; //!< @brief mpc_output buffer for delay time compensation @@ -381,24 +377,28 @@ class MPC * @param [out] diagnostic diagnostic msg to be filled-out */ bool calculateMPC( - const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, - const double current_velocity, const geometry_msgs::msg::Pose & current_pose, - autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd, - autoware_auto_planning_msgs::msg::Trajectory & predicted_traj, - tier4_debug_msgs::msg::Float32MultiArrayStamped & diagnostic); + const SteeringReport & current_steer, const double current_velocity, const Pose & current_pose, + AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_traj, + Float32MultiArrayStamped & diagnostic); /** * @brief set the reference trajectory to follow */ void setReferenceTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg, - const double traj_resample_dist, const bool enable_path_smoothing, - const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj, - const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control); + const Trajectory & trajectory_msg, const double traj_resample_dist, + const bool enable_path_smoothing, const int path_filter_moving_ave_num, + const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer, + const bool extend_trajectory_for_end_yaw_control); + + /** + * @brief reset previous result of MPC + */ + void resetPrevResult(const SteeringReport & current_steer); + /** * @brief set the vehicle model of this MPC */ inline void setVehicleModel( - std::shared_ptr vehicle_model_ptr, + std::shared_ptr vehicle_model_ptr, const std::string & vehicle_model_type) { m_vehicle_model_ptr = vehicle_model_ptr; @@ -407,7 +407,7 @@ class MPC /** * @brief set the QP solver of this MPC */ - inline void setQPSolver(std::shared_ptr qpsolver_ptr) + inline void setQPSolver(std::shared_ptr qpsolver_ptr) { m_qpsolver_ptr = qpsolver_ptr; } diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index 9ab555635a8a2..17d29d1542cde 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -107,7 +107,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase std::deque m_trajectory_buffer; // MPC object - mpc_lateral_controller::MPC m_mpc; + MPC m_mpc; //!< @brief measured kinematic state nav_msgs::msg::Odometry m_current_kinematic_state; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index d9fce24f4185f..b1b67e03f2ab8 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -16,8 +16,6 @@ #define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ #include "eigen3/Eigen/Core" -#include "geometry/common_2d.hpp" -#include "helper_functions/angle_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/control/mpc_lateral_controller/package.xml b/control/mpc_lateral_controller/package.xml index c805abe341bef..04a80ef9d9080 100644 --- a/control/mpc_lateral_controller/package.xml +++ b/control/mpc_lateral_controller/package.xml @@ -19,7 +19,6 @@ autoware_cmake autoware_auto_control_msgs - autoware_auto_geometry autoware_auto_planning_msgs autoware_auto_vehicle_msgs diagnostic_msgs diff --git a/control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp b/control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp deleted file mode 100644 index 70c610433fa06..0000000000000 --- a/control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2018-2021 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 "mpc_lateral_controller/longitudinal_controller_utils.hpp" - -#include "tf2/LinearMath/Matrix3x3.h" -#include "tf2/LinearMath/Quaternion.h" - -#include // NOLINT - -#ifdef ROS_DISTRO_GALACTIC -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#else -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#endif - -#include -#include - -namespace autoware::motion::control::mpc_lateral_controller -{ -namespace longitudinal_utils -{ - -bool isValidTrajectory(const Trajectory & traj) -{ - for (const auto & p : traj.points) { - if ( - !isfinite(p.pose.position.x) || !isfinite(p.pose.position.y) || - !isfinite(p.pose.position.z) || !isfinite(p.pose.orientation.w) || - !isfinite(p.pose.orientation.x) || !isfinite(p.pose.orientation.y) || - !isfinite(p.pose.orientation.z) || !isfinite(p.longitudinal_velocity_mps) || - !isfinite(p.lateral_velocity_mps) || !isfinite(p.acceleration_mps2) || - !isfinite(p.heading_rate_rps)) { - return false; - } - } - - // when trajectory is empty - if (traj.points.empty()) { - return false; - } - - return true; -} - -double calcStopDistance( - const Pose & current_pose, const Trajectory & traj, const double max_dist, const double max_yaw) -{ - const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points); - - const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; - const size_t seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, current_pose, max_dist, max_yaw); - const double signed_length_on_traj = motion_utils::calcSignedArcLength( - traj.points, current_pose.position, seg_idx, traj.points.at(end_idx).pose.position, - std::min(end_idx, traj.points.size() - 2)); - - if (std::isnan(signed_length_on_traj)) { - return 0.0; - } - return signed_length_on_traj; -} - -double getPitchByPose(const Quaternion & quaternion_msg) -{ - double roll, pitch, yaw; - tf2::Quaternion quaternion; - tf2::fromMsg(quaternion_msg, quaternion); - tf2::Matrix3x3{quaternion}.getRPY(roll, pitch, yaw); - - return pitch; -} - -double getPitchByTraj( - const Trajectory & trajectory, const size_t nearest_idx, const double wheel_base) -{ - using autoware::common::geometry::distance_2d; - // cannot calculate pitch - if (trajectory.points.size() <= 1) { - return 0.0; - } - - for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = - distance_2d(trajectory.points.at(nearest_idx), trajectory.points.at(i)); - if (dist > wheel_base) { - // calculate pitch from trajectory between rear wheel (nearest) and front center (i) - return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i)); - } - } - - // close to goal - for (size_t i = trajectory.points.size() - 1; i > 0; --i) { - const double dist = distance_2d(trajectory.points.back(), trajectory.points.at(i)); - - if (dist > wheel_base) { - // calculate pitch from trajectory - // between wheelbase behind the end of trajectory (i) and the end of trajectory (back) - return calcElevationAngle(trajectory.points.at(i), trajectory.points.back()); - } - } - - // calculate pitch from trajectory between the beginning and end of trajectory - return calcElevationAngle(trajectory.points.at(0), trajectory.points.back()); -} - -double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to) -{ - const double dx = p_from.pose.position.x - p_to.pose.position.x; - const double dy = p_from.pose.position.y - p_to.pose.position.y; - const double dz = p_from.pose.position.z - p_to.pose.position.z; - - const double dxy = std::max(std::hypot(dx, dy), std::numeric_limits::epsilon()); - const double pitch = std::atan2(dz, dxy); - - return pitch; -} - -Pose calcPoseAfterTimeDelay( - const Pose & current_pose, const double delay_time, const double current_vel) -{ - // simple linear prediction - const double yaw = tf2::getYaw(current_pose.orientation); - const double running_distance = delay_time * current_vel; - const double dx = running_distance * std::cos(yaw); - const double dy = running_distance * std::sin(yaw); - - auto pred_pose = current_pose; - pred_pose.position.x += dx; - pred_pose.position.y += dy; - return pred_pose; -} - -double lerp(const double v_from, const double v_to, const double ratio) -{ - return v_from + (v_to - v_from) * ratio; -} - -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); - - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} - -double applyDiffLimitFilter( - const double input_val, const double prev_val, const double dt, const double max_val, - const double min_val) -{ - const double diff_raw = (input_val - prev_val) / dt; - const double diff = std::min(std::max(diff_raw, min_val), max_val); - const double filtered_val = prev_val + diff * dt; - return filtered_val; -} - -double applyDiffLimitFilter( - const double input_val, const double prev_val, const double dt, const double lim_val) -{ - const double max_val = std::fabs(lim_val); - const double min_val = -max_val; - return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val); -} -} // namespace longitudinal_utils -} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index f5da49b1d5a82..107bca1274e48 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -32,14 +32,12 @@ namespace autoware::motion::control::mpc_lateral_controller using namespace std::literals::chrono_literals; bool MPC::calculateMPC( - const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, - const double current_velocity, const geometry_msgs::msg::Pose & current_pose, - autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd, - autoware_auto_planning_msgs::msg::Trajectory & predicted_traj, - tier4_debug_msgs::msg::Float32MultiArrayStamped & diagnostic) + const SteeringReport & current_steer, const double current_velocity, const Pose & current_pose, + AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_traj, + Float32MultiArrayStamped & diagnostic) { /* recalculate velocity from ego-velocity with dynamics */ - mpc_lateral_controller::MPCTrajectory reference_trajectory = + MPCTrajectory reference_trajectory = applyVelocityDynamicsFilter(m_ref_traj, current_pose, current_velocity); MPCData mpc_data; @@ -60,7 +58,7 @@ bool MPC::calculateMPC( } /* resample ref_traj with mpc sampling time */ - mpc_lateral_controller::MPCTrajectory mpc_resampled_ref_traj; + MPCTrajectory mpc_resampled_ref_traj; const double mpc_start_time = mpc_data.nearest_time + m_param.input_delay; const double prediction_dt = getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_pose); @@ -101,7 +99,7 @@ bool MPC::calculateMPC( /* calculate predicted trajectory */ Eigen::VectorXd Xex = mpc_matrix.Aex * x0 + mpc_matrix.Bex * Uex + mpc_matrix.Wex; - mpc_lateral_controller::MPCTrajectory mpc_predicted_traj; + MPCTrajectory mpc_predicted_traj; const auto & traj = mpc_resampled_ref_traj; for (size_t i = 0; i < static_cast(m_param.prediction_horizon); ++i) { const int DIM_X = m_vehicle_model_ptr->getDimX(); @@ -117,7 +115,7 @@ bool MPC::calculateMPC( const double relative_time = traj.relative_time[i]; mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time); } - mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj, predicted_traj); + MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj, predicted_traj); /* prepare diagnostic message */ const double nearest_k = reference_trajectory.k[static_cast(mpc_data.nearest_idx)]; @@ -171,18 +169,18 @@ bool MPC::calculateMPC( } void MPC::setReferenceTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg, - const double traj_resample_dist, const bool enable_path_smoothing, - const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj, - const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control) + const Trajectory & trajectory_msg, const double traj_resample_dist, + const bool enable_path_smoothing, const int path_filter_moving_ave_num, + const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer, + const bool extend_trajectory_for_end_yaw_control) { - mpc_lateral_controller::MPCTrajectory mpc_traj_raw; // received raw trajectory - mpc_lateral_controller::MPCTrajectory mpc_traj_resampled; // resampled trajectory - mpc_lateral_controller::MPCTrajectory mpc_traj_smoothed; // smooth filtered trajectory + MPCTrajectory mpc_traj_raw; // received raw trajectory + MPCTrajectory mpc_traj_resampled; // resampled trajectory + MPCTrajectory mpc_traj_smoothed; // smooth filtered trajectory /* resampling */ - mpc_lateral_controller::MPCUtils::convertToMPCTrajectory(trajectory_msg, mpc_traj_raw); - if (!mpc_lateral_controller::MPCUtils::resampleMPCTrajectoryByDistance( + MPCUtils::convertToMPCTrajectory(trajectory_msg, mpc_traj_raw); + if (!MPCUtils::resampleMPCTrajectoryByDistance( mpc_traj_raw, traj_resample_dist, &mpc_traj_resampled)) { RCLCPP_WARN(m_logger, "[setReferenceTrajectory] spline error when resampling by distance"); return; @@ -197,15 +195,12 @@ void MPC::setReferenceTrajectory( mpc_traj_smoothed = mpc_traj_resampled; const int mpc_traj_resampled_size = static_cast(mpc_traj_resampled.size()); if (enable_path_smoothing && mpc_traj_resampled_size > 2 * path_filter_moving_ave_num) { + using MoveAverageFilter::filt_vector; if ( - !mpc_lateral_controller::MoveAverageFilter::filt_vector( - path_filter_moving_ave_num, mpc_traj_smoothed.x) || - !mpc_lateral_controller::MoveAverageFilter::filt_vector( - path_filter_moving_ave_num, mpc_traj_smoothed.y) || - !mpc_lateral_controller::MoveAverageFilter::filt_vector( - path_filter_moving_ave_num, mpc_traj_smoothed.yaw) || - !mpc_lateral_controller::MoveAverageFilter::filt_vector( - path_filter_moving_ave_num, mpc_traj_smoothed.vx)) { + !filt_vector(path_filter_moving_ave_num, mpc_traj_smoothed.x) || + !filt_vector(path_filter_moving_ave_num, mpc_traj_smoothed.y) || + !filt_vector(path_filter_moving_ave_num, mpc_traj_smoothed.yaw) || + !filt_vector(path_filter_moving_ave_num, mpc_traj_smoothed.vx)) { RCLCPP_DEBUG(m_logger, "path callback: filtering error. stop filtering."); mpc_traj_smoothed = mpc_traj_resampled; } @@ -218,16 +213,16 @@ void MPC::setReferenceTrajectory( * well-defined considering the end point attitude angle, this feature is not necessary. */ if (extend_trajectory_for_end_yaw_control) { - mpc_lateral_controller::MPCUtils::extendTrajectoryInYawDirection( + MPCUtils::extendTrajectoryInYawDirection( mpc_traj_raw.yaw.back(), traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed); } /* calculate yaw angle */ - mpc_lateral_controller::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift); - mpc_lateral_controller::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw); + MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift); + MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw); /* calculate curvature */ - mpc_lateral_controller::MPCUtils::calcTrajectoryCurvature( + MPCUtils::calcTrajectoryCurvature( static_cast(curvature_smoothing_num_traj), static_cast(curvature_smoothing_num_ref_steer), &mpc_traj_smoothed); @@ -251,28 +246,24 @@ void MPC::setReferenceTrajectory( m_ref_traj = mpc_traj_smoothed; } -void MPC::resetPrevResult(const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer) +void MPC::resetPrevResult(const SteeringReport & current_steer) { - m_raw_steer_cmd_prev = current_steer.steering_tire_angle; - m_raw_steer_cmd_pprev = current_steer.steering_tire_angle; + // Consider limit. The prev value larger than limitaion brakes the optimization constraint and + // results in optimization failure. + const float steer_lim_f = static_cast(m_steer_lim); + m_raw_steer_cmd_prev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f); + m_raw_steer_cmd_pprev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f); } bool MPC::getData( - const mpc_lateral_controller::MPCTrajectory & traj, - const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, - const geometry_msgs::msg::Pose & current_pose, MPCData * data) + const MPCTrajectory & traj, const SteeringReport & current_steer, const Pose & current_pose, + MPCData * data) { static constexpr auto duration = 5000 /*ms*/; size_t nearest_idx; - if (!mpc_lateral_controller::MPCUtils::calcNearestPoseInterp( + if (!MPCUtils::calcNearestPoseInterp( traj, current_pose, &(data->nearest_pose), &(nearest_idx), &(data->nearest_time), ego_nearest_dist_threshold, ego_nearest_yaw_threshold, m_logger, *m_clock)) { - // reset previous MPC result - // Note: When a large deviation from the trajectory occurs, the optimization stops and - // the vehicle will return to the path by re-planning the trajectory or external operation. - // After the recovery, the previous value of the optimization may deviate greatly from - // the actual steer angle, and it may make the optimization result unstable. - resetPrevResult(current_steer); RCLCPP_WARN_SKIPFIRST_THROTTLE( m_logger, *m_clock, duration, "calculateMPC: error in calculating nearest pose. stop mpc."); return false; @@ -281,9 +272,8 @@ bool MPC::getData( /* get data */ data->nearest_idx = static_cast(nearest_idx); data->steer = static_cast(current_steer.steering_tire_angle); - data->lateral_err = - mpc_lateral_controller::MPCUtils::calcLateralError(current_pose, data->nearest_pose); - data->yaw_err = autoware::common::helper_functions::wrap_angle( + data->lateral_err = MPCUtils::calcLateralError(current_pose, data->nearest_pose); + data->yaw_err = tier4_autoware_utils::normalizeRadian( tf2::getYaw(current_pose.orientation) - tf2::getYaw(data->nearest_pose.orientation)); /* get predicted steer */ @@ -294,8 +284,8 @@ bool MPC::getData( *m_steer_prediction_prev = data->predicted_steer; /* check error limit */ - const double dist_err = autoware::common::geometry::distance_2d( - current_pose.position, data->nearest_pose.position); + const double dist_err = + tier4_autoware_utils::calcDistance2d(current_pose.position, data->nearest_pose.position); if (dist_err > m_admissible_position_error) { RCLCPP_WARN_SKIPFIRST_THROTTLE( m_logger, *m_clock, duration, "position error is over limit. error = %fm, limit: %fm", @@ -381,7 +371,7 @@ double MPC::getSteerCmdSum( void MPC::storeSteerCmd(const double steer) { const auto time_delayed = m_clock->now() + rclcpp::Duration::from_seconds(m_param.input_delay); - autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; + AckermannLateralCommand cmd; cmd.stamp = time_delayed; cmd.steering_tire_angle = static_cast(steer); @@ -400,15 +390,14 @@ void MPC::storeSteerCmd(const double steer) } bool MPC::resampleMPCTrajectoryByTime( - const double ts, const double prediction_dt, const mpc_lateral_controller::MPCTrajectory & input, - mpc_lateral_controller::MPCTrajectory * output) const + const double ts, const double prediction_dt, const MPCTrajectory & input, + MPCTrajectory * output) const { std::vector mpc_time_v; for (double i = 0; i < static_cast(m_param.prediction_horizon); ++i) { mpc_time_v.push_back(ts + i * prediction_dt); } - if (!mpc_lateral_controller::MPCUtils::linearInterpMPCTrajectory( - input.relative_time, input, mpc_time_v, output)) { + if (!MPCUtils::linearInterpMPCTrajectory(input.relative_time, input, mpc_time_v, output)) { RCLCPP_WARN_SKIPFIRST_THROTTLE( m_logger, *m_clock, 1000 /*ms*/, "calculateMPC: mpc resample error. stop mpc calculation. check code!"); @@ -447,8 +436,7 @@ Eigen::VectorXd MPC::getInitialState(const MPCData & data) } bool MPC::updateStateForDelayCompensation( - const mpc_lateral_controller::MPCTrajectory & traj, const double & start_time, - Eigen::VectorXd * x) + const MPCTrajectory & traj, const double & start_time, Eigen::VectorXd * x) { const int DIM_X = m_vehicle_model_ptr->getDimX(); const int DIM_U = m_vehicle_model_ptr->getDimU(); @@ -465,8 +453,8 @@ bool MPC::updateStateForDelayCompensation( double k = 0.0; double v = 0.0; if ( - !mpc_lateral_controller::linearInterpolate(traj.relative_time, traj.k, mpc_curr_time, k) || - !mpc_lateral_controller::linearInterpolate(traj.relative_time, traj.vx, mpc_curr_time, v)) { + !linearInterpolate(traj.relative_time, traj.k, mpc_curr_time, k) || + !linearInterpolate(traj.relative_time, traj.vx, mpc_curr_time, v)) { RCLCPP_ERROR( m_logger, "mpc resample error at delay compensation, stop mpc calculation. check code!"); return false; @@ -485,13 +473,11 @@ bool MPC::updateStateForDelayCompensation( return true; } -mpc_lateral_controller::MPCTrajectory MPC::applyVelocityDynamicsFilter( - const mpc_lateral_controller::MPCTrajectory & input, - const geometry_msgs::msg::Pose & current_pose, const double v0) const +MPCTrajectory MPC::applyVelocityDynamicsFilter( + const MPCTrajectory & input, const Pose & current_pose, const double v0) const { - autoware_auto_planning_msgs::msg::Trajectory autoware_traj; - autoware::motion::control::mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory( - input, autoware_traj); + Trajectory autoware_traj; + MPCUtils::convertToAutowareTrajectory(input, autoware_traj); if (autoware_traj.points.empty()) { return input; } @@ -502,9 +488,8 @@ mpc_lateral_controller::MPCTrajectory MPC::applyVelocityDynamicsFilter( const double acc_lim = m_param.acceleration_limit; const double tau = m_param.velocity_time_constant; - mpc_lateral_controller::MPCTrajectory output = input; - mpc_lateral_controller::MPCUtils::dynamicSmoothingVelocity( - static_cast(nearest_idx), v0, acc_lim, tau, output); + MPCTrajectory output = input; + MPCUtils::dynamicSmoothingVelocity(static_cast(nearest_idx), v0, acc_lim, tau, output); const double t_ext = 100.0; // extra time to prevent mpc calculation failure due to short time const double t_end = output.relative_time.back() + t_ext; const double v_end = 0.0; @@ -521,7 +506,7 @@ mpc_lateral_controller::MPCTrajectory MPC::applyVelocityDynamicsFilter( * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) */ MPCMatrix MPC::generateMPCMatrix( - const mpc_lateral_controller::MPCTrajectory & reference_trajectory, const double prediction_dt) + const MPCTrajectory & reference_trajectory, const double prediction_dt) { using Eigen::MatrixXd; @@ -783,13 +768,11 @@ void MPC::addSteerWeightF(const double prediction_dt, Eigen::MatrixXd * f_ptr) c } double MPC::getPredictionDeltaTime( - const double start_time, const mpc_lateral_controller::MPCTrajectory & input, - const geometry_msgs::msg::Pose & current_pose) const + const double start_time, const MPCTrajectory & input, const Pose & current_pose) const { // Calculate the time min_prediction_length ahead from current_pose - autoware_auto_planning_msgs::msg::Trajectory autoware_traj; - autoware::motion::control::mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory( - input, autoware_traj); + Trajectory autoware_traj; + MPCUtils::convertToAutowareTrajectory(input, autoware_traj); const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( autoware_traj.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); double sum_dist = 0; diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index a884b7f8b32e3..2a129d544587b 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -81,13 +81,13 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} /* vehicle model setup */ const std::string vehicle_model_type = node_->declare_parameter("vehicle_model_type"); - std::shared_ptr vehicle_model_ptr; + std::shared_ptr vehicle_model_ptr; if (vehicle_model_type == "kinematics") { - vehicle_model_ptr = std::make_shared( + vehicle_model_ptr = std::make_shared( wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau); } else if (vehicle_model_type == "kinematics_no_delay") { - vehicle_model_ptr = std::make_shared( - wheelbase, m_mpc.m_steer_lim); + vehicle_model_ptr = + std::make_shared(wheelbase, m_mpc.m_steer_lim); } else if (vehicle_model_type == "dynamics") { const double mass_fl = node_->declare_parameter("vehicle.mass_fl"); const double mass_fr = node_->declare_parameter("vehicle.mass_fr"); @@ -98,19 +98,19 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time // // NOLINT - vehicle_model_ptr = std::make_shared( - wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); + vehicle_model_ptr = + std::make_shared(wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); } else { RCLCPP_ERROR(node_->get_logger(), "vehicle_model_type is undefined"); } /* QP solver setup */ const std::string qp_solver_type = node_->declare_parameter("qp_solver_type"); - std::shared_ptr qpsolver_ptr; + std::shared_ptr qpsolver_ptr; if (qp_solver_type == "unconstraint_fast") { - qpsolver_ptr = std::make_shared(); + qpsolver_ptr = std::make_shared(); } else if (qp_solver_type == "osqp") { - qpsolver_ptr = std::make_shared(node_->get_logger()); + qpsolver_ptr = std::make_shared(node_->get_logger()); } else { RCLCPP_ERROR(node_->get_logger(), "qp_solver_type is undefined"); } @@ -204,6 +204,15 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_current_steering, m_current_kinematic_state.twist.twist.linear.x, m_current_kinematic_state.pose.pose, ctrl_cmd, predicted_traj, debug_values); + // reset previous MPC result + // Note: When a large deviation from the trajectory occurs, the optimization stops and + // the vehicle will return to the path by re-planning the trajectory or external operation. + // After the recovery, the previous value of the optimization may deviate greatly from + // the actual steer angle, and it may make the optimization result unstable. + if (!is_mpc_solved) { + m_mpc.resetPrevResult(m_current_steering); + } + if (enable_auto_steering_offset_removal_) { steering_offset_->updateOffset( m_current_kinematic_state.twist.twist, @@ -452,7 +461,7 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( result.reason = "success"; // strong exception safety wrt MPCParam - mpc_lateral_controller::MPCParam param = m_mpc.m_param; + MPCParam param = m_mpc.m_param; try { update_param(parameters, "mpc_prediction_horizon", param.prediction_horizon); update_param(parameters, "mpc_prediction_dt", param.prediction_dt); diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index f1af25167c240..8a3c613ac16ec 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -26,7 +26,8 @@ namespace autoware::motion::control::mpc_lateral_controller { namespace MPCUtils { -using autoware::common::geometry::distance_2d; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::normalizeRadian; geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double & yaw) { @@ -42,7 +43,7 @@ void convertEulerAngleToMonotonic(std::vector * a) } for (uint i = 1; i < a->size(); ++i) { const double da = a->at(i) - a->at(i - 1); - a->at(i) = a->at(i - 1) + autoware::common::helper_functions::wrap_angle(da); + a->at(i) = a->at(i - 1) + normalizeRadian(da); } } @@ -200,12 +201,12 @@ std::vector calcTrajectoryCurvature( p1.y = traj.y[prev_idx]; p2.y = traj.y[curr_idx]; p3.y = traj.y[next_idx]; - const double den = std::max( - distance_2d(p1, p2) * distance_2d(p2, p3) * distance_2d(p3, p1), - std::numeric_limits::epsilon()); - const double curvature = - 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den; - curvature_vec.at(curr_idx) = curvature; + try { + curvature_vec.at(curr_idx) = tier4_autoware_utils::calcCurvature(p1, p2, p3); + } catch (...) { + std::cerr << "[MPC] 2 points are too close to calculate curvature." << std::endl; + curvature_vec.at(curr_idx) = 0.0; + } } /* first and last curvature is copied from next value */ @@ -357,10 +358,9 @@ bool calcNearestPoseInterp( alpha * traj.x[*nearest_index] + (1 - alpha) * traj.x[second_nearest_index]; nearest_pose->position.y = alpha * traj.y[*nearest_index] + (1 - alpha) * traj.y[second_nearest_index]; - const double tmp_yaw_err = autoware::common::helper_functions::wrap_angle( - traj.yaw[*nearest_index] - traj.yaw[second_nearest_index]); - const double nearest_yaw = autoware::common::helper_functions::wrap_angle( - traj.yaw[second_nearest_index] + alpha * tmp_yaw_err); + const double tmp_yaw_err = + normalizeRadian(traj.yaw[*nearest_index] - traj.yaw[second_nearest_index]); + const double nearest_yaw = normalizeRadian(traj.yaw[second_nearest_index] + alpha * tmp_yaw_err); nearest_pose->orientation = getQuaternionFromYaw(nearest_yaw); *nearest_time = alpha * traj.relative_time[*nearest_index] + (1 - alpha) * traj.relative_time[second_nearest_index]; @@ -380,7 +380,7 @@ double calcStopDistance( for (int i = origin + 1; i < static_cast(current_trajectory.points.size()) - 1; ++i) { const auto & p0 = current_trajectory.points.at(static_cast(i)); const auto & p1 = current_trajectory.points.at(static_cast(i - 1)); - stop_dist += distance_2d(p0, p1); + stop_dist += calcDistance2d(p0, p1); if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) { break; } @@ -395,7 +395,7 @@ double calcStopDistance( if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) { break; } - stop_dist -= distance_2d(p0, p1); + stop_dist -= calcDistance2d(p0, p1); } return stop_dist; } @@ -408,8 +408,7 @@ void extendTrajectoryInYawDirection( // get terminal pose autoware_auto_planning_msgs::msg::Trajectory autoware_traj; - autoware::motion::control::mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory( - traj, autoware_traj); + MPCUtils::convertToAutowareTrajectory(traj, autoware_traj); auto extended_pose = autoware_traj.points.back().pose; constexpr double extend_dist = 10.0; diff --git a/control/mpc_lateral_controller/src/smooth_stop.cpp b/control/mpc_lateral_controller/src/smooth_stop.cpp deleted file mode 100644 index 564acd62c0d82..0000000000000 --- a/control/mpc_lateral_controller/src/smooth_stop.cpp +++ /dev/null @@ -1,161 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "mpc_lateral_controller/smooth_stop.hpp" - -#include // NOLINT - -#include -#include -#include -#include -#include - -namespace autoware::motion::control::mpc_lateral_controller -{ -void SmoothStop::init(const double pred_vel_in_target, const double pred_stop_dist) -{ - m_weak_acc_time = rclcpp::Clock{RCL_ROS_TIME}.now(); - - // when distance to stopline is near the car - if (pred_stop_dist < std::numeric_limits::epsilon()) { - m_strong_acc = m_params.min_strong_acc; - return; - } - - m_strong_acc = -std::pow(pred_vel_in_target, 2) / (2 * pred_stop_dist); - m_strong_acc = std::max(std::min(m_strong_acc, m_params.max_strong_acc), m_params.min_strong_acc); -} - -void SmoothStop::setParams( - double max_strong_acc, double min_strong_acc, double weak_acc, double weak_stop_acc, - double strong_stop_acc, double min_fast_vel, double min_running_vel, double min_running_acc, - double weak_stop_time, double weak_stop_dist, double strong_stop_dist) -{ - m_params.max_strong_acc = max_strong_acc; - m_params.min_strong_acc = min_strong_acc; - m_params.weak_acc = weak_acc; - m_params.weak_stop_acc = weak_stop_acc; - m_params.strong_stop_acc = strong_stop_acc; - - m_params.min_fast_vel = min_fast_vel; - m_params.min_running_vel = min_running_vel; - m_params.min_running_acc = min_running_acc; - m_params.weak_stop_time = weak_stop_time; - - m_params.weak_stop_dist = weak_stop_dist; - m_params.strong_stop_dist = strong_stop_dist; - - m_is_set_params = true; -} - -std::experimental::optional SmoothStop::calcTimeToStop( - const std::vector> & vel_hist) const -{ - if (!m_is_set_params) { - throw std::runtime_error("Trying to calculate uninitialized SmoothStop"); - } - - // return when vel_hist is empty - const double vel_hist_size = static_cast(vel_hist.size()); - if (vel_hist_size == 0.0) { - return {}; - } - - // calculate some variables for fitting - const rclcpp::Time current_ros_time = rclcpp::Clock{RCL_ROS_TIME}.now(); - double mean_t = 0.0; - double mean_v = 0.0; - double sum_tv = 0.0; - double sum_tt = 0.0; - for (const auto & vel : vel_hist) { - const double t = (vel.first - current_ros_time).seconds(); - const double v = vel.second; - - mean_t += t / vel_hist_size; - mean_v += v / vel_hist_size; - sum_tv += t * v; - sum_tt += t * t; - } - - // return when gradient a (of v = at + b) cannot be calculated. - // See the following calculation of a - if (std::abs(vel_hist_size * mean_t * mean_t - sum_tt) < std::numeric_limits::epsilon()) { - return {}; - } - - // calculate coefficients of linear function (v = at + b) - const double a = - (vel_hist_size * mean_t * mean_v - sum_tv) / (vel_hist_size * mean_t * mean_t - sum_tt); - const double b = mean_v - a * mean_t; - - // return when v is independent of time (v = b) - if (std::abs(a) < std::numeric_limits::epsilon()) { - return {}; - } - - // calculate time to stop by substituting v = 0 for v = at + b - const double time_to_stop = -b / a; - if (time_to_stop > 0) { - return time_to_stop; - } - - return {}; -} - -double SmoothStop::calculate( - const double stop_dist, const double current_vel, const double current_acc, - const std::vector> & vel_hist, const double delay_time) -{ - if (!m_is_set_params) { - throw std::runtime_error("Trying to calculate uninitialized SmoothStop"); - } - - // predict time to stop - const auto time_to_stop = calcTimeToStop(vel_hist); - - // calculate some flags - const bool is_fast_vel = std::abs(current_vel) > m_params.min_fast_vel; - const bool is_running = std::abs(current_vel) > m_params.min_running_vel || - std::abs(current_acc) > m_params.min_running_acc; - - // when exceeding the stopline (stop_dist is negative in these cases.) - if (stop_dist < m_params.strong_stop_dist) { // when exceeding the stopline much - return m_params.strong_stop_acc; - } else if (stop_dist < m_params.weak_stop_dist) { // when exceeding the stopline a bit - return m_params.weak_stop_acc; - } - - // when the car is running - if (is_running) { - // when the car will not stop in a certain time - if (time_to_stop && *time_to_stop > m_params.weak_stop_time + delay_time) { - return m_strong_acc; - } else if (!time_to_stop && is_fast_vel) { - return m_strong_acc; - } - - m_weak_acc_time = rclcpp::Clock{RCL_ROS_TIME}.now(); - return m_params.weak_acc; - } - - // for 0.5 seconds after the car stopped - if ((rclcpp::Clock{RCL_ROS_TIME}.now() - m_weak_acc_time).seconds() < 0.5) { - return m_params.weak_acc; - } - - // when the car is not running - return m_params.strong_stop_acc; -} -} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index afb02acfc2934..e918ee9836c9c 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -17,7 +17,6 @@ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" -#include "geometry/common_2d.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml index 8473ead09cce7..e8825eaab1778 100644 --- a/control/pid_longitudinal_controller/package.xml +++ b/control/pid_longitudinal_controller/package.xml @@ -20,7 +20,6 @@ autoware_adapi_v1_msgs autoware_auto_control_msgs - autoware_auto_geometry autoware_auto_planning_msgs autoware_auto_vehicle_msgs diagnostic_msgs diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index a6affe01ed034..8cbf02b90ce51 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -86,15 +86,14 @@ double getPitchByPose(const Quaternion & quaternion_msg) double getPitchByTraj( const Trajectory & trajectory, const size_t nearest_idx, const double wheel_base) { - using autoware::common::geometry::distance_2d; // cannot calculate pitch if (trajectory.points.size() <= 1) { return 0.0; } for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = - distance_2d(trajectory.points.at(nearest_idx), trajectory.points.at(i)); + const double dist = tier4_autoware_utils::calcDistance2d( + trajectory.points.at(nearest_idx), trajectory.points.at(i)); if (dist > wheel_base) { // calculate pitch from trajectory between rear wheel (nearest) and front center (i) return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i)); @@ -103,7 +102,8 @@ double getPitchByTraj( // close to goal for (size_t i = trajectory.points.size() - 1; i > 0; --i) { - const double dist = distance_2d(trajectory.points.back(), trajectory.points.at(i)); + const double dist = + tier4_autoware_utils::calcDistance2d(trajectory.points.back(), trajectory.points.at(i)); if (dist > wheel_base) { // calculate pitch from trajectory diff --git a/control/trajectory_follower_base/package.xml b/control/trajectory_follower_base/package.xml index 1a231bec3cc7a..b3e6a53228fbb 100644 --- a/control/trajectory_follower_base/package.xml +++ b/control/trajectory_follower_base/package.xml @@ -22,7 +22,6 @@ autoware_adapi_v1_msgs autoware_auto_control_msgs - autoware_auto_geometry autoware_auto_planning_msgs autoware_auto_vehicle_msgs diagnostic_msgs diff --git a/control/trajectory_follower_node/CMakeLists.txt b/control/trajectory_follower_node/CMakeLists.txt index a2316a6db30d2..4ba5bee16eb2d 100644 --- a/control/trajectory_follower_node/CMakeLists.txt +++ b/control/trajectory_follower_node/CMakeLists.txt @@ -10,7 +10,6 @@ ament_auto_add_library(${CONTROLLER_NODE} SHARED src/controller_node.cpp ) -target_compile_options(${CONTROLLER_NODE} PRIVATE -Wno-error=old-style-cast) rclcpp_components_register_node(${CONTROLLER_NODE} PLUGIN "autoware::motion::control::trajectory_follower_node::Controller" EXECUTABLE ${CONTROLLER_NODE}_exe @@ -23,9 +22,6 @@ ament_auto_add_library(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} SHARED src/simple_trajectory_follower.cpp ) -# TODO(simple_trajectory_follower) : RCLCPP_ERROR_THROTTLE() has built-in old-style casts. -# TODO(simple_trajectory_follower) : Temporary workaround until this is fixed. -target_compile_options(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} PRIVATE -Wno-error=old-style-cast) rclcpp_components_register_node(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} PLUGIN "simple_trajectory_follower::SimpleTrajectoryFollower" EXECUTABLE ${SIMPLE_TRAJECTORY_FOLLOWER_NODE}_exe diff --git a/evaluator/diagnostic_converter/CMakeLists.txt b/evaluator/diagnostic_converter/CMakeLists.txt new file mode 100644 index 0000000000000..3f47eb0386a1c --- /dev/null +++ b/evaluator/diagnostic_converter/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.16.3) # Ubuntu 20.04 default CMake version + +project(diagnostic_converter) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +find_package(pluginlib REQUIRED) + +ament_auto_find_build_dependencies() + + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/converter_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "diagnostic_converter::DiagnosticConverter" + EXECUTABLE ${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_${PROJECT_NAME} + test/test_converter_node.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME}_node + ) +endif() + +ament_auto_package() diff --git a/evaluator/diagnostic_converter/README.md b/evaluator/diagnostic_converter/README.md new file mode 100644 index 0000000000000..adc84c3967d25 --- /dev/null +++ b/evaluator/diagnostic_converter/README.md @@ -0,0 +1,53 @@ +# Planning Evaluator + +## Purpose + +This package provides a node to convert `diagnostic_msgs::msg::DiagnosticArray` messages +into `tier4_simulation_msgs::msg::UserDefinedValue` messages. + +## Inner-workings / Algorithms + +The node subscribes to all topics listed in the parameters and assumes they publish +`DiagnosticArray` messages. +Each time such message is received, +it is converted into as many `UserDefinedValue` messages as the number of `KeyValue` objects. +The format of the output topic is detailed in the _output_ section. + +## Inputs / Outputs + +### Inputs + +The node listens to `DiagnosticArray` messages on the topics specified in the parameters. + +### Outputs + +The node outputs `UserDefinedValue` messages that are converted from the received `DiagnosticArray`. + +The name of the output topics are generated from the corresponding input topic, the name of the diagnostic status, and the key of the diagnostic. +For example, we might listen to topic `/diagnostic_topic` and receive a `DiagnosticArray` with 2 status: + +- Status with `name: "x"`. + - Key: `a`. + - Key: `b`. +- Status with `name: "y"`. + - Key: `a`. + - Key: `c`. + +The resulting topics to publish the `UserDefinedValue` are as follows: + +- `/metrics_x_a`. +- `/metrics_x_b`. +- `/metrics_y_a`. +- `/metrics_y_c`. + +## Parameters + +| Name | Type | Description | +| :------------------ | :--------------- | :------------------------------------------------------------ | +| `diagnostic_topics` | list of `string` | list of DiagnosticArray topics to convert to UserDefinedValue | + +## Assumptions / Known limits + +Values in the `KeyValue` objects of a `DiagnosticStatus` are assumed to be of type `double`. + +## Future extensions / Unimplemented parts diff --git a/evaluator/diagnostic_converter/include/converter_node.hpp b/evaluator/diagnostic_converter/include/converter_node.hpp new file mode 100644 index 0000000000000..53c762dac0ffe --- /dev/null +++ b/evaluator/diagnostic_converter/include/converter_node.hpp @@ -0,0 +1,66 @@ +// Copyright 2023 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 CONVERTER_NODE_HPP_ +#define CONVERTER_NODE_HPP_ + +#include + +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "tier4_simulation_msgs/msg/user_defined_value.hpp" +#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" + +#include +#include +#include +#include + +namespace diagnostic_converter +{ +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using tier4_simulation_msgs::msg::UserDefinedValue; +using tier4_simulation_msgs::msg::UserDefinedValueType; + +/** + * @brief Node for converting from DiagnosticArray to UserDefinedValue + */ +class DiagnosticConverter : public rclcpp::Node +{ +public: + explicit DiagnosticConverter(const rclcpp::NodeOptions & node_options); + + /** + * @brief callback for DiagnosticArray msgs that publishes equivalent UserDefinedValue msgs + * @param [in] diag_msg received diagnostic message + */ + void onDiagnostic( + const DiagnosticArray::ConstSharedPtr diag_msg, const size_t diag_idx, + const std::string & topic); + + UserDefinedValue createUserDefinedValue(const KeyValue & key_value) const; + + rclcpp::Publisher::SharedPtr getPublisher( + const std::string & topic, const size_t pub_idx); + +private: + // ROS + std::vector::SharedPtr> diagnostics_sub_; + std::vector::SharedPtr>> + params_pub_; +}; +} // namespace diagnostic_converter + +#endif // CONVERTER_NODE_HPP_ diff --git a/evaluator/diagnostic_converter/package.xml b/evaluator/diagnostic_converter/package.xml new file mode 100644 index 0000000000000..c561cfcac6c65 --- /dev/null +++ b/evaluator/diagnostic_converter/package.xml @@ -0,0 +1,29 @@ + + + + diagnostic_converter + 0.5.6 + Node for converting diagnostic messages into ParameterDeclaration messages + Kyoichi Sugahara + Maxime CLEMENT + Takamasa Horibe + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + diagnostic_msgs + pluginlib + rclcpp + rclcpp_components + tier4_simulation_msgs + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/evaluator/diagnostic_converter/src/converter_node.cpp b/evaluator/diagnostic_converter/src/converter_node.cpp new file mode 100644 index 0000000000000..e61b19833d2fe --- /dev/null +++ b/evaluator/diagnostic_converter/src/converter_node.cpp @@ -0,0 +1,101 @@ +// Copyright 2023 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 "converter_node.hpp" + +#include + +namespace +{ +std::string removeInvalidTopicString(const std::string & input_string) +{ + std::regex pattern{R"([a-zA-Z0-9/_]+)"}; + + std::string result; + for (std::sregex_iterator itr(std::begin(input_string), std::end(input_string), pattern), end; + itr != end; ++itr) { + result += itr->str(); + } + return result; +} + +std::string removeUnitString(const std::string & input_string) +{ + for (size_t i = 0; i < input_string.size(); ++i) { + if (input_string.at(i) == '[') { + if (i != 0 && input_string.at(i - 1) == ' ') { + // Blank is also removed + return std::string{input_string.begin(), input_string.begin() + i - 1}; + } + return std::string{input_string.begin(), input_string.begin() + i}; + } + } + return input_string; +} +} // namespace + +namespace diagnostic_converter +{ +DiagnosticConverter::DiagnosticConverter(const rclcpp::NodeOptions & node_options) +: Node("diagnostic_converter", node_options) +{ + using std::placeholders::_1; + + size_t sub_counter = 0; + std::vector diagnostic_topics; + declare_parameter>("diagnostic_topics", std::vector()); + get_parameter>("diagnostic_topics", diagnostic_topics); + for (const std::string & diagnostic_topic : diagnostic_topics) { + // std::function required with multiple arguments https://answers.ros.org/question/289207 + const std::function fn = + std::bind(&DiagnosticConverter::onDiagnostic, this, _1, sub_counter++, diagnostic_topic); + diagnostics_sub_.push_back(create_subscription(diagnostic_topic, 1, fn)); + } + params_pub_.resize(diagnostics_sub_.size()); +} + +void DiagnosticConverter::onDiagnostic( + const DiagnosticArray::ConstSharedPtr diag_msg, const size_t diag_idx, + const std::string & base_topic) +{ + for (const auto & status : diag_msg->status) { + std::string status_topic = base_topic + (status.name.empty() ? "" : "_" + status.name); + for (const auto & key_value : status.values) { + const auto valid_topic_name = removeInvalidTopicString(status_topic + "_" + key_value.key); + getPublisher(valid_topic_name, diag_idx)->publish(createUserDefinedValue(key_value)); + } + } +} + +UserDefinedValue DiagnosticConverter::createUserDefinedValue(const KeyValue & key_value) const +{ + UserDefinedValue param_msg; + param_msg.type.data = UserDefinedValueType::DOUBLE; + param_msg.value = removeUnitString(key_value.value); + return param_msg; +} + +rclcpp::Publisher::SharedPtr DiagnosticConverter::getPublisher( + const std::string & topic, const size_t pub_idx) +{ + auto & pubs = params_pub_[pub_idx]; + if (pubs.count(topic) == 0) { + pubs[topic] = create_publisher(topic, 1); + } + return pubs.at(topic); +} +} // namespace diagnostic_converter + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_converter::DiagnosticConverter) diff --git a/evaluator/diagnostic_converter/test/test_converter_node.cpp b/evaluator/diagnostic_converter/test/test_converter_node.cpp new file mode 100644 index 0000000000000..47f42d018ea71 --- /dev/null +++ b/evaluator/diagnostic_converter/test/test_converter_node.cpp @@ -0,0 +1,127 @@ +// Copyright 2023 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 "converter_node.hpp" +#include "gtest/gtest.h" + +#include + +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "tier4_simulation_msgs/msg/user_defined_value.hpp" +#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" + +#include +#include +#include +#include + +using ConverterNode = diagnostic_converter::DiagnosticConverter; +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using tier4_simulation_msgs::msg::UserDefinedValue; + +void waitForMsg( + bool & flag, const rclcpp::Node::SharedPtr node1, const rclcpp::Node::SharedPtr node2) +{ + while (!flag) { + rclcpp::spin_some(node1); + rclcpp::spin_some(node2); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + flag = false; +} + +std::function generateCallback( + bool & flag, UserDefinedValue & msg) +{ + return [&](UserDefinedValue::ConstSharedPtr received_msg) { + flag = true; + msg = *received_msg; + }; +} + +TEST(ConverterNode, ConvertDiagnostics) +{ + const std::vector input_topics = {"/test1/diag", "/test2/diag"}; + + rclcpp::init(0, nullptr); + rclcpp::Node::SharedPtr dummy_node = std::make_shared("converter_test_node"); + + rclcpp::NodeOptions options; + options.append_parameter_override("diagnostic_topics", rclcpp::ParameterValue(input_topics)); + auto node = std::make_shared(options); + + { // Simple case with 1 resulting UserDefinedValue + bool msg_received = false; + UserDefinedValue param; + // DiagnosticArray publishers + const auto diag_pub = dummy_node->create_publisher(input_topics[0], 1); + // UserDefinedValue subscribers + const auto param_sub_a = dummy_node->create_subscription( + input_topics[0] + "_a", 1, generateCallback(msg_received, param)); + DiagnosticArray diag; + DiagnosticStatus status; + status.name = ""; + KeyValue key_value = KeyValue().set__key("a").set__value("1"); + status.values.push_back(key_value); + diag.status.push_back(status); + diag_pub->publish(diag); + waitForMsg(msg_received, node, dummy_node); + EXPECT_EQ(param.value, "1"); + } + { // Case with multiple UserDefinedValue converted from one DiagnosticArray + bool msg_received_xa = false; + bool msg_received_xb = false; + bool msg_received_ya = false; + bool msg_received_yb = false; + UserDefinedValue param_xa; + UserDefinedValue param_xb; + UserDefinedValue param_ya; + UserDefinedValue param_yb; + // DiagnosticArray publishers + const auto diag_pub = dummy_node->create_publisher(input_topics[1], 1); + // UserDefinedValue subscribers + const auto param_sub_xa = dummy_node->create_subscription( + input_topics[1] + "_x_a", 1, generateCallback(msg_received_xa, param_xa)); + const auto param_sub_xb = dummy_node->create_subscription( + input_topics[1] + "_x_b", 1, generateCallback(msg_received_xb, param_xb)); + const auto param_sub_ya = dummy_node->create_subscription( + input_topics[1] + "_y_a", 1, generateCallback(msg_received_ya, param_ya)); + const auto param_sub_yb = dummy_node->create_subscription( + input_topics[1] + "_y_b", 1, generateCallback(msg_received_yb, param_yb)); + DiagnosticArray diag; + DiagnosticStatus status_x; + status_x.name = "x"; + status_x.values.push_back(KeyValue().set__key("a").set__value("1")); + status_x.values.push_back(KeyValue().set__key("b").set__value("10")); + diag.status.push_back(status_x); + DiagnosticStatus status_y; + status_y.name = "y"; + status_y.values.push_back(KeyValue().set__key("a").set__value("9")); + status_y.values.push_back(KeyValue().set__key("b").set__value("6")); + diag.status.push_back(status_y); + diag_pub->publish(diag); + waitForMsg(msg_received_xa, node, dummy_node); + EXPECT_EQ(param_xa.value, "1"); + waitForMsg(msg_received_xb, node, dummy_node); + EXPECT_EQ(param_xb.value, "10"); + waitForMsg(msg_received_ya, node, dummy_node); + EXPECT_EQ(param_ya.value, "9"); + waitForMsg(msg_received_yb, node, dummy_node); + EXPECT_EQ(param_yb.value, "6"); + } + + rclcpp::shutdown(); +} diff --git a/planning/planning_evaluator/CMakeLists.txt b/evaluator/planning_evaluator/CMakeLists.txt similarity index 96% rename from planning/planning_evaluator/CMakeLists.txt rename to evaluator/planning_evaluator/CMakeLists.txt index 6865f8eef7005..4b643b3c85e44 100644 --- a/planning/planning_evaluator/CMakeLists.txt +++ b/evaluator/planning_evaluator/CMakeLists.txt @@ -4,6 +4,8 @@ project(planning_evaluator) find_package(autoware_cmake REQUIRED) autoware_package() +find_package(pluginlib REQUIRED) + ament_auto_add_library(${PROJECT_NAME}_node SHARED src/metrics_calculator.cpp src/${PROJECT_NAME}_node.cpp diff --git a/planning/planning_evaluator/README.md b/evaluator/planning_evaluator/README.md similarity index 100% rename from planning/planning_evaluator/README.md rename to evaluator/planning_evaluator/README.md diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp similarity index 68% rename from planning/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp index e71b887ff3a10..04a5b758d62e1 100644 --- a/planning/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp @@ -26,6 +26,8 @@ namespace metrics { using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; /** * @brief calculate lateral deviation of the given trajectory from the reference trajectory @@ -51,6 +53,30 @@ Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj); */ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj); +/** + * @brief calculate longitudinal deviation of the given ego pose from the modified goal pose + * @param [in] base_pose base pose + * @param [in] target_point target point + * @return calculated statistics + */ +Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point); + +/** + * @brief calculate lateral deviation of the given ego pose from the modified goal pose + * @param [in] base_pose base pose + * @param [in] target_point target point + * @return calculated statistics + */ +Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point); + +/** + * @brief calculate yaw deviation of the given ego pose from the modified goal pose + * @param [in] base_pose base pose + * @param [in] target_pose target pose + * @return calculated statistics + */ +Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose); + } // namespace metrics } // namespace planning_diagnostics diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/metric.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp similarity index 81% rename from planning/planning_evaluator/include/planning_evaluator/metrics/metric.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp index 16811f91b32b1..e1ec69dbaef5c 100644 --- a/planning/planning_evaluator/include/planning_evaluator/metrics/metric.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp @@ -41,6 +41,9 @@ enum class Metric { stability_frechet, obstacle_distance, obstacle_ttc, + modified_goal_longitudinal_deviation, + modified_goal_lateral_deviation, + modified_goal_yaw_deviation, SIZE, }; @@ -63,7 +66,10 @@ static const std::unordered_map str_to_metric = { {"stability_frechet", Metric::stability_frechet}, {"obstacle_distance", Metric::obstacle_distance}, {"obstacle_ttc", Metric::obstacle_ttc}, -}; + {"modified_goal_longitudinal_deviation", Metric::modified_goal_longitudinal_deviation}, + {"modified_goal_lateral_deviation", Metric::modified_goal_lateral_deviation}, + {"modified_goal_yaw_deviation", Metric::modified_goal_yaw_deviation}}; + static const std::unordered_map metric_to_str = { {Metric::curvature, "curvature"}, {Metric::point_interval, "point_interval"}, @@ -79,7 +85,10 @@ static const std::unordered_map metric_to_str = { {Metric::stability, "stability"}, {Metric::stability_frechet, "stability_frechet"}, {Metric::obstacle_distance, "obstacle_distance"}, - {Metric::obstacle_ttc, "obstacle_ttc"}}; + {Metric::obstacle_ttc, "obstacle_ttc"}, + {Metric::modified_goal_longitudinal_deviation, "modified_goal_longitudinal_deviation"}, + {Metric::modified_goal_lateral_deviation, "modified_goal_lateral_deviation"}, + {Metric::modified_goal_yaw_deviation, "modified_goal_yaw_deviation"}}; // Metrics descriptions static const std::unordered_map metric_descriptions = { @@ -97,7 +106,10 @@ static const std::unordered_map metric_descriptions = { {Metric::stability, "Stability[m]"}, {Metric::stability_frechet, "StabilityFrechet[m]"}, {Metric::obstacle_distance, "Obstacle_distance[m]"}, - {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"}}; + {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"}, + {Metric::modified_goal_longitudinal_deviation, "Modified_goal_longitudinal_deviation[m]"}, + {Metric::modified_goal_lateral_deviation, "Modified_goal_lateral_deviation[m]"}, + {Metric::modified_goal_yaw_deviation, "Modified_goal_yaw_deviation[rad]"}}; namespace details { diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp similarity index 85% rename from planning/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp index 0ed2af7b3862e..851678e55d755 100644 --- a/planning/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp @@ -14,7 +14,6 @@ #ifndef PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ #define PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ - #include "planning_evaluator/metrics/metric.hpp" #include "planning_evaluator/parameters.hpp" #include "planning_evaluator/stat.hpp" @@ -22,13 +21,19 @@ #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" +#include + namespace planning_diagnostics { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::PoseWithUuidStamped; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; class MetricsCalculator { @@ -42,7 +47,9 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return string describing the requested metric */ - Stat calculate(const Metric metric, const Trajectory & traj) const; + std::optional> calculate(const Metric metric, const Trajectory & traj) const; + std::optional> calculate( + const Metric metric, const Pose & base_pose, const Pose & target_pose) const; /** * @brief set the reference trajectory used to calculate the deviation metrics @@ -68,6 +75,12 @@ class MetricsCalculator */ void setEgoPose(const geometry_msgs::msg::Pose & pose); + /** + * @brief get the ego pose + * @return ego pose + */ + Pose getEgoPose(); + private: /** * @brief trim a trajectory from the current ego pose to some fixed time or distance @@ -86,6 +99,7 @@ class MetricsCalculator Trajectory previous_trajectory_lookahead_; PredictedObjects dynamic_objects_; geometry_msgs::msg::Pose ego_pose_; + PoseWithUuidStamped modified_goal_; }; // class MetricsCalculator } // namespace planning_diagnostics diff --git a/planning/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/parameters.hpp b/evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/parameters.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp diff --git a/planning/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp similarity index 79% rename from planning/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp index e9fd82c85cf7b..888eea855295c 100644 --- a/planning/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp @@ -24,7 +24,9 @@ #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "nav_msgs/msg/odometry.hpp" #include #include @@ -37,8 +39,10 @@ namespace planning_diagnostics using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::PoseWithUuidStamped; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; +using nav_msgs::msg::Odometry; /** * @brief Node for planning evaluation @@ -49,6 +53,12 @@ class PlanningEvaluatorNode : public rclcpp::Node explicit PlanningEvaluatorNode(const rclcpp::NodeOptions & node_options); ~PlanningEvaluatorNode(); + /** + * @brief callback on receiving an odometry + * @param [in] odometry_msg received odometry message + */ + void onOdometry(const Odometry::ConstSharedPtr odometry_msg); + /** * @brief callback on receiving a trajectory * @param [in] traj_msg received trajectory message @@ -68,9 +78,10 @@ class PlanningEvaluatorNode : public rclcpp::Node void onObjects(const PredictedObjects::ConstSharedPtr objects_msg); /** - * @brief update the ego pose stored in the MetricsCalculator + * @brief callback on receiving a modified goal + * @param [in] modified_goal_msg received modified goal message */ - void updateCalculatorEgoPose(const std::string & target_frame); + void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg); /** * @brief publish the given metric statistic @@ -80,11 +91,15 @@ class PlanningEvaluatorNode : public rclcpp::Node private: static bool isFinite(const TrajectoryPoint & p); + void publishModifiedGoalDeviationMetrics(); // ROS rclcpp::Subscription::SharedPtr traj_sub_; rclcpp::Subscription::SharedPtr ref_sub_; rclcpp::Subscription::SharedPtr objects_sub_; + rclcpp::Subscription::SharedPtr modified_goal_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr metrics_pub_; std::shared_ptr transform_listener_{nullptr}; std::unique_ptr tf_buffer_; @@ -99,6 +114,9 @@ class PlanningEvaluatorNode : public rclcpp::Node std::vector metrics_; std::deque stamps_; std::array>, static_cast(Metric::SIZE)> metric_stats_; + + Odometry::ConstSharedPtr ego_state_ptr_; + PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_; }; } // namespace planning_diagnostics diff --git a/planning/planning_evaluator/include/planning_evaluator/stat.hpp b/evaluator/planning_evaluator/include/planning_evaluator/stat.hpp similarity index 100% rename from planning/planning_evaluator/include/planning_evaluator/stat.hpp rename to evaluator/planning_evaluator/include/planning_evaluator/stat.hpp diff --git a/planning/planning_evaluator/launch/motion_evaluator.launch.xml b/evaluator/planning_evaluator/launch/motion_evaluator.launch.xml similarity index 100% rename from planning/planning_evaluator/launch/motion_evaluator.launch.xml rename to evaluator/planning_evaluator/launch/motion_evaluator.launch.xml diff --git a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml new file mode 100644 index 0000000000000..1b7510c2ced69 --- /dev/null +++ b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/planning/planning_evaluator/package.xml b/evaluator/planning_evaluator/package.xml similarity index 88% rename from planning/planning_evaluator/package.xml rename to evaluator/planning_evaluator/package.xml index b288d911b4c3c..ef2bca288c8b0 100644 --- a/planning/planning_evaluator/package.xml +++ b/evaluator/planning_evaluator/package.xml @@ -5,6 +5,7 @@ 0.1.0 ROS2 node for evaluating planners Maxime CLEMENT + Kyoichi Sugahara Apache License 2.0 Maxime CLEMENT @@ -15,11 +16,13 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_planning_msgs diagnostic_msgs eigen geometry_msgs motion_utils nav_msgs + pluginlib rclcpp rclcpp_components tf2 diff --git a/planning/planning_evaluator/param/planning_evaluator.defaults.yaml b/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml similarity index 88% rename from planning/planning_evaluator/param/planning_evaluator.defaults.yaml rename to evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml index bd47deedb282f..2e92a174ca79f 100644 --- a/planning/planning_evaluator/param/planning_evaluator.defaults.yaml +++ b/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml @@ -19,6 +19,9 @@ - stability_frechet - obstacle_distance - obstacle_ttc + - modified_goal_longitudinal_deviation + - modified_goal_lateral_deviation + - modified_goal_yaw_deviation trajectory: min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation diff --git a/planning/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp similarity index 79% rename from planning/planning_evaluator/src/metrics/deviation_metrics.cpp rename to evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp index d6d8aa661e5ea..5146fcd3ec0f2 100644 --- a/planning/planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp @@ -77,5 +77,25 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr return stat; } +Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) +{ + Stat stat; + stat.add(std::abs(tier4_autoware_utils::calcLongitudinalDeviation(base_pose, target_point))); + return stat; +} + +Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point) +{ + Stat stat; + stat.add(std::abs(tier4_autoware_utils::calcLateralDeviation(base_pose, target_point))); + return stat; +} + +Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose) +{ + Stat stat; + stat.add(std::abs(tier4_autoware_utils::calcYawDeviation(base_pose, target_pose))); + return stat; +} } // namespace metrics } // namespace planning_diagnostics diff --git a/planning/planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp similarity index 100% rename from planning/planning_evaluator/src/metrics/metrics_utils.cpp rename to evaluator/planning_evaluator/src/metrics/metrics_utils.cpp diff --git a/planning/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp similarity index 100% rename from planning/planning_evaluator/src/metrics/obstacle_metrics.cpp rename to evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp diff --git a/planning/planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp similarity index 100% rename from planning/planning_evaluator/src/metrics/stability_metrics.cpp rename to evaluator/planning_evaluator/src/metrics/stability_metrics.cpp diff --git a/planning/planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp similarity index 100% rename from planning/planning_evaluator/src/metrics/trajectory_metrics.cpp rename to evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp diff --git a/planning/planning_evaluator/src/metrics_calculator.cpp b/evaluator/planning_evaluator/src/metrics_calculator.cpp similarity index 84% rename from planning/planning_evaluator/src/metrics_calculator.cpp rename to evaluator/planning_evaluator/src/metrics_calculator.cpp index 802e1dde94fd1..adfdddd7d2c5e 100644 --- a/planning/planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/planning_evaluator/src/metrics_calculator.cpp @@ -23,9 +23,10 @@ namespace planning_diagnostics { -Stat MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const +std::optional> MetricsCalculator::calculate( + const Metric metric, const Trajectory & traj) const { - // Functions to calculate metrics + // Functions to calculate trajectory metrics switch (metric) { case Metric::curvature: return metrics::calcTrajectoryCurvature(traj); @@ -70,9 +71,23 @@ Stat MetricsCalculator::calculate(const Metric metric, const Trajectory case Metric::obstacle_ttc: return metrics::calcTimeToCollision(dynamic_objects_, traj, parameters.obstacle.dist_thr_m); default: - throw std::runtime_error( - "[MetricsCalculator][calculate()] unknown Metric " + - std::to_string(static_cast(metric))); + return {}; + } +} + +std::optional> MetricsCalculator::calculate( + const Metric metric, const Pose & base_pose, const Pose & target_pose) const +{ + // Functions to calculate pose metrics + switch (metric) { + case Metric::modified_goal_longitudinal_deviation: + return metrics::calcLongitudinalDeviation(base_pose, target_pose.position); + case Metric::modified_goal_lateral_deviation: + return metrics::calcLateralDeviation(base_pose, target_pose.position); + case Metric::modified_goal_yaw_deviation: + return metrics::calcYawDeviation(base_pose, target_pose); + default: + return {}; } } @@ -93,6 +108,8 @@ void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects) void MetricsCalculator::setEgoPose(const geometry_msgs::msg::Pose & pose) { ego_pose_ = pose; } +Pose MetricsCalculator::getEgoPose() { return ego_pose_; } + Trajectory MetricsCalculator::getLookaheadTrajectory( const Trajectory & traj, const double max_dist_m, const double max_time_s) const { diff --git a/planning/planning_evaluator/src/motion_evaluator_node.cpp b/evaluator/planning_evaluator/src/motion_evaluator_node.cpp similarity index 97% rename from planning/planning_evaluator/src/motion_evaluator_node.cpp rename to evaluator/planning_evaluator/src/motion_evaluator_node.cpp index 56b08fca81f9f..9a103890d53ac 100644 --- a/planning/planning_evaluator/src/motion_evaluator_node.cpp +++ b/evaluator/planning_evaluator/src/motion_evaluator_node.cpp @@ -55,7 +55,9 @@ MotionEvaluatorNode::~MotionEvaluatorNode() f << std::endl; for (Metric metric : metrics_) { const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_); - f /* << std::setw(3 * column_width) */ << stat << " "; + if (stat) { + f /* << std::setw(3 * column_width) */ << *stat << " "; + } } f.close(); } diff --git a/planning/planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/planning_evaluator/src/planning_evaluator_node.cpp similarity index 73% rename from planning/planning_evaluator/src/planning_evaluator_node.cpp rename to evaluator/planning_evaluator/src/planning_evaluator_node.cpp index cca9bbf7c62e3..039991d54a846 100644 --- a/planning/planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/planning_evaluator/src/planning_evaluator_node.cpp @@ -40,6 +40,13 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op objects_sub_ = create_subscription( "~/input/objects", 1, std::bind(&PlanningEvaluatorNode::onObjects, this, _1)); + + modified_goal_sub_ = create_subscription( + "~/input/modified_goal", 1, std::bind(&PlanningEvaluatorNode::onModifiedGoal, this, _1)); + + odom_sub_ = create_subscription( + "~/input/odometry", 1, std::bind(&PlanningEvaluatorNode::onOdometry, this, _1)); + tf_buffer_ = std::make_unique(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); @@ -98,24 +105,6 @@ PlanningEvaluatorNode::~PlanningEvaluatorNode() } } -void PlanningEvaluatorNode::updateCalculatorEgoPose(const std::string & target_frame) -{ - try { - const geometry_msgs::msg::TransformStamped transform = - tf_buffer_->lookupTransform(target_frame, ego_frame_str_, tf2::TimePointZero); - geometry_msgs::msg::Pose ego_pose; - ego_pose.position.x = transform.transform.translation.x; - ego_pose.position.y = transform.transform.translation.y; - ego_pose.position.z = transform.transform.translation.z; - ego_pose.orientation = transform.transform.rotation; - metrics_calculator_.setEgoPose(ego_pose); - } catch (tf2::TransformException & ex) { - RCLCPP_INFO( - this->get_logger(), "Cannot set ego pose: could not transform %s to %s: %s", - target_frame.c_str(), ego_frame_str_.c_str(), ex.what()); - } -} - DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( const Metric & metric, const Stat & metric_stat) const { @@ -137,18 +126,29 @@ DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_msg) { - auto start = now(); - stamps_.push_back(traj_msg->header.stamp); + if (!ego_state_ptr_) { + return; + } - updateCalculatorEgoPose(traj_msg->header.frame_id); + auto start = now(); + if (!output_file_str_.empty()) { + stamps_.push_back(traj_msg->header.stamp); + } DiagnosticArray metrics_msg; metrics_msg.header.stamp = now(); for (Metric metric : metrics_) { - const Stat metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg); - metric_stats_[static_cast(metric)].push_back(metric_stat); - if (metric_stat.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, metric_stat)); + const auto metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg); + if (!metric_stat) { + continue; + } + + if (!output_file_str_.empty()) { + metric_stats_[static_cast(metric)].push_back(*metric_stat); + } + + if (metric_stat->count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); } } if (!metrics_msg.status.empty()) { @@ -156,7 +156,52 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m } metrics_calculator_.setPreviousTrajectory(*traj_msg); auto runtime = (now() - start).seconds(); - RCLCPP_INFO(get_logger(), "Calculation time: %2.2f ms", runtime * 1e3); + RCLCPP_DEBUG(get_logger(), "Planning evaluation calculation time: %2.2f ms", runtime * 1e3); +} + +void PlanningEvaluatorNode::onModifiedGoal( + const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg) +{ + modified_goal_ptr_ = modified_goal_msg; + if (ego_state_ptr_) { + publishModifiedGoalDeviationMetrics(); + } +} + +void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg) +{ + ego_state_ptr_ = odometry_msg; + metrics_calculator_.setEgoPose(odometry_msg->pose.pose); + + if (modified_goal_ptr_) { + publishModifiedGoalDeviationMetrics(); + } +} + +void PlanningEvaluatorNode::publishModifiedGoalDeviationMetrics() +{ + auto start = now(); + + DiagnosticArray metrics_msg; + metrics_msg.header.stamp = now(); + for (Metric metric : metrics_) { + const auto metric_stat = metrics_calculator_.calculate( + Metric(metric), modified_goal_ptr_->pose, ego_state_ptr_->pose.pose); + if (!metric_stat) { + continue; + } + metric_stats_[static_cast(metric)].push_back(*metric_stat); + if (metric_stat->count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); + } + } + if (!metrics_msg.status.empty()) { + metrics_pub_->publish(metrics_msg); + } + auto runtime = (now() - start).seconds(); + RCLCPP_DEBUG( + get_logger(), "Planning evaluation modified goal deviation calculation time: %2.2f ms", + runtime * 1e3); } void PlanningEvaluatorNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr traj_msg) diff --git a/planning/planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp similarity index 81% rename from planning/planning_evaluator/test/test_planning_evaluator_node.cpp rename to evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp index 8e9bcdd734222..4f2ab014a79d6 100644 --- a/planning/planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp @@ -36,7 +36,11 @@ using EvalNode = planning_diagnostics::PlanningEvaluatorNode; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; using Objects = autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::PoseWithUuidStamped; using diagnostic_msgs::msg::DiagnosticArray; +using nav_msgs::msg::Odometry; + +constexpr double epsilon = 1e-6; class EvalTest : public ::testing::Test { @@ -70,6 +74,10 @@ class EvalTest : public ::testing::Test dummy_node, "/planning_evaluator/input/reference_trajectory", 1); objects_pub_ = rclcpp::create_publisher(dummy_node, "/planning_evaluator/input/objects", 1); + odom_pub_ = + rclcpp::create_publisher(dummy_node, "/planning_evaluator/input/odometry", 1); + modified_goal_pub_ = rclcpp::create_publisher( + dummy_node, "/planning_evaluator/input/modified_goal", 1); tf_broadcaster_ = std::make_unique(dummy_node); publishEgoPose(0.0, 0.0, 0.0); @@ -139,28 +147,51 @@ class EvalTest : public ::testing::Test return metric_value_; } - void publishEgoPose(const double x, const double y, const double yaw) + double publishModifiedGoalAndGetMetric(const double x, const double y, const double yaw) { - geometry_msgs::msg::TransformStamped t; + metric_updated_ = false; - // Read message content and assign it to - // corresponding tf variables - t.header.stamp = dummy_node->now(); - t.header.frame_id = "map"; - t.child_frame_id = "base_link"; + PoseWithUuidStamped goal; + goal.header.frame_id = "map"; + goal.header.stamp = dummy_node->now(); + goal.pose.position.x = x; + goal.pose.position.y = y; + goal.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + goal.pose.orientation.x = q.x(); + goal.pose.orientation.y = q.y(); + goal.pose.orientation.z = q.z(); + goal.pose.orientation.w = q.w(); + modified_goal_pub_->publish(goal); - t.transform.translation.x = x; - t.transform.translation.y = y; - t.transform.translation.z = 0.0; + while (!metric_updated_) { + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + return metric_value_; + } + void publishEgoPose(const double x, const double y, const double yaw) + { + Odometry odom; + odom.header.frame_id = "map"; + odom.header.stamp = dummy_node->now(); + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + odom.pose.pose.position.z = 0.0; tf2::Quaternion q; q.setRPY(0.0, 0.0, yaw); - t.transform.rotation.x = q.x(); - t.transform.rotation.y = q.y(); - t.transform.rotation.z = q.z(); - t.transform.rotation.w = q.w(); + odom.pose.pose.orientation.x = q.x(); + odom.pose.pose.orientation.y = q.y(); + odom.pose.pose.orientation.z = q.z(); + odom.pose.pose.orientation.w = q.w(); - tf_broadcaster_->sendTransform(t); + odom_pub_->publish(odom); + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); } // Latest metric value @@ -173,6 +204,8 @@ class EvalTest : public ::testing::Test rclcpp::Publisher::SharedPtr traj_pub_; rclcpp::Publisher::SharedPtr ref_traj_pub_; rclcpp::Publisher::SharedPtr objects_pub_; + rclcpp::Publisher::SharedPtr modified_goal_pub_; + rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Subscription::SharedPtr metric_sub_; // TF broadcaster std::unique_ptr tf_broadcaster_; @@ -407,3 +440,29 @@ TEST_F(EvalTest, TestObstacleTTC) t.points[1].pose.position.x = 1.0; EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 2.0); } + +TEST_F(EvalTest, TestModifiedGoalLongitudinalDeviation) +{ + setTargetMetric(planning_diagnostics::Metric::modified_goal_longitudinal_deviation); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, 0.0), 1.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, M_PI_2), 0.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, 0.0), 0.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, M_PI_2), 1.0, epsilon); +} + +TEST_F(EvalTest, TestModifiedGoalLateralDeviation) +{ + setTargetMetric(planning_diagnostics::Metric::modified_goal_lateral_deviation); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, 0.0), 0.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, M_PI_2), 1.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, 0.0), 1.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, M_PI_2), 0.0, epsilon); +} + +TEST_F(EvalTest, TestModifiedGoalYawDeviation) +{ + setTargetMetric(planning_diagnostics::Metric::modified_goal_yaw_deviation); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 0.0, M_PI_2), M_PI_2, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 1.0, -M_PI_2), M_PI_2, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 1.0, -M_PI_4), M_PI_4, epsilon); +} diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 593a79cd2b361..63f3523503219 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -59,6 +59,8 @@ def launch_setup(context, *args, **kwargs): LaunchConfiguration("obstacle_collision_checker_param_path").perform(context), "r" ) as f: obstacle_collision_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("aeb_param_path").perform(context), "r") as f: + aeb_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( package="trajectory_follower_node", @@ -126,6 +128,33 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # autonomous emergency braking + autonomous_emergency_braking = ComposableNode( + package="autonomous_emergency_braking", + plugin="autoware::motion::control::autonomous_emergency_braking::AEB", + name="autonomous_emergency_braking", + remappings=[ + ("~/input/pointcloud", "/perception/obstacle_segmentation/pointcloud"), + ("~/input/velocity", "/vehicle/status/velocity_status"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/odometry", "/localization/kinematic_state"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[ + aeb_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + autonomous_emergency_braking_loader = LoadComposableNodes( + condition=IfCondition(LaunchConfiguration("enable_autonomous_emergency_braking")), + composable_node_descriptions=[autonomous_emergency_braking], + target_container="/control/control_container", + ) + # vehicle cmd gate vehicle_cmd_gate_component = ComposableNode( package="vehicle_cmd_gate", @@ -170,6 +199,11 @@ def launch_setup(context, *args, **kwargs): parameters=[ vehicle_cmd_gate_param, vehicle_info_param, + { + "check_external_emergency_heartbeat": LaunchConfiguration( + "check_external_emergency_heartbeat" + ), + }, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -274,6 +308,7 @@ def launch_setup(context, *args, **kwargs): external_cmd_selector_loader, external_cmd_converter_loader, obstacle_collision_checker_loader, + autonomous_emergency_braking_loader, ] ) @@ -306,6 +341,9 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("shift_decider_param_path") add_launch_arg("obstacle_collision_checker_param_path") add_launch_arg("external_cmd_selector_param_path") + add_launch_arg("aeb_param_path") + add_launch_arg("enable_autonomous_emergency_braking") + add_launch_arg("check_external_emergency_heartbeat") # component add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 14fe9c55f7e5d..210e5d049727c 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -6,6 +6,9 @@ + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index 3c109769e13ba..59fd5174fa38c 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -2,9 +2,6 @@ - - - @@ -15,8 +12,7 @@ - - + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml index d820e06deb614..31a453c082522 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.xml +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -14,10 +14,8 @@ - - - - + + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 13102ea5f9b70..7c15e1be21be8 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -5,6 +5,9 @@ 0.1.0 The tier4_localization_launch package Yamato Ando + Koji Minoda + Kento Yabuuchi + Ryu Yamamoto Apache License 2.0 Yamato Ando diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index bd636b289ca93..d3735e5509a94 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -38,6 +38,12 @@ def __init__(self, context): self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] + self.timer_interval_ms = self.pointcloud_map_filter_param["timer_interval_ms"] + self.use_dynamic_map_loading = self.pointcloud_map_filter_param["use_dynamic_map_loading"] + self.map_update_distance_threshold = self.pointcloud_map_filter_param[ + "map_update_distance_threshold" + ] + self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"] def create_pipeline(self): if self.use_down_sample_filter: @@ -56,10 +62,17 @@ def create_normal_pipeline(self): ("input", LaunchConfiguration("input_topic")), ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), + ("map_loader_service", "/map/get_differential_pointcloud_map"), + ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), ], parameters=[ { "distance_threshold": self.distance_threshold, + "timer_interval_ms": self.timer_interval_ms, + "use_dynamic_map_loading": self.use_dynamic_map_loading, + "map_update_distance_threshold": self.map_update_distance_threshold, + "map_loader_radius": self.map_loader_radius, + "input_frame": "map", } ], extra_arguments=[ diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index c072d4deb3059..105bcb0e76e93 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -327,6 +327,7 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con "use_lane_filter": False, "use_inpaint": True, "inpaint_radius": 1.0, + "lane_margin": 2.0, "param_file_path": PathJoinSubstitution( [ LaunchConfiguration( diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py b/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py index d371fa43986ab..0a6b459d9f5f3 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py @@ -12,8 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition @@ -21,23 +23,14 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +import yaml -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) +def launch_setup(context, *args, **kwargs): + # load parameter files + param_file = LaunchConfiguration("param_file").perform(context) + with open(param_file, "r") as f: + laserscan_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"]["ros__parameters"] composable_nodes = [ ComposableNode( @@ -53,7 +46,9 @@ def add_launch_arg(name: str, default_value=None): ], parameters=[ { - "target_frame": "base_link", # Leave disabled to output scan in pointcloud frame + "target_frame": laserscan_based_occupancy_grid_map_node_params[ + "scan_origin_frame" + ], # Leave disabled to output scan in pointcloud frame "transform_tolerance": 0.01, "min_height": 0.0, "max_height": 2.0, @@ -86,14 +81,13 @@ def add_launch_arg(name: str, default_value=None): ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], parameters=[ + laserscan_based_occupancy_grid_map_node_params, { - "map_resolution": 0.5, - "use_height_filter": True, "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"), "input_obstacle_and_raw_pointcloud": LaunchConfiguration( "input_obstacle_and_raw_pointcloud" ), - } + }, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), @@ -115,6 +109,25 @@ def add_launch_arg(name: str, default_value=None): condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + return LaunchDescription( [ add_launch_arg("use_multithread", "false"), @@ -128,11 +141,15 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("output/stixel", "virtual_scan/stixel"), add_launch_arg("input_obstacle_pointcloud", "false"), add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), - add_launch_arg("use_pointcloud_container", "False"), + add_launch_arg( + "param_file", + get_package_share_directory("probabilistic_occupancy_grid_map") + + "/config/laserscan_based_occupancy_grid_map.param.yaml", + ), + add_launch_arg("use_pointcloud_container", "false"), add_launch_arg("container_name", "occupancy_grid_map_container"), set_container_executable, set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, ] + + [OpaqueFunction(function=launch_setup)] ) diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py b/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py index 2feefdfb1053d..e8a9e173cfe0a 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py @@ -14,6 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition @@ -21,23 +22,16 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +import yaml -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) +def launch_setup(context, *args, **kwargs): + # load parameter files + param_file = LaunchConfiguration("param_file").perform(context) + with open(param_file, "r") as f: + pointcloud_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"][ + "ros__parameters" + ] composable_nodes = [ ComposableNode( @@ -49,12 +43,7 @@ def add_launch_arg(name: str, default_value=None): ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], - parameters=[ - { - "map_resolution": 0.5, - "use_height_filter": True, - } - ], + parameters=[pointcloud_based_occupancy_grid_map_node_params], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), ] @@ -75,18 +64,37 @@ def add_launch_arg(name: str, default_value=None): condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + return LaunchDescription( [ - add_launch_arg("use_multithread", "False"), - add_launch_arg("use_intra_process", "True"), - add_launch_arg("use_pointcloud_container", "False"), + add_launch_arg("use_multithread", "false"), + add_launch_arg("use_intra_process", "true"), + add_launch_arg("use_pointcloud_container", "false"), add_launch_arg("container_name", "occupancy_grid_map_container"), add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), add_launch_arg("output", "occupancy_grid"), + add_launch_arg("param_file", "config/pointcloud_based_occupancy_grid_map.param.yaml"), set_container_executable, set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, ] + + [OpaqueFunction(function=launch_setup)] ) diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index ffc6f908983bb..b2a800c5b278d 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -76,6 +76,7 @@ + diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index f0e9545250e5e..a64427b70ec44 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -86,5 +86,10 @@ + + + + + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index aa1877be4f536..eb5fee271d662 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -2,10 +2,13 @@ + + + @@ -41,6 +44,7 @@ + @@ -97,13 +101,18 @@ - - - - + + + + + + + + + diff --git a/launch/tier4_simulator_launch/package.xml b/launch/tier4_simulator_launch/package.xml index 7964e7c499a11..8094f95d78b15 100644 --- a/launch/tier4_simulator_launch/package.xml +++ b/launch/tier4_simulator_launch/package.xml @@ -5,6 +5,10 @@ 0.1.0 The tier4_simulator_launch package Keisuke Shima + Takayuki Murooka + Takamasa Horibe + Tomoya Kimura + Taiki Tanaka Apache License 2.0 ament_cmake_auto diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 8e674b0435528..016d613cfa72d 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -21,7 +21,7 @@ - + @@ -66,6 +66,7 @@ + diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 6ae8cefc9088c..bd9d22523ea06 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -75,5 +75,6 @@ endif() ament_auto_package( INSTALL_TO_SHARE + config launch ) diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml new file mode 100644 index 0000000000000..322325d239004 --- /dev/null +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -0,0 +1,23 @@ +/**: + ros__parameters: + show_debug_info: false + enable_yaw_bias_estimation: True + predict_frequency: 50.0 + tf_rate: 50.0 + extend_state_step: 50 + + # for Pose measurement + pose_additional_delay: 0.0 + pose_measure_uncertainty_time: 0.01 + pose_smoothing_steps: 5 + pose_gate_dist: 10000.0 + + # for twist measurement + twist_additional_delay: 0.0 + twist_smoothing_steps: 2 + twist_gate_dist: 10000.0 + + # for process model + proc_stddev_yaw_c: 0.005 + proc_stddev_vx_c: 10.0 + proc_stddev_wz_c: 5.0 diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml index c716d58bfc00c..ee0504293602a 100644 --- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -1,30 +1,12 @@ - - - - - + - + - - - - - - - - - - - - - - @@ -45,26 +27,6 @@ - - - - - - - - - - - - - - - - - - - - @@ -72,5 +34,7 @@ + + diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index e94f86c8d6c7e..c1486b71aae9c 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -1,24 +1,6 @@ /**: ros__parameters: - - # from gnss - gnss_particle_covariance: - [ - 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, - ] - - # output - output_pose_covariance: - [ - 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, - ] + gnss_enabled: true + ndt_enabled: true + ekf_enabled: true + stop_check_enabled: true diff --git a/localization/pose_initializer/config/pose_initializer_common.param.yaml b/localization/pose_initializer/config/pose_initializer_common.param.yaml new file mode 100644 index 0000000000000..a05cc7c35cb1a --- /dev/null +++ b/localization/pose_initializer/config/pose_initializer_common.param.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + gnss_pose_timeout: 3.0 # [sec] + stop_check_duration: 3.0 # [sec] + + # from gnss + gnss_particle_covariance: + [ + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, + ] + + # output + output_pose_covariance: + [ + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, + ] diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 9483ff47bc093..86b20833ec4df 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -1,24 +1,16 @@ + - - - - + - - - - - - - - + + diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 4c3b418239a5c..b8abb4da84e4a 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -65,6 +65,9 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() add_testcase(test/test_sphere_box_overlap.cpp) + add_testcase(test/test_pointcloud_map_loader_module.cpp) + add_testcase(test/test_partial_map_loader_module.cpp) + add_testcase(test/test_differential_map_loader_module.cpp) endif() install(PROGRAMS diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index 1c67d25f08f7a..b4aa930831a04 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -14,10 +14,9 @@ #include "pointcloud_map_loader_module.hpp" +#include "utils.hpp" + #include -#include -#include -#include #include #include diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index aebc5a17454ab..6a87643b57bff 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -21,6 +21,10 @@ #include +#include +#include +#include + #include #include diff --git a/map/map_loader/test/test_differential_map_loader_module.cpp b/map/map_loader/test/test_differential_map_loader_module.cpp new file mode 100644 index 0000000000000..5742dd851b078 --- /dev/null +++ b/map/map_loader/test/test_differential_map_loader_module.cpp @@ -0,0 +1,98 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/pointcloud_map_loader/differential_map_loader_module.hpp" + +#include + +#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" + +#include +#include + +#include + +using autoware_map_msgs::srv::GetDifferentialPointCloudMap; + +class TestDifferentialMapLoaderModule : public ::testing::Test +{ +protected: + void SetUp() override + { + // Initialize ROS node + rclcpp::init(0, nullptr); + node_ = std::make_shared("test_differential_map_loader_module"); + + // Generate a sample dummy pointcloud and save it to a file + pcl::PointCloud dummy_cloud; + dummy_cloud.width = 3; + dummy_cloud.height = 1; + dummy_cloud.points.resize(dummy_cloud.width * dummy_cloud.height); + dummy_cloud.points[0] = pcl::PointXYZ(-1.0, -1.0, -1.0); + dummy_cloud.points[1] = pcl::PointXYZ(0.0, 0.0, 0.0); + dummy_cloud.points[2] = pcl::PointXYZ(1.0, 1.0, 1.0); + pcl::io::savePCDFileASCII("/tmp/dummy.pcd", dummy_cloud); + + // Generate a sample dummy pointcloud metadata dictionary + std::map dummy_metadata_dict; + PCDFileMetadata dummy_metadata; + dummy_metadata.min = pcl::PointXYZ(-1.0, -1.0, -1.0); + dummy_metadata.max = pcl::PointXYZ(1.0, 1.0, 1.0); + dummy_metadata_dict["/tmp/dummy.pcd"] = dummy_metadata; + + // Initialize the DifferentialMapLoaderModule with the dummy metadata dictionary + module_ = std::make_shared(node_.get(), dummy_metadata_dict); + + // Create a client for the GetDifferentialPointCloudMap service + client_ = + node_->create_client("service/get_differential_pcd_map"); + } + + void TearDown() override { rclcpp::shutdown(); } + + rclcpp::Node::SharedPtr node_; + std::shared_ptr module_; + rclcpp::Client::SharedPtr client_; +}; + +TEST_F(TestDifferentialMapLoaderModule, LoadDifferentialPCDFiles) +{ + // Wait for the GetDifferentialPointCloudMap service to be available + ASSERT_TRUE(client_->wait_for_service(std::chrono::seconds(3))); + + // Prepare a request for the service + auto request = std::make_shared(); + request->area.center.x = 0; + request->area.center.y = 0; + request->area.center.z = 0; + request->area.radius = 2; + request->cached_ids.clear(); + + // Call the service + auto result_future = client_->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result_future), rclcpp::FutureReturnCode::SUCCESS); + + // Check the result + auto result = result_future.get(); + ASSERT_EQ(static_cast(result->new_pointcloud_with_ids.size()), 1); + EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd"); + EXPECT_EQ(static_cast(result->ids_to_remove.size()), 0); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/map/map_loader/test/test_partial_map_loader_module.cpp b/map/map_loader/test/test_partial_map_loader_module.cpp new file mode 100644 index 0000000000000..5aa3a8910a8cb --- /dev/null +++ b/map/map_loader/test/test_partial_map_loader_module.cpp @@ -0,0 +1,94 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/pointcloud_map_loader/partial_map_loader_module.hpp" + +#include + +#include "autoware_map_msgs/srv/get_partial_point_cloud_map.hpp" + +#include + +#include + +using autoware_map_msgs::srv::GetPartialPointCloudMap; + +class TestPartialMapLoaderModule : public ::testing::Test +{ +protected: + void SetUp() override + { + // Initialize ROS node + rclcpp::init(0, nullptr); + node_ = std::make_shared("test_partial_map_loader_module"); + + // Generate a sample dummy pointcloud and save it to a file + pcl::PointCloud dummy_cloud; + dummy_cloud.width = 3; + dummy_cloud.height = 1; + dummy_cloud.points.resize(dummy_cloud.width * dummy_cloud.height); + dummy_cloud.points[0] = pcl::PointXYZ(-1.0, -1.0, -1.0); + dummy_cloud.points[1] = pcl::PointXYZ(0.0, 0.0, 0.0); + dummy_cloud.points[2] = pcl::PointXYZ(1.0, 1.0, 1.0); + pcl::io::savePCDFileASCII("/tmp/dummy.pcd", dummy_cloud); + + // Generate a sample dummy pointcloud metadata dictionary + std::map dummy_metadata_dict; + PCDFileMetadata dummy_metadata; + dummy_metadata.min = pcl::PointXYZ(-1.0, -1.0, -1.0); + dummy_metadata.max = pcl::PointXYZ(1.0, 1.0, 1.0); + dummy_metadata_dict["/tmp/dummy.pcd"] = dummy_metadata; + + // Initialize the PartialMapLoaderModule with the dummy metadata dictionary + module_ = std::make_shared(node_.get(), dummy_metadata_dict); + + // Create a client for the GetPartialPointCloudMap service + client_ = node_->create_client("service/get_partial_pcd_map"); + } + + void TearDown() override { rclcpp::shutdown(); } + + rclcpp::Node::SharedPtr node_; + std::shared_ptr module_; + rclcpp::Client::SharedPtr client_; +}; + +TEST_F(TestPartialMapLoaderModule, LoadPartialPCDFiles) +{ + // Wait for the GetPartialPointCloudMap service to be available + ASSERT_TRUE(client_->wait_for_service(std::chrono::seconds(3))); + + // Prepare a request for the service + auto request = std::make_shared(); + request->area.center.x = 0; + request->area.center.y = 0; + request->area.center.z = 0; + request->area.radius = 2; + + // Call the service + auto result_future = client_->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result_future), rclcpp::FutureReturnCode::SUCCESS); + + // Check the result + auto result = result_future.get(); + ASSERT_EQ(static_cast(result->new_pointcloud_with_ids.size()), 1); + EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd"); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/map/map_loader/test/test_pointcloud_map_loader_module.cpp b/map/map_loader/test/test_pointcloud_map_loader_module.cpp new file mode 100644 index 0000000000000..e28f5fce66ede --- /dev/null +++ b/map/map_loader/test/test_pointcloud_map_loader_module.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/pointcloud_map_loader/pointcloud_map_loader_module.hpp" +#include "../src/pointcloud_map_loader/utils.hpp" + +#include + +#include + +#include +#include +#include + +#include +#include +#include + +using std::chrono_literals::operator""ms; + +class TestPointcloudMapLoaderModule : public ::testing::Test +{ +protected: + rclcpp::Node::SharedPtr node; + std::string temp_pcd_path; + + void SetUp() override + { + rclcpp::init(0, nullptr); + node = rclcpp::Node::make_shared("test_pointcloud_map_loader_module"); + + // Create a temporary PCD file with dummy point cloud data + pcl::PointCloud cloud; + cloud.width = 5; + cloud.height = 1; + cloud.is_dense = false; + cloud.points.resize(cloud.width * cloud.height); + + for (size_t i = 0; i < cloud.points.size(); ++i) { + cloud.points[i].x = static_cast(i); + cloud.points[i].y = static_cast(i * 2); + cloud.points[i].z = static_cast(i * 3); + } + + temp_pcd_path = "/tmp/test_pointcloud_map_loader_module.pcd"; + pcl::io::savePCDFileASCII(temp_pcd_path, cloud); + } + + void TearDown() override + { + // Delete the temporary PCD file + std::filesystem::remove(temp_pcd_path); + + rclcpp::shutdown(); + } +}; + +TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) +{ + // Prepare PCD paths + std::vector pcd_paths = {temp_pcd_path}; + + // Create PointcloudMapLoaderModule instance + PointcloudMapLoaderModule loader(node.get(), pcd_paths, "pointcloud_map_no_downsample", false); + + // Create a subscriber to the published pointcloud topic + auto pointcloud_received = std::make_shared(false); + auto pointcloud_msg = std::make_shared(); + + rclcpp::QoS durable_qos{10}; + durable_qos.transient_local(); // Match publisher's Durability setting + + auto pointcloud_sub = node->create_subscription( + "pointcloud_map_no_downsample", durable_qos, + [pointcloud_received, pointcloud_msg](const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { + *pointcloud_received = true; + *pointcloud_msg = *msg; + }); + + // Spin until pointcloud is received or timeout occurs + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + auto start_time = node->now(); + while (!*pointcloud_received && (node->now() - start_time).seconds() < 3) { + executor.spin_some(50ms); + } + + // Check if the point cloud is received and the content is as expected + ASSERT_TRUE(*pointcloud_received); + + // Convert the received point cloud to pcl::PointCloudpcl::PointXYZ + pcl::PointCloud received_cloud; + pcl::fromROSMsg(*pointcloud_msg, received_cloud); + + // Check if the received point cloud matches the content of the temporary PCD file + ASSERT_EQ(static_cast(received_cloud.width), 5); + ASSERT_EQ(static_cast(received_cloud.height), 1); + + for (size_t i = 0; i < received_cloud.points.size(); ++i) { + EXPECT_FLOAT_EQ(received_cloud.points[i].x, static_cast(i)); + EXPECT_FLOAT_EQ(received_cloud.points[i].y, static_cast(i * 2)); + EXPECT_FLOAT_EQ(received_cloud.points[i].z, static_cast(i * 3)); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/bytetrack/CMakeLists.txt b/perception/bytetrack/CMakeLists.txt new file mode 100644 index 0000000000000..3e3d520eef14a --- /dev/null +++ b/perception/bytetrack/CMakeLists.txt @@ -0,0 +1,93 @@ +cmake_minimum_required(VERSION 3.5) +project(bytetrack) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(OpenCV REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(OpenMP REQUIRED) +if(OpenMP_FOUND) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() +find_package(Boost REQUIRED) + +# +# Core library +# +file(GLOB bytetrack_lib_src + "lib/src/*.cpp" +) + +ament_auto_add_library(bytetrack_lib SHARED + ${bytetrack_lib_src} +) + +target_include_directories(bytetrack_lib + PUBLIC + $ + $ +) + +# +# ROS node +# +ament_auto_add_library(${PROJECT_NAME} SHARED + src/bytetrack.cpp +) + +ament_target_dependencies(${PROJECT_NAME} + OpenCV +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) + +target_compile_definitions(${PROJECT_NAME} PRIVATE + TENSORRT_VERSION_MAJOR=${TENSORRT_VERSION_MAJOR} +) + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/bytetrack_node.cpp +) + +ament_target_dependencies(${PROJECT_NAME}_node + OpenCV +) + +target_link_libraries(${PROJECT_NAME}_node + ${PROJECT_NAME} +) + +target_compile_definitions(${PROJECT_NAME}_node PRIVATE + TENSORRT_VERSION_MAJOR=${TENSORRT_VERSION_MAJOR} +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "bytetrack::ByteTrackNode" + EXECUTABLE ${PROJECT_NAME}_node_exe +) + +# +# Visualizer +# +ament_auto_add_library(${PROJECT_NAME}_visualizer SHARED + src/bytetrack_visualizer_node.cpp +) + +ament_target_dependencies(${PROJECT_NAME}_visualizer + OpenCV +) + +rclcpp_components_register_node(${PROJECT_NAME}_visualizer + PLUGIN "bytetrack::ByteTrackVisualizerNode" + EXECUTABLE ${PROJECT_NAME}_visualizer_node_exe +) + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/perception/bytetrack/README.md b/perception/bytetrack/README.md new file mode 100644 index 0000000000000..c71b62cada29f --- /dev/null +++ b/perception/bytetrack/README.md @@ -0,0 +1,99 @@ +# bytetrack + +## Purpose + +The core algorithm, named `ByteTrack`, mainly aims to perform multi-object tracking. +Because the algorithm associates almost every detection box including ones with low detection scores, +the number of false negatives is expected to decrease by using it. + +[demo video](https://user-images.githubusercontent.com/3022416/225920856-745a3bb7-6b35-403d-87b0-6e5085952d70.mp4) + +## Inner-workings / Algorithms + +### Cite + +- Yifu Zhang, Peize Sun, Yi Jiang, Dongdong Yu, Fucheng Weng, Zehuan Yuan, Ping Luo, Wenyu Liu, and Xinggang Wang, + "ByteTrack: Multi-Object Tracking by Associating Every Detection Box", in the proc. of the ECCV + 2022, [[ref](https://arxiv.org/abs/2110.06864)] +- This package is ported version toward Autoware from [this repository](https://github.com/ifzhang/ByteTrack/tree/main/deploy/TensorRT/cpp) + (The C++ implementation by the ByteTrack's authors) + +## Inputs / Outputs + +### bytetrack_node + +#### Input + +| Name | Type | Description | +| --------- | -------------------------------------------------- | ------------------------------------------- | +| `in/rect` | `tier4_perception_msgs/DetectedObjectsWithFeature` | The detected objects with 2D bounding boxes | + +#### Output + +| Name | Type | Description | +| ------------------------ | -------------------------------------------------- | --------------------------------------------------------- | +| `out/objects` | `tier4_perception_msgs/DetectedObjectsWithFeature` | The detected objects with 2D bounding boxes | +| `out/objects/debug/uuid` | `tier4_perception_msgs/DynamicObjectArray` | The universally unique identifiers (UUID) for each object | + +### bytetrack_visualizer + +#### Input + +| Name | Type | Description | +| ---------- | ---------------------------------------------------- | --------------------------------------------------------- | +| `in/image` | `sensor_msgs/Image` or `sensor_msgs/CompressedImage` | The input image on which object detection is performed | +| `in/rect` | `tier4_perception_msgs/DetectedObjectsWithFeature` | The detected objects with 2D bounding boxes | +| `in/uuid` | `tier4_perception_msgs/DynamicObjectArray` | The universally unique identifiers (UUID) for each object | + +#### Output + +| Name | Type | Description | +| ----------- | ------------------- | ----------------------------------------------------------------- | +| `out/image` | `sensor_msgs/Image` | The image that detection bounding boxes and their UUIDs are drawn | + +## Parameters + +### bytetrack_node + +| Name | Type | Default Value | Description | +| --------------------- | ---- | ------------- | -------------------------------------------------------- | +| `track_buffer_length` | int | 30 | The frame count that a tracklet is considered to be lost | + +### bytetrack_visualizer + +| Name | Type | Default Value | Description | +| --------- | ---- | ------------- | --------------------------------------------------------------------------------------------- | +| `use_raw` | bool | false | The flag for the node to switch `sensor_msgs/Image` or `sensor_msgs/CompressedImage` as input | + +## Assumptions/Known limits + +## Reference repositories + +- + +## License + +The codes under the `lib` directory are copied from [the original codes](https://github.com/ifzhang/ByteTrack/tree/72ca8b45d36caf5a39e949c6aa815d9abffd1ab5/deploy/TensorRT/cpp) and modified. +The original codes belong to the MIT license stated as follows, while this ported packages are provided with Apache License 2.0: + +> MIT License +> +> Copyright (c) 2021 Yifu Zhang +> +> Permission is hereby granted, free of charge, to any person obtaining a copy +> of this software and associated documentation files (the "Software"), to deal +> in the Software without restriction, including without limitation the rights +> to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +> copies of the Software, and to permit persons to whom the Software is +> furnished to do so, subject to the following conditions: +> +> The above copyright notice and this permission notice shall be included in all +> copies or substantial portions of the Software. +> +> THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +> IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +> FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +> AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +> LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +> OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +> SOFTWARE. diff --git a/perception/bytetrack/include/bytetrack/bytetrack.hpp b/perception/bytetrack/include/bytetrack/bytetrack.hpp new file mode 100644 index 0000000000000..549ecaac84567 --- /dev/null +++ b/perception/bytetrack/include/bytetrack/bytetrack.hpp @@ -0,0 +1,62 @@ +// Copyright 2023 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 BYTETRACK__BYTETRACK_HPP_ +#define BYTETRACK__BYTETRACK_HPP_ + +#include "byte_tracker.h" +#include "strack.h" + +#include +#include + +#include + +#include +#include +#include +#include + +namespace bytetrack +{ +struct Object +{ + int32_t x_offset; + int32_t y_offset; + int32_t height; + int32_t width; + float score; + int32_t type; + int32_t track_id; + boost::uuids::uuid unique_id; +}; + +using ObjectArray = std::vector; + +class ByteTrack +{ +public: + explicit ByteTrack(const int track_buffer_length = 30); + + bool do_inference(ObjectArray & objects); + ObjectArray update_tracker(ObjectArray & input_objects); + +private: + std::unique_ptr tracker_; + ObjectArray latest_objects_; +}; + +} // namespace bytetrack + +#endif // BYTETRACK__BYTETRACK_HPP_ diff --git a/perception/bytetrack/include/bytetrack/bytetrack_node.hpp b/perception/bytetrack/include/bytetrack/bytetrack_node.hpp new file mode 100644 index 0000000000000..6b801ddf93268 --- /dev/null +++ b/perception/bytetrack/include/bytetrack/bytetrack_node.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 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 BYTETRACK__BYTETRACK_NODE_HPP_ +#define BYTETRACK__BYTETRACK_NODE_HPP_ + +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace bytetrack +{ +using LabelMap = std::map; + +class ByteTrackNode : public rclcpp::Node +{ +public: + explicit ByteTrackNode(const rclcpp::NodeOptions & node_options); + +private: + void on_connect(); + void on_rect(const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr msg); + + rclcpp::Publisher::SharedPtr objects_pub_; + rclcpp::Publisher::SharedPtr objects_uuid_pub_; + + rclcpp::Subscription::SharedPtr + detection_rect_sub_; + + rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr bytetrack_; +}; + +} // namespace bytetrack + +#endif // BYTETRACK__BYTETRACK_NODE_HPP_ diff --git a/perception/bytetrack/include/bytetrack/bytetrack_visualizer_node.hpp b/perception/bytetrack/include/bytetrack/bytetrack_visualizer_node.hpp new file mode 100644 index 0000000000000..56f060ecfa9d5 --- /dev/null +++ b/perception/bytetrack/include/bytetrack/bytetrack_visualizer_node.hpp @@ -0,0 +1,104 @@ +// Copyright 2023 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 BYTETRACK__BYTETRACK_VISUALIZER_NODE_HPP_ +#define BYTETRACK__BYTETRACK_VISUALIZER_NODE_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace bytetrack +{ +// A helper class to generate bright color instance +class ColorMapper +{ +public: + const size_t kColorNum = 512; + ColorMapper() + { + // generate bright color map + cv::Mat src = cv::Mat::zeros(cv::Size(kColorNum, 1), CV_8UC1); + for (size_t i = 0; i < kColorNum; i++) { + src.at(0, i) = i; + } + cv::applyColorMap(src, color_table_, cv::COLORMAP_HSV); + } + + cv::Scalar operator()(size_t idx) + { + if (kColorNum <= idx) { + throw std::runtime_error("idx should be between [0, 255]"); + } + return color_table_.at(0, idx); + } + +protected: + cv::Mat color_table_; +}; + +class ByteTrackVisualizerNode : public rclcpp::Node +{ +public: + explicit ByteTrackVisualizerNode(const rclcpp::NodeOptions & node_options); + ~ByteTrackVisualizerNode(); + +protected: + void on_timer(); + bool get_topic_qos(const std::string & query_topic, rclcpp::QoS & qos); + + void callback( + const sensor_msgs::msg::Image::SharedPtr & image_msg, + const tier4_perception_msgs::msg::DetectedObjectsWithFeature::SharedPtr & rect_msg, + const tier4_perception_msgs::msg::DynamicObjectArray::SharedPtr & uuid_msg); + void draw( + cv::Mat & image, const std::vector & bboxes, + const std::vector & uuids); + + image_transport::SubscriberFilter image_sub_; + message_filters::Subscriber rect_sub_; + message_filters::Subscriber uuid_sub_; + + using ExactTimeSyncPolicy = message_filters::sync_policies::ExactTime< + sensor_msgs::msg::Image, tier4_perception_msgs::msg::DetectedObjectsWithFeature, + tier4_perception_msgs::msg::DynamicObjectArray>; + using ExactTimeSync = message_filters::Synchronizer; + std::shared_ptr sync_ptr_; + + image_transport::Publisher image_pub_; + + rclcpp::TimerBase::SharedPtr timer_; + + bool use_raw_; + ColorMapper color_map_; +}; +} // namespace bytetrack + +#endif // BYTETRACK__BYTETRACK_VISUALIZER_NODE_HPP_ diff --git a/perception/bytetrack/launch/bytetrack.launch.xml b/perception/bytetrack/launch/bytetrack.launch.xml new file mode 100644 index 0000000000000..25ed56b991c30 --- /dev/null +++ b/perception/bytetrack/launch/bytetrack.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/bytetrack/lib/include/byte_tracker.h b/perception/bytetrack/lib/include/byte_tracker.h new file mode 100644 index 0000000000000..08394e6b0ccdf --- /dev/null +++ b/perception/bytetrack/lib/include/byte_tracker.h @@ -0,0 +1,100 @@ +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Copyright 2023 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. + +#pragma once + +#include "strack.h" + +#include + +struct ByteTrackObject +{ + cv::Rect_ rect; + int label; + float prob; +}; + +class ByteTracker +{ +public: + ByteTracker( + int track_buffer = 30, float track_thresh = 0.5, float high_thresh = 0.6, + float match_thresh = 0.8); + ~ByteTracker(); + + std::vector update(const std::vector & objects); + cv::Scalar get_color(int idx); + +private: + std::vector joint_stracks(std::vector & tlista, std::vector & tlistb); + std::vector joint_stracks(std::vector & tlista, std::vector & tlistb); + + std::vector sub_stracks(std::vector & tlista, std::vector & tlistb); + void remove_duplicate_stracks( + std::vector & resa, std::vector & resb, std::vector & stracksa, + std::vector & stracksb); + + void linear_assignment( + std::vector> & cost_matrix, int cost_matrix_size, int cost_matrix_size_size, + float thresh, std::vector> & matches, std::vector & unmatched_a, + std::vector & unmatched_b); + std::vector> iou_distance( + std::vector & atracks, std::vector & btracks, int & dist_size, + int & dist_size_size); + std::vector> iou_distance( + std::vector & atracks, std::vector & btracks); + std::vector> ious( + std::vector> & atlbrs, std::vector> & btlbrs); + + double lapjv( + const std::vector> & cost, std::vector & rowsol, + std::vector & colsol, bool extend_cost = false, float cost_limit = LONG_MAX, + bool return_cost = true); + +private: + float track_thresh; + float high_thresh; + float match_thresh; + int frame_id; + int max_time_lost; + + std::vector tracked_stracks; + std::vector lost_stracks; + std::vector removed_stracks; + byte_kalman::KalmanFilter kalman_filter; +}; diff --git a/perception/bytetrack/lib/include/data_type.h b/perception/bytetrack/lib/include/data_type.h new file mode 100644 index 0000000000000..d76baa8945637 --- /dev/null +++ b/perception/bytetrack/lib/include/data_type.h @@ -0,0 +1,76 @@ +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Copyright 2023 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. + +#pragma once + +#include +#include + +#include +#include +#include +typedef Eigen::Matrix DETECTBOX; +typedef Eigen::Matrix DETECTBOXSS; +typedef Eigen::Matrix FEATURE; +typedef Eigen::Matrix FEATURESS; +// typedef std::vector FEATURESS; + +// Kalmanfilter +// typedef Eigen::Matrix KAL_FILTER; +typedef Eigen::Matrix KAL_MEAN; +typedef Eigen::Matrix KAL_COVA; +typedef Eigen::Matrix KAL_HMEAN; +typedef Eigen::Matrix KAL_HCOVA; +using KAL_DATA = std::pair; +using KAL_HDATA = std::pair; + +// main +using RESULT_DATA = std::pair; + +// tracker: +using TRACKER_DATA = std::pair; +using MATCH_DATA = std::pair; +typedef struct t +{ + std::vector matches; + std::vector unmatched_tracks; + std::vector unmatched_detections; +} TRACHER_MATCHD; + +// linear_assignment: +typedef Eigen::Matrix DYNAMICM; diff --git a/perception/bytetrack/lib/include/kalman_filter.h b/perception/bytetrack/lib/include/kalman_filter.h new file mode 100644 index 0000000000000..f2b1e1c7817dd --- /dev/null +++ b/perception/bytetrack/lib/include/kalman_filter.h @@ -0,0 +1,67 @@ +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Copyright 2023 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. + +#pragma once + +#include "data_type.h" + +#include +namespace byte_kalman +{ +class KalmanFilter +{ +public: + static const double chi2inv95[10]; + KalmanFilter(); + KAL_DATA initiate(const DETECTBOX & measurement); + void predict(KAL_MEAN & mean, KAL_COVA & covariance); + KAL_HDATA project(const KAL_MEAN & mean, const KAL_COVA & covariance); + KAL_DATA update( + const KAL_MEAN & mean, const KAL_COVA & covariance, const DETECTBOX & measurement); + + Eigen::Matrix gating_distance( + const KAL_MEAN & mean, const KAL_COVA & covariance, const std::vector & measurements, + bool only_position = false); + +private: + Eigen::Matrix _motion_mat; + Eigen::Matrix _update_mat; + float _std_weight_position; + float _std_weight_velocity; +}; +} // namespace byte_kalman diff --git a/perception/bytetrack/lib/include/lapjv.h b/perception/bytetrack/lib/include/lapjv.h new file mode 100644 index 0000000000000..d197b747e6f7d --- /dev/null +++ b/perception/bytetrack/lib/include/lapjv.h @@ -0,0 +1,110 @@ +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Copyright 2023 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 LAPJV_H_ +#define LAPJV_H_ + +#define LARGE 1000000 + +#if !defined TRUE +#define TRUE 1 +#endif +#if !defined FALSE +#define FALSE 0 +#endif + +#define NEW(x, t, n) \ + if ((x = reinterpret_cast(malloc(sizeof(t) * (n)))) == 0) { \ + return -1; \ + } +#define FREE(x) \ + if (x != 0) { \ + free(x); \ + x = 0; \ + } +#define SWAP_INDICES(a, b) \ + { \ + int_t _temp_index = a; \ + a = b; \ + b = _temp_index; \ + } + +#if 0 +#include +#define ASSERT(cond) assert(cond) +#define PRINTF(fmt, ...) printf(fmt, ##__VA_ARGS__) +#define PRINT_COST_ARRAY(a, n) \ + while (1) { \ + printf(#a " = ["); \ + if ((n) > 0) { \ + printf("%f", (a)[0]); \ + for (uint_t j = 1; j < n; j++) { \ + printf(", %f", (a)[j]); \ + } \ + } \ + printf("]\n"); \ + break; \ + } +#define PRINT_INDEX_ARRAY(a, n) \ + while (1) { \ + printf(#a " = ["); \ + if ((n) > 0) { \ + printf("%d", (a)[0]); \ + for (uint_t j = 1; j < n; j++) { \ + printf(", %d", (a)[j]); \ + } \ + } \ + printf("]\n"); \ + break; \ + } +#else +#define ASSERT(cond) +#define PRINTF(fmt, ...) +#define PRINT_COST_ARRAY(a, n) +#define PRINT_INDEX_ARRAY(a, n) +#endif + +typedef signed int int_t; +typedef unsigned int uint_t; +typedef double cost_t; +typedef char boolean; +typedef enum fp_t { FP_1 = 1, FP_2 = 2, FP_DYNAMIC = 3 } fp_t; + +extern int_t lapjv_internal(const uint_t n, cost_t * cost[], int_t * x, int_t * y); + +#endif // LAPJV_H_ diff --git a/perception/bytetrack/lib/include/strack.h b/perception/bytetrack/lib/include/strack.h new file mode 100644 index 0000000000000..2110723e86c58 --- /dev/null +++ b/perception/bytetrack/lib/include/strack.h @@ -0,0 +1,94 @@ +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Copyright 2023 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. + +#pragma once + +#include "kalman_filter.h" + +#include + +#include + +#include + +enum TrackState { New = 0, Tracked, Lost, Removed }; + +class STrack +{ +public: + STrack(std::vector tlwh_, float score, int label); + ~STrack(); + + std::vector static tlbr_to_tlwh(std::vector & tlbr); + static void multi_predict( + std::vector & stracks, byte_kalman::KalmanFilter & kalman_filter); + void static_tlwh(); + void static_tlbr(); + std::vector tlwh_to_xyah(std::vector tlwh_tmp); + std::vector to_xyah(); + void mark_lost(); + void mark_removed(); + int next_id(); + int end_frame(); + + void activate(byte_kalman::KalmanFilter & kalman_filter, int frame_id); + void re_activate(STrack & new_track, int frame_id, bool new_id = false); + void update(STrack & new_track, int frame_id); + +public: + bool is_activated; + int track_id; + boost::uuids::uuid unique_id; + int state; + + std::vector _tlwh; + std::vector tlwh; + std::vector tlbr; + int frame_id; + int tracklet_len; + int start_frame; + + KAL_MEAN mean; + KAL_COVA covariance; + float score; + + int label; + +private: + byte_kalman::KalmanFilter kalman_filter; +}; diff --git a/perception/bytetrack/lib/src/byte_tracker.cpp b/perception/bytetrack/lib/src/byte_tracker.cpp new file mode 100644 index 0000000000000..77daf8437ea02 --- /dev/null +++ b/perception/bytetrack/lib/src/byte_tracker.cpp @@ -0,0 +1,243 @@ +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Copyright 2023 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 "byte_tracker.h" + +#include +#include + +ByteTracker::ByteTracker( + int track_buffer, float track_thresh, float high_thresh, float match_thresh) +: track_thresh(track_thresh), high_thresh(high_thresh), match_thresh(match_thresh) + +{ + frame_id = 0; + max_time_lost = track_buffer; + std::cout << "Init ByteTrack!" << std::endl; +} + +ByteTracker::~ByteTracker() {} + +std::vector ByteTracker::update(const std::vector & objects) +{ + ////////////////// Step 1: Get detections ////////////////// + this->frame_id++; + std::vector activated_stracks; + std::vector refind_stracks; + std::vector removed_stracks; + std::vector lost_stracks; + std::vector detections; + std::vector detections_low; + + std::vector detections_cp; + std::vector tracked_stracks_swap; + std::vector resa, resb; + std::vector output_stracks; + + std::vector unconfirmed; + std::vector tracked_stracks; + std::vector strack_pool; + std::vector r_tracked_stracks; + + if (objects.size() > 0) { + for (size_t i = 0; i < objects.size(); i++) { + std::vector tlbr_; + tlbr_.resize(4); + tlbr_[0] = objects[i].rect.x; + tlbr_[1] = objects[i].rect.y; + tlbr_[2] = objects[i].rect.x + objects[i].rect.width; + tlbr_[3] = objects[i].rect.y + objects[i].rect.height; + + float score = objects[i].prob; + + STrack strack(STrack::tlbr_to_tlwh(tlbr_), score, objects[i].label); + if (score >= track_thresh) { + detections.push_back(strack); + } else { + detections_low.push_back(strack); + } + } + } + + // Add newly detected tracklets to tracked_stracks + for (size_t i = 0; i < this->tracked_stracks.size(); i++) { + if (!this->tracked_stracks[i].is_activated) + unconfirmed.push_back(&this->tracked_stracks[i]); + else + tracked_stracks.push_back(&this->tracked_stracks[i]); + } + + ////////////////// Step 2: First association, with IoU ////////////////// + strack_pool = joint_stracks(tracked_stracks, this->lost_stracks); + STrack::multi_predict(strack_pool, this->kalman_filter); + + std::vector > dists; + int dist_size = 0, dist_size_size = 0; + dists = iou_distance(strack_pool, detections, dist_size, dist_size_size); + + std::vector > matches; + std::vector u_track, u_detection; + linear_assignment(dists, dist_size, dist_size_size, match_thresh, matches, u_track, u_detection); + + for (size_t i = 0; i < matches.size(); i++) { + STrack * track = strack_pool[matches[i][0]]; + STrack * det = &detections[matches[i][1]]; + if (track->state == TrackState::Tracked) { + track->update(*det, this->frame_id); + activated_stracks.push_back(*track); + } else { + track->re_activate(*det, this->frame_id, false); + refind_stracks.push_back(*track); + } + } + + ////////////////// Step 3: Second association, using low score dets ////////////////// + for (size_t i = 0; i < u_detection.size(); i++) { + detections_cp.push_back(detections[u_detection[i]]); + } + detections.clear(); + detections.assign(detections_low.begin(), detections_low.end()); + + for (size_t i = 0; i < u_track.size(); i++) { + if (strack_pool[u_track[i]]->state == TrackState::Tracked) { + r_tracked_stracks.push_back(strack_pool[u_track[i]]); + } + } + + dists.clear(); + dists = iou_distance(r_tracked_stracks, detections, dist_size, dist_size_size); + + matches.clear(); + u_track.clear(); + u_detection.clear(); + linear_assignment(dists, dist_size, dist_size_size, 0.5, matches, u_track, u_detection); + + for (size_t i = 0; i < matches.size(); i++) { + STrack * track = r_tracked_stracks[matches[i][0]]; + STrack * det = &detections[matches[i][1]]; + if (track->state == TrackState::Tracked) { + track->update(*det, this->frame_id); + activated_stracks.push_back(*track); + } else { + track->re_activate(*det, this->frame_id, false); + refind_stracks.push_back(*track); + } + } + + for (size_t i = 0; i < u_track.size(); i++) { + STrack * track = r_tracked_stracks[u_track[i]]; + if (track->state != TrackState::Lost) { + track->mark_lost(); + lost_stracks.push_back(*track); + } + } + + // Deal with unconfirmed tracks, usually tracks with only one beginning frame + detections.clear(); + detections.assign(detections_cp.begin(), detections_cp.end()); + + dists.clear(); + dists = iou_distance(unconfirmed, detections, dist_size, dist_size_size); + + matches.clear(); + std::vector u_unconfirmed; + u_detection.clear(); + linear_assignment(dists, dist_size, dist_size_size, 0.7, matches, u_unconfirmed, u_detection); + + for (size_t i = 0; i < matches.size(); i++) { + unconfirmed[matches[i][0]]->update(detections[matches[i][1]], this->frame_id); + activated_stracks.push_back(*unconfirmed[matches[i][0]]); + } + + for (size_t i = 0; i < u_unconfirmed.size(); i++) { + STrack * track = unconfirmed[u_unconfirmed[i]]; + track->mark_removed(); + removed_stracks.push_back(*track); + } + + ////////////////// Step 4: Init new stracks ////////////////// + for (size_t i = 0; i < u_detection.size(); i++) { + STrack * track = &detections[u_detection[i]]; + if (track->score < this->high_thresh) continue; + track->activate(this->kalman_filter, this->frame_id); + activated_stracks.push_back(*track); + } + + ////////////////// Step 5: Update state ////////////////// + for (size_t i = 0; i < this->lost_stracks.size(); i++) { + if (this->frame_id - this->lost_stracks[i].end_frame() > this->max_time_lost) { + this->lost_stracks[i].mark_removed(); + removed_stracks.push_back(this->lost_stracks[i]); + } + } + + for (size_t i = 0; i < this->tracked_stracks.size(); i++) { + if (this->tracked_stracks[i].state == TrackState::Tracked) { + tracked_stracks_swap.push_back(this->tracked_stracks[i]); + } + } + this->tracked_stracks.clear(); + this->tracked_stracks.assign(tracked_stracks_swap.begin(), tracked_stracks_swap.end()); + + this->tracked_stracks = joint_stracks(this->tracked_stracks, activated_stracks); + this->tracked_stracks = joint_stracks(this->tracked_stracks, refind_stracks); + + this->lost_stracks = sub_stracks(this->lost_stracks, this->tracked_stracks); + for (size_t i = 0; i < lost_stracks.size(); i++) { + this->lost_stracks.push_back(lost_stracks[i]); + } + + this->lost_stracks = sub_stracks(this->lost_stracks, this->removed_stracks); + for (size_t i = 0; i < removed_stracks.size(); i++) { + this->removed_stracks.push_back(removed_stracks[i]); + } + + remove_duplicate_stracks(resa, resb, this->tracked_stracks, this->lost_stracks); + + this->tracked_stracks.clear(); + this->tracked_stracks.assign(resa.begin(), resa.end()); + this->lost_stracks.clear(); + this->lost_stracks.assign(resb.begin(), resb.end()); + + for (size_t i = 0; i < this->tracked_stracks.size(); i++) { + if (this->tracked_stracks[i].is_activated) { + output_stracks.push_back(this->tracked_stracks[i]); + } + } + return output_stracks; +} diff --git a/perception/bytetrack/lib/src/kalman_filter.cpp b/perception/bytetrack/lib/src/kalman_filter.cpp new file mode 100644 index 0000000000000..bc1a88ff60b0e --- /dev/null +++ b/perception/bytetrack/lib/src/kalman_filter.cpp @@ -0,0 +1,175 @@ +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Copyright 2023 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 "kalman_filter.h" + +#include + +namespace byte_kalman +{ +const double KalmanFilter::chi2inv95[10] = {0, 3.8415, 5.9915, 7.8147, 9.4877, + 11.070, 12.592, 14.067, 15.507, 16.919}; +KalmanFilter::KalmanFilter() +{ + int ndim = 4; + double dt = 1.; + + _motion_mat = Eigen::MatrixXf::Identity(8, 8); + for (int i = 0; i < ndim; i++) { + _motion_mat(i, ndim + i) = dt; + } + _update_mat = Eigen::MatrixXf::Identity(4, 8); + + this->_std_weight_position = 1. / 20; + this->_std_weight_velocity = 1. / 160; +} + +KAL_DATA KalmanFilter::initiate(const DETECTBOX & measurement) +{ + DETECTBOX mean_pos = measurement; + DETECTBOX mean_vel; + for (int i = 0; i < 4; i++) mean_vel(i) = 0; + + KAL_MEAN mean; + for (int i = 0; i < 8; i++) { + if (i < 4) + mean(i) = mean_pos(i); + else + mean(i) = mean_vel(i - 4); + } + + KAL_MEAN std; + std(0) = 2 * _std_weight_position * measurement[3]; + std(1) = 2 * _std_weight_position * measurement[3]; + std(2) = 1e-2; + std(3) = 2 * _std_weight_position * measurement[3]; + std(4) = 10 * _std_weight_velocity * measurement[3]; + std(5) = 10 * _std_weight_velocity * measurement[3]; + std(6) = 1e-5; + std(7) = 10 * _std_weight_velocity * measurement[3]; + + KAL_MEAN tmp = std.array().square(); + KAL_COVA var = tmp.asDiagonal(); + return std::make_pair(mean, var); +} + +void KalmanFilter::predict(KAL_MEAN & mean, KAL_COVA & covariance) +{ + // revise the data; + DETECTBOX std_pos; + std_pos << _std_weight_position * mean(3), _std_weight_position * mean(3), 1e-2, + _std_weight_position * mean(3); + DETECTBOX std_vel; + std_vel << _std_weight_velocity * mean(3), _std_weight_velocity * mean(3), 1e-5, + _std_weight_velocity * mean(3); + KAL_MEAN tmp; + tmp.block<1, 4>(0, 0) = std_pos; + tmp.block<1, 4>(0, 4) = std_vel; + tmp = tmp.array().square(); + KAL_COVA motion_cov = tmp.asDiagonal(); + KAL_MEAN mean1 = this->_motion_mat * mean.transpose(); + KAL_COVA covariance1 = this->_motion_mat * covariance * (_motion_mat.transpose()); + covariance1 += motion_cov; + + mean = mean1; + covariance = covariance1; +} + +KAL_HDATA KalmanFilter::project(const KAL_MEAN & mean, const KAL_COVA & covariance) +{ + DETECTBOX std; + std << _std_weight_position * mean(3), _std_weight_position * mean(3), 1e-1, + _std_weight_position * mean(3); + KAL_HMEAN mean1 = _update_mat * mean.transpose(); + KAL_HCOVA covariance1 = _update_mat * covariance * (_update_mat.transpose()); + Eigen::Matrix diag = std.asDiagonal(); + diag = diag.array().square().matrix(); + covariance1 += diag; + // covariance1.diagonal() << diag; + return std::make_pair(mean1, covariance1); +} + +KAL_DATA +KalmanFilter::update( + const KAL_MEAN & mean, const KAL_COVA & covariance, const DETECTBOX & measurement) +{ + KAL_HDATA pa = project(mean, covariance); + KAL_HMEAN projected_mean = pa.first; + KAL_HCOVA projected_cov = pa.second; + + // chol_factor, lower = + // scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False) + // kalmain_gain = + // scipy.linalg.cho_solve((cho_factor, lower), + // np.dot(covariance, self._upadte_mat.T).T, + // check_finite=False).T + Eigen::Matrix B = (covariance * (_update_mat.transpose())).transpose(); + Eigen::Matrix kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4 + Eigen::Matrix innovation = measurement - projected_mean; // eg.1x4 + auto tmp = innovation * (kalman_gain.transpose()); + KAL_MEAN new_mean = (mean.array() + tmp.array()).matrix(); + KAL_COVA new_covariance = covariance - kalman_gain * projected_cov * (kalman_gain.transpose()); + return std::make_pair(new_mean, new_covariance); +} + +Eigen::Matrix KalmanFilter::gating_distance( + const KAL_MEAN & mean, const KAL_COVA & covariance, const std::vector & measurements, + bool only_position) +{ + KAL_HDATA pa = this->project(mean, covariance); + if (only_position) { + printf("not implement!"); + exit(0); + } + KAL_HMEAN mean1 = pa.first; + KAL_HCOVA covariance1 = pa.second; + + // Eigen::Matrix d(size, 4); + DETECTBOXSS d(measurements.size(), 4); + int pos = 0; + for (DETECTBOX box : measurements) { + d.row(pos++) = box - mean1; + } + Eigen::Matrix factor = covariance1.llt().matrixL(); + Eigen::Matrix z = + factor.triangularView().solve(d).transpose(); + auto zz = ((z.array()) * (z.array())).matrix(); + auto square_maha = zz.colwise().sum(); + return square_maha; +} +} // namespace byte_kalman diff --git a/perception/bytetrack/lib/src/lapjv.cpp b/perception/bytetrack/lib/src/lapjv.cpp new file mode 100644 index 0000000000000..ecf4c31b81ff8 --- /dev/null +++ b/perception/bytetrack/lib/src/lapjv.cpp @@ -0,0 +1,362 @@ +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Copyright 2023 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 "lapjv.h" + +#include +#include +#include + +/** Column-reduction and reduction transfer for a dense cost matrix. + */ +int_t _ccrrt_dense( + const uint_t n, cost_t * cost[], int_t * free_rows, int_t * x, int_t * y, cost_t * v) +{ + int_t n_free_rows; + boolean * unique; + + for (uint_t i = 0; i < n; i++) { + x[i] = -1; + v[i] = LARGE; + y[i] = 0; + } + for (uint_t i = 0; i < n; i++) { + for (uint_t j = 0; j < n; j++) { + const cost_t c = cost[i][j]; + if (c < v[j]) { + v[j] = c; + y[j] = i; + } + PRINTF("i=%d, j=%d, c[i,j]=%f, v[j]=%f y[j]=%d\n", i, j, c, v[j], y[j]); + } + } + PRINT_COST_ARRAY(v, n); + PRINT_INDEX_ARRAY(y, n); + NEW(unique, boolean, n); + memset(unique, TRUE, n); + { + int_t j = n; + do { + j--; + const int_t i = y[j]; + if (x[i] < 0) { + x[i] = j; + } else { + unique[i] = FALSE; + y[j] = -1; + } + } while (j > 0); + } + n_free_rows = 0; + for (uint_t i = 0; i < n; i++) { + if (x[i] < 0) { + free_rows[n_free_rows++] = i; + } else if (unique[i]) { + const int_t j = x[i]; + cost_t min = LARGE; + for (uint_t j2 = 0; j2 < n; j2++) { + if (j2 == (uint_t)j) { + continue; + } + const cost_t c = cost[i][j2] - v[j2]; + if (c < min) { + min = c; + } + } + PRINTF("v[%d] = %f - %f\n", j, v[j], min); + v[j] -= min; + } + } + FREE(unique); + return n_free_rows; +} + +/** Augmenting row reduction for a dense cost matrix. + */ +int_t _carr_dense( + const uint_t n, cost_t * cost[], const uint_t n_free_rows, int_t * free_rows, int_t * x, + int_t * y, cost_t * v) +{ + uint_t current = 0; + int_t new_free_rows = 0; + uint_t rr_cnt = 0; + PRINT_INDEX_ARRAY(x, n); + PRINT_INDEX_ARRAY(y, n); + PRINT_COST_ARRAY(v, n); + PRINT_INDEX_ARRAY(free_rows, n_free_rows); + while (current < n_free_rows) { + int_t i0; + int_t j1, j2; + cost_t v1, v2, v1_new; + boolean v1_lowers; + + rr_cnt++; + PRINTF("current = %d rr_cnt = %d\n", current, rr_cnt); + const int_t free_i = free_rows[current++]; + j1 = 0; + v1 = cost[free_i][0] - v[0]; + j2 = -1; + v2 = LARGE; + for (uint_t j = 1; j < n; j++) { + PRINTF("%d = %f %d = %f\n", j1, v1, j2, v2); + const cost_t c = cost[free_i][j] - v[j]; + if (c < v2) { + if (c >= v1) { + v2 = c; + j2 = j; + } else { + v2 = v1; + v1 = c; + j2 = j1; + j1 = j; + } + } + } + i0 = y[j1]; + v1_new = v[j1] - (v2 - v1); + v1_lowers = v1_new < v[j1]; + PRINTF( + "%d %d 1=%d,%f 2=%d,%f v1'=%f(%d,%g) \n", free_i, i0, j1, v1, j2, v2, v1_new, v1_lowers, + v[j1] - v1_new); + if (rr_cnt < current * n) { + if (v1_lowers) { + v[j1] = v1_new; + } else if (i0 >= 0 && j2 >= 0) { + j1 = j2; + i0 = y[j2]; + } + if (i0 >= 0) { + if (v1_lowers) { + free_rows[--current] = i0; + } else { + free_rows[new_free_rows++] = i0; + } + } + } else { + PRINTF("rr_cnt=%d >= %d (current=%d * n=%d)\n", rr_cnt, current * n, current, n); + if (i0 >= 0) { + free_rows[new_free_rows++] = i0; + } + } + x[free_i] = j1; + y[j1] = free_i; + } + return new_free_rows; +} + +/** Find columns with minimum d[j] and put them on the SCAN list. + */ +uint_t _find_dense(const uint_t n, uint_t lo, cost_t * d, int_t * cols, [[maybe_unused]] int_t * y) +{ + uint_t hi = lo + 1; + cost_t mind = d[cols[lo]]; + for (uint_t k = hi; k < n; k++) { + int_t j = cols[k]; + if (d[j] <= mind) { + if (d[j] < mind) { + hi = lo; + mind = d[j]; + } + cols[k] = cols[hi]; + cols[hi++] = j; + } + } + return hi; +} + +// Scan all columns in TODO starting from arbitrary column in SCAN +// and try to decrease d of the TODO columns using the SCAN column. +int_t _scan_dense( + const uint_t n, cost_t * cost[], uint_t * plo, uint_t * phi, cost_t * d, int_t * cols, + int_t * pred, int_t * y, cost_t * v) +{ + uint_t lo = *plo; + uint_t hi = *phi; + cost_t h, cred_ij; + + while (lo != hi) { + int_t j = cols[lo++]; + const int_t i = y[j]; + const cost_t mind = d[j]; + h = cost[i][j] - v[j] - mind; + PRINTF("i=%d j=%d h=%f\n", i, j, h); + // For all columns in TODO + for (uint_t k = hi; k < n; k++) { + j = cols[k]; + cred_ij = cost[i][j] - v[j] - h; + if (cred_ij < d[j]) { + d[j] = cred_ij; + pred[j] = i; + if (cred_ij == mind) { + if (y[j] < 0) { + return j; + } + cols[k] = cols[hi]; + cols[hi++] = j; + } + } + } + } + *plo = lo; + *phi = hi; + return -1; +} + +/** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper. + * + * This is a dense matrix version. + * + * \return The closest free column index. + */ +int_t find_path_dense( + const uint_t n, cost_t * cost[], const int_t start_i, int_t * y, cost_t * v, int_t * pred) +{ + uint_t lo = 0, hi = 0; + int_t final_j = -1; + uint_t n_ready = 0; + int_t * cols; + cost_t * d; + + NEW(cols, int_t, n); + NEW(d, cost_t, n); + + for (uint_t i = 0; i < n; i++) { + cols[i] = i; + pred[i] = start_i; + d[i] = cost[start_i][i] - v[i]; + } + PRINT_COST_ARRAY(d, n); + while (final_j == -1) { + // No columns left on the SCAN list. + if (lo == hi) { + PRINTF("%d..%d -> find\n", lo, hi); + n_ready = lo; + hi = _find_dense(n, lo, d, cols, y); + PRINTF("check %d..%d\n", lo, hi); + PRINT_INDEX_ARRAY(cols, n); + for (uint_t k = lo; k < hi; k++) { + const int_t j = cols[k]; + if (y[j] < 0) { + final_j = j; + } + } + } + if (final_j == -1) { + PRINTF("%d..%d -> scan\n", lo, hi); + final_j = _scan_dense(n, cost, &lo, &hi, d, cols, pred, y, v); + PRINT_COST_ARRAY(d, n); + PRINT_INDEX_ARRAY(cols, n); + PRINT_INDEX_ARRAY(pred, n); + } + } + + PRINTF("found final_j=%d\n", final_j); + PRINT_INDEX_ARRAY(cols, n); + { + const cost_t mind = d[cols[lo]]; + for (uint_t k = 0; k < n_ready; k++) { + const int_t j = cols[k]; + v[j] += d[j] - mind; + } + } + + FREE(cols); + FREE(d); + + return final_j; +} + +/** Augment for a dense cost matrix. + */ +int_t _ca_dense( + const uint_t n, cost_t * cost[], const uint_t n_free_rows, int_t * free_rows, int_t * x, + int_t * y, cost_t * v) +{ + int_t * pred; + + NEW(pred, int_t, n); + + for (int_t * pfree_i = free_rows; pfree_i < free_rows + n_free_rows; pfree_i++) { + int_t i = -1, j; + uint_t k = 0; + + PRINTF("looking at free_i=%d\n", *pfree_i); + j = find_path_dense(n, cost, *pfree_i, y, v, pred); + ASSERT(j >= 0); + ASSERT(j < n); + while (i != *pfree_i) { + PRINTF("augment %d\n", j); + PRINT_INDEX_ARRAY(pred, n); + i = pred[j]; + PRINTF("y[%d]=%d -> %d\n", j, y[j], i); + y[j] = i; + PRINT_INDEX_ARRAY(x, n); + SWAP_INDICES(j, x[i]); + k++; + if (k >= n) { + ASSERT(FALSE); + } + } + } + FREE(pred); + return 0; +} + +/** Solve dense sparse LAP. + */ +int lapjv_internal(const uint_t n, cost_t * cost[], int_t * x, int_t * y) +{ + int ret; + int_t * free_rows; + cost_t * v; + + NEW(free_rows, int_t, n); + NEW(v, cost_t, n); + ret = _ccrrt_dense(n, cost, free_rows, x, y, v); + int i = 0; + while (ret > 0 && i < 2) { + ret = _carr_dense(n, cost, ret, free_rows, x, y, v); + i++; + } + if (ret > 0) { + ret = _ca_dense(n, cost, ret, free_rows, x, y, v); + } + FREE(v); + FREE(free_rows); + return ret; +} diff --git a/perception/bytetrack/lib/src/strack.cpp b/perception/bytetrack/lib/src/strack.cpp new file mode 100644 index 0000000000000..c57085fe8d248 --- /dev/null +++ b/perception/bytetrack/lib/src/strack.cpp @@ -0,0 +1,222 @@ +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Copyright 2023 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 "strack.h" + +#include + +STrack::STrack(std::vector tlwh_, float score, int label) +{ + _tlwh.resize(4); + _tlwh.assign(tlwh_.begin(), tlwh_.end()); + + is_activated = false; + track_id = 0; + state = TrackState::New; + + tlwh.resize(4); + tlbr.resize(4); + + static_tlwh(); + static_tlbr(); + frame_id = 0; + tracklet_len = 0; + this->score = score; + start_frame = 0; + this->label = label; +} + +STrack::~STrack() {} + +void STrack::activate(byte_kalman::KalmanFilter & kalman_filter, int frame_id) +{ + this->kalman_filter = kalman_filter; + this->track_id = this->next_id(); + this->unique_id = boost::uuids::random_generator()(); + + std::vector _tlwh_tmp(4); + _tlwh_tmp[0] = this->_tlwh[0]; + _tlwh_tmp[1] = this->_tlwh[1]; + _tlwh_tmp[2] = this->_tlwh[2]; + _tlwh_tmp[3] = this->_tlwh[3]; + std::vector xyah = tlwh_to_xyah(_tlwh_tmp); + DETECTBOX xyah_box; + xyah_box[0] = xyah[0]; + xyah_box[1] = xyah[1]; + xyah_box[2] = xyah[2]; + xyah_box[3] = xyah[3]; + auto mc = this->kalman_filter.initiate(xyah_box); + this->mean = mc.first; + this->covariance = mc.second; + + static_tlwh(); + static_tlbr(); + + this->tracklet_len = 0; + this->state = TrackState::Tracked; + // if (frame_id == 1) + // { + // this->is_activated = true; + // } + this->is_activated = true; + this->frame_id = frame_id; + this->start_frame = frame_id; +} + +void STrack::re_activate(STrack & new_track, int frame_id, bool new_id) +{ + std::vector xyah = tlwh_to_xyah(new_track.tlwh); + DETECTBOX xyah_box; + xyah_box[0] = xyah[0]; + xyah_box[1] = xyah[1]; + xyah_box[2] = xyah[2]; + xyah_box[3] = xyah[3]; + auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box); + this->mean = mc.first; + this->covariance = mc.second; + + static_tlwh(); + static_tlbr(); + + this->tracklet_len = 0; + this->state = TrackState::Tracked; + this->is_activated = true; + this->frame_id = frame_id; + this->score = new_track.score; + if (new_id) { + this->track_id = next_id(); + this->unique_id = boost::uuids::random_generator()(); + } +} + +void STrack::update(STrack & new_track, int frame_id) +{ + this->frame_id = frame_id; + this->tracklet_len++; + + std::vector xyah = tlwh_to_xyah(new_track.tlwh); + DETECTBOX xyah_box; + xyah_box[0] = xyah[0]; + xyah_box[1] = xyah[1]; + xyah_box[2] = xyah[2]; + xyah_box[3] = xyah[3]; + + auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box); + this->mean = mc.first; + this->covariance = mc.second; + + static_tlwh(); + static_tlbr(); + + this->state = TrackState::Tracked; + this->is_activated = true; + + this->score = new_track.score; +} + +void STrack::static_tlwh() +{ + if (this->state == TrackState::New) { + tlwh[0] = _tlwh[0]; + tlwh[1] = _tlwh[1]; + tlwh[2] = _tlwh[2]; + tlwh[3] = _tlwh[3]; + return; + } + + tlwh[0] = mean[0]; + tlwh[1] = mean[1]; + tlwh[2] = mean[2]; + tlwh[3] = mean[3]; + + tlwh[2] *= tlwh[3]; + tlwh[0] -= tlwh[2] / 2; + tlwh[1] -= tlwh[3] / 2; +} + +void STrack::static_tlbr() +{ + tlbr.clear(); + tlbr.assign(tlwh.begin(), tlwh.end()); + tlbr[2] += tlbr[0]; + tlbr[3] += tlbr[1]; +} + +std::vector STrack::tlwh_to_xyah(std::vector tlwh_tmp) +{ + std::vector tlwh_output = tlwh_tmp; + tlwh_output[0] += tlwh_output[2] / 2; + tlwh_output[1] += tlwh_output[3] / 2; + tlwh_output[2] /= tlwh_output[3]; + return tlwh_output; +} + +std::vector STrack::to_xyah() { return tlwh_to_xyah(tlwh); } + +std::vector STrack::tlbr_to_tlwh(std::vector & tlbr) +{ + tlbr[2] -= tlbr[0]; + tlbr[3] -= tlbr[1]; + return tlbr; +} + +void STrack::mark_lost() { state = TrackState::Lost; } + +void STrack::mark_removed() { state = TrackState::Removed; } + +int STrack::next_id() +{ + static int _count = 0; + _count++; + return _count; +} + +int STrack::end_frame() { return this->frame_id; } + +void STrack::multi_predict( + std::vector & stracks, byte_kalman::KalmanFilter & kalman_filter) +{ + for (size_t i = 0; i < stracks.size(); i++) { + if (stracks[i]->state != TrackState::Tracked) { + stracks[i]->mean[7] = 0; + } + kalman_filter.predict(stracks[i]->mean, stracks[i]->covariance); + stracks[i]->static_tlwh(); + stracks[i]->static_tlbr(); + } +} diff --git a/perception/bytetrack/lib/src/utils.cpp b/perception/bytetrack/lib/src/utils.cpp new file mode 100644 index 0000000000000..64ad27cf36eba --- /dev/null +++ b/perception/bytetrack/lib/src/utils.cpp @@ -0,0 +1,399 @@ +/* + * MIT License + * + * Copyright (c) 2021 Yifu Zhang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Copyright 2023 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 "byte_tracker.h" +#include "lapjv.h" + +#include + +std::vector ByteTracker::joint_stracks( + std::vector & tlista, std::vector & tlistb) +{ + std::map exists; + std::vector res; + for (size_t i = 0; i < tlista.size(); i++) { + exists.insert(std::pair(tlista[i]->track_id, 1)); + res.push_back(tlista[i]); + } + for (size_t i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (!exists[tid] || exists.count(tid) == 0) { + exists[tid] = 1; + res.push_back(&tlistb[i]); + } + } + return res; +} + +std::vector ByteTracker::joint_stracks( + std::vector & tlista, std::vector & tlistb) +{ + std::map exists; + std::vector res; + for (size_t i = 0; i < tlista.size(); i++) { + exists.insert(std::pair(tlista[i].track_id, 1)); + res.push_back(tlista[i]); + } + for (size_t i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (!exists[tid] || exists.count(tid) == 0) { + exists[tid] = 1; + res.push_back(tlistb[i]); + } + } + return res; +} + +std::vector ByteTracker::sub_stracks( + std::vector & tlista, std::vector & tlistb) +{ + std::map stracks; + for (size_t i = 0; i < tlista.size(); i++) { + stracks.insert(std::pair(tlista[i].track_id, tlista[i])); + } + for (size_t i = 0; i < tlistb.size(); i++) { + int tid = tlistb[i].track_id; + if (stracks.count(tid) != 0) { + stracks.erase(tid); + } + } + + std::vector res; + std::map::iterator it; + for (it = stracks.begin(); it != stracks.end(); ++it) { + res.push_back(it->second); + } + + return res; +} + +void ByteTracker::remove_duplicate_stracks( + std::vector & resa, std::vector & resb, std::vector & stracksa, + std::vector & stracksb) +{ + std::vector> pdist = iou_distance(stracksa, stracksb); + std::vector> pairs; + for (size_t i = 0; i < pdist.size(); i++) { + for (size_t j = 0; j < pdist[i].size(); j++) { + if (pdist[i][j] < 0.15) { + pairs.push_back(std::pair(i, j)); + } + } + } + + std::vector dupa, dupb; + for (size_t i = 0; i < pairs.size(); i++) { + int timep = stracksa[pairs[i].first].frame_id - stracksa[pairs[i].first].start_frame; + int timeq = stracksb[pairs[i].second].frame_id - stracksb[pairs[i].second].start_frame; + if (timep > timeq) + dupb.push_back(pairs[i].second); + else + dupa.push_back(pairs[i].first); + } + + for (size_t i = 0; i < stracksa.size(); i++) { + std::vector::iterator iter = find(dupa.begin(), dupa.end(), i); + if (iter == dupa.end()) { + resa.push_back(stracksa[i]); + } + } + + for (size_t i = 0; i < stracksb.size(); i++) { + std::vector::iterator iter = find(dupb.begin(), dupb.end(), i); + if (iter == dupb.end()) { + resb.push_back(stracksb[i]); + } + } +} + +void ByteTracker::linear_assignment( + std::vector> & cost_matrix, int cost_matrix_size, int cost_matrix_size_size, + float thresh, std::vector> & matches, std::vector & unmatched_a, + std::vector & unmatched_b) +{ + if (cost_matrix.size() == 0) { + for (int i = 0; i < cost_matrix_size; i++) { + unmatched_a.push_back(i); + } + for (int i = 0; i < cost_matrix_size_size; i++) { + unmatched_b.push_back(i); + } + return; + } + + std::vector rowsol; + std::vector colsol; + [[maybe_unused]] float c = lapjv(cost_matrix, rowsol, colsol, true, thresh); + for (size_t i = 0; i < rowsol.size(); i++) { + if (rowsol[i] >= 0) { + std::vector match; + match.push_back(i); + match.push_back(rowsol[i]); + matches.push_back(match); + } else { + unmatched_a.push_back(i); + } + } + + for (size_t i = 0; i < colsol.size(); i++) { + if (colsol[i] < 0) { + unmatched_b.push_back(i); + } + } +} + +std::vector> ByteTracker::ious( + std::vector> & atlbrs, std::vector> & btlbrs) +{ + std::vector> ious; + if (atlbrs.size() * btlbrs.size() == 0) return ious; + + ious.resize(atlbrs.size()); + for (size_t i = 0; i < ious.size(); i++) { + ious[i].resize(btlbrs.size()); + } + + // bbox_ious + for (size_t k = 0; k < btlbrs.size(); k++) { + std::vector ious_tmp; + float box_area = (btlbrs[k][2] - btlbrs[k][0] + 1) * (btlbrs[k][3] - btlbrs[k][1] + 1); + for (size_t n = 0; n < atlbrs.size(); n++) { + float iw = std::min(atlbrs[n][2], btlbrs[k][2]) - std::max(atlbrs[n][0], btlbrs[k][0]) + 1; + if (iw > 0) { + float ih = std::min(atlbrs[n][3], btlbrs[k][3]) - std::max(atlbrs[n][1], btlbrs[k][1]) + 1; + if (ih > 0) { + float ua = (atlbrs[n][2] - atlbrs[n][0] + 1) * (atlbrs[n][3] - atlbrs[n][1] + 1) + + box_area - iw * ih; + ious[n][k] = iw * ih / ua; + } else { + ious[n][k] = 0.0; + } + } else { + ious[n][k] = 0.0; + } + } + } + + return ious; +} + +std::vector> ByteTracker::iou_distance( + std::vector & atracks, std::vector & btracks, int & dist_size, + int & dist_size_size) +{ + std::vector> cost_matrix; + if (atracks.size() * btracks.size() == 0) { + dist_size = atracks.size(); + dist_size_size = btracks.size(); + return cost_matrix; + } + std::vector> atlbrs, btlbrs; + for (size_t i = 0; i < atracks.size(); i++) { + atlbrs.push_back(atracks[i]->tlbr); + } + for (size_t i = 0; i < btracks.size(); i++) { + btlbrs.push_back(btracks[i].tlbr); + } + + dist_size = atracks.size(); + dist_size_size = btracks.size(); + + std::vector> _ious = ious(atlbrs, btlbrs); + + for (size_t i = 0; i < _ious.size(); i++) { + std::vector _iou; + for (size_t j = 0; j < _ious[i].size(); j++) { + _iou.push_back(1 - _ious[i][j]); + } + cost_matrix.push_back(_iou); + } + + return cost_matrix; +} + +std::vector> ByteTracker::iou_distance( + std::vector & atracks, std::vector & btracks) +{ + std::vector> atlbrs, btlbrs; + for (size_t i = 0; i < atracks.size(); i++) { + atlbrs.push_back(atracks[i].tlbr); + } + for (size_t i = 0; i < btracks.size(); i++) { + btlbrs.push_back(btracks[i].tlbr); + } + + std::vector> _ious = ious(atlbrs, btlbrs); + std::vector> cost_matrix; + for (size_t i = 0; i < _ious.size(); i++) { + std::vector _iou; + for (size_t j = 0; j < _ious[i].size(); j++) { + _iou.push_back(1 - _ious[i][j]); + } + cost_matrix.push_back(_iou); + } + + return cost_matrix; +} + +double ByteTracker::lapjv( + const std::vector> & cost, std::vector & rowsol, + std::vector & colsol, bool extend_cost, float cost_limit, bool return_cost) +{ + std::vector> cost_c; + cost_c.assign(cost.begin(), cost.end()); + + std::vector> cost_c_extended; + + int n_rows = cost.size(); + int n_cols = cost[0].size(); + rowsol.resize(n_rows); + colsol.resize(n_cols); + + int n = 0; + if (n_rows == n_cols) { + n = n_rows; + } else { + if (!extend_cost) { + std::cout << "set extend_cost=True" << std::endl; + // system("pause"); + exit(0); + } + } + + if (extend_cost || cost_limit < LONG_MAX) { + n = n_rows + n_cols; + cost_c_extended.resize(n); + for (size_t i = 0; i < cost_c_extended.size(); i++) cost_c_extended[i].resize(n); + + if (cost_limit < LONG_MAX) { + for (size_t i = 0; i < cost_c_extended.size(); i++) { + for (size_t j = 0; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = cost_limit / 2.0; + } + } + } else { + float cost_max = -1; + for (size_t i = 0; i < cost_c.size(); i++) { + for (size_t j = 0; j < cost_c[i].size(); j++) { + if (cost_c[i][j] > cost_max) cost_max = cost_c[i][j]; + } + } + for (size_t i = 0; i < cost_c_extended.size(); i++) { + for (size_t j = 0; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = cost_max + 1; + } + } + } + + for (size_t i = n_rows; i < cost_c_extended.size(); i++) { + for (size_t j = n_cols; j < cost_c_extended[i].size(); j++) { + cost_c_extended[i][j] = 0; + } + } + for (int i = 0; i < n_rows; i++) { + for (int j = 0; j < n_cols; j++) { + cost_c_extended[i][j] = cost_c[i][j]; + } + } + + cost_c.clear(); + cost_c.assign(cost_c_extended.begin(), cost_c_extended.end()); + } + + double ** cost_ptr; + cost_ptr = new double *[sizeof(double *) * n]; + for (int i = 0; i < n; i++) cost_ptr[i] = new double[sizeof(double) * n]; + + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + cost_ptr[i][j] = cost_c[i][j]; + } + } + + int * x_c = new int[sizeof(int) * n]; + int * y_c = new int[sizeof(int) * n]; + + int ret = lapjv_internal(n, cost_ptr, x_c, y_c); + if (ret != 0) { + std::cout << "Calculate Wrong!" << std::endl; + // system("pause"); + exit(0); + } + + double opt = 0.0; + + if (n != n_rows) { + for (int i = 0; i < n; i++) { + if (x_c[i] >= n_cols) x_c[i] = -1; + if (y_c[i] >= n_rows) y_c[i] = -1; + } + for (int i = 0; i < n_rows; i++) { + rowsol[i] = x_c[i]; + } + for (int i = 0; i < n_cols; i++) { + colsol[i] = y_c[i]; + } + + if (return_cost) { + for (size_t i = 0; i < rowsol.size(); i++) { + if (rowsol[i] != -1) { + opt += cost_ptr[i][rowsol[i]]; + } + } + } + } else if (return_cost) { + for (size_t i = 0; i < rowsol.size(); i++) { + opt += cost_ptr[i][rowsol[i]]; + } + } + + for (int i = 0; i < n; i++) { + delete[] cost_ptr[i]; + } + delete[] cost_ptr; + delete[] x_c; + delete[] y_c; + + return opt; +} + +cv::Scalar ByteTracker::get_color(int idx) +{ + idx += 3; + return cv::Scalar(37 * idx % 255, 17 * idx % 255, 29 * idx % 255); +} diff --git a/perception/bytetrack/package.xml b/perception/bytetrack/package.xml new file mode 100644 index 0000000000000..526ee210dabf9 --- /dev/null +++ b/perception/bytetrack/package.xml @@ -0,0 +1,35 @@ + + + + bytetrack + 0.0.1 + ByteTrack implementation ported toward Autoware + Manato HIRABAYASHI + Apache License 2.0 + + ament_cmake_auto + cudnn_cmake_module + + cudnn_cmake_module + tensorrt_cmake_module + + autoware_cmake + + autoware_auto_perception_msgs + cuda_utils + cv_bridge + image_transport + libopencv-dev + rclcpp + rclcpp_components + sensor_msgs + tensorrt_common + tier4_perception_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/bytetrack/src/bytetrack.cpp b/perception/bytetrack/src/bytetrack.cpp new file mode 100644 index 0000000000000..981bdb7f24afc --- /dev/null +++ b/perception/bytetrack/src/bytetrack.cpp @@ -0,0 +1,68 @@ +// Copyright 2023 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 + +#include +#include +#include + +namespace bytetrack +{ +ByteTrack::ByteTrack(const int track_buffer_length) +{ + // Tracker initialization + tracker_ = std::make_unique(track_buffer_length); +} + +bool ByteTrack::do_inference(ObjectArray & objects) +{ + // Re-format data + std::vector bytetrack_objects; + for (auto & obj : objects) { + ByteTrackObject bytetrack_obj; + bytetrack_obj.rect = cv::Rect(obj.x_offset, obj.y_offset, obj.width, obj.height); + bytetrack_obj.prob = obj.score; + bytetrack_obj.label = obj.type; + bytetrack_objects.emplace_back(bytetrack_obj); + } + + // Update tracker + std::vector output_stracks = tracker_->update(bytetrack_objects); + + // Pack results + latest_objects_.clear(); + for (const auto & tracking_result : output_stracks) { + Object object{}; + std::vector tlwh = tracking_result.tlwh; + object.x_offset = tlwh[0]; + object.y_offset = tlwh[1]; + object.width = tlwh[2]; + object.height = tlwh[3]; + object.score = tracking_result.score; + object.type = tracking_result.label; + object.track_id = tracking_result.track_id; + object.unique_id = tracking_result.unique_id; + latest_objects_.emplace_back(object); + } + + return true; +} + +ObjectArray ByteTrack::update_tracker(ObjectArray & input_objects) +{ + do_inference(input_objects); + return latest_objects_; +} +} // namespace bytetrack diff --git a/perception/bytetrack/src/bytetrack_node.cpp b/perception/bytetrack/src/bytetrack_node.cpp new file mode 100644 index 0000000000000..358a5053b5d4a --- /dev/null +++ b/perception/bytetrack/src/bytetrack_node.cpp @@ -0,0 +1,113 @@ +// Copyright 2023 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 "bytetrack/bytetrack.hpp" + +#include +#include + +#include "autoware_auto_perception_msgs/msg/object_classification.hpp" + +#include + +#include +#include + +namespace bytetrack +{ +ByteTrackNode::ByteTrackNode(const rclcpp::NodeOptions & node_options) +: Node("bytetrack", node_options) +{ + using std::placeholders::_1; + using std::chrono_literals::operator""ms; + + int track_buffer_length = declare_parameter("track_buffer_length", 30); + + this->bytetrack_ = std::make_unique(track_buffer_length); + + timer_ = + rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&ByteTrackNode::on_connect, this)); + + objects_pub_ = this->create_publisher( + "~/out/objects", 1); + objects_uuid_pub_ = this->create_publisher( + "~/out/objects/debug/uuid", 1); +} + +void ByteTrackNode::on_connect() +{ + using std::placeholders::_1; + if ( + objects_pub_->get_subscription_count() == 0 && + objects_pub_->get_intra_process_subscription_count() == 0) { + detection_rect_sub_.reset(); + } else if (!detection_rect_sub_) { + detection_rect_sub_ = + this->create_subscription( + "~/in/rect", 1, std::bind(&ByteTrackNode::on_rect, this, _1)); + } +} + +void ByteTrackNode::on_rect( + const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr msg) +{ + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + + tier4_perception_msgs::msg::DetectedObjectsWithFeature out_objects; + tier4_perception_msgs::msg::DynamicObjectArray out_objects_uuid; + + // Unpack detection results + ObjectArray object_array; + for (auto & feat_obj : msg->feature_objects) { + Object obj; + obj.x_offset = feat_obj.feature.roi.x_offset; + obj.y_offset = feat_obj.feature.roi.y_offset; + obj.height = feat_obj.feature.roi.height; + obj.width = feat_obj.feature.roi.width; + obj.score = feat_obj.object.existence_probability; + obj.type = feat_obj.object.classification.front().label; + object_array.emplace_back(obj); + } + + bytetrack::ObjectArray objects = bytetrack_->update_tracker(object_array); + for (const auto & tracked_object : objects) { + tier4_perception_msgs::msg::DetectedObjectWithFeature object; + object.feature.roi.x_offset = tracked_object.x_offset; + object.feature.roi.y_offset = tracked_object.y_offset; + object.feature.roi.width = tracked_object.width; + object.feature.roi.height = tracked_object.height; + object.object.existence_probability = tracked_object.score; + object.object.classification.emplace_back( + autoware_auto_perception_msgs::build