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_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/control/autonomous_emergency_braking/CMakeLists.txt b/control/autonomous_emergency_braking/CMakeLists.txt index 6027aeac76333..53a629fafa0cc 100644 --- a/control/autonomous_emergency_braking/CMakeLists.txt +++ b/control/autonomous_emergency_braking/CMakeLists.txt @@ -4,6 +4,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 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/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/package.xml b/control/autonomous_emergency_braking/package.xml index 198afb1018e31..e4e7da168b239 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -4,7 +4,8 @@ autonomous_emergency_braking 0.1.0 Autonomous Emergency Braking package as a ROS2 node - yutaka + Yutaka Shimizu + Takamasa Horibe Apache License 2.0 ament_cmake diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 77ea7e6318820..92ede8b3a9c05 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -216,7 +216,7 @@ bool AEB::isDataReady() } if (use_imu_path_ && !imu_ptr_) { - return missing("object pointcloud"); + return missing("imu"); } if (use_predicted_trajectory_ && !predicted_traj_ptr_) { 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 c6a920e347cab..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" 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/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index fb4353d142ee0..107bca1274e48 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -273,7 +273,7 @@ bool MPC::getData( data->nearest_idx = static_cast(nearest_idx); data->steer = static_cast(current_steer.steering_tire_angle); data->lateral_err = MPCUtils::calcLateralError(current_pose, data->nearest_pose); - data->yaw_err = autoware::common::helper_functions::wrap_angle( + data->yaw_err = tier4_autoware_utils::normalizeRadian( tf2::getYaw(current_pose.orientation) - tf2::getYaw(data->nearest_pose.orientation)); /* get predicted steer */ @@ -284,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", diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index 41b15f17bf075..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; } 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/evaluator/diagnostic_converter/src/converter_node.cpp b/evaluator/diagnostic_converter/src/converter_node.cpp index 798515b8667bd..e61b19833d2fe 100644 --- a/evaluator/diagnostic_converter/src/converter_node.cpp +++ b/evaluator/diagnostic_converter/src/converter_node.cpp @@ -14,6 +14,37 @@ #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) @@ -41,8 +72,8 @@ void DiagnosticConverter::onDiagnostic( 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) { - getPublisher(status_topic + "_" + key_value.key, diag_idx) - ->publish(createUserDefinedValue(key_value)); + const auto valid_topic_name = removeInvalidTopicString(status_topic + "_" + key_value.key); + getPublisher(valid_topic_name, diag_idx)->publish(createUserDefinedValue(key_value)); } } } @@ -51,7 +82,7 @@ UserDefinedValue DiagnosticConverter::createUserDefinedValue(const KeyValue & ke { UserDefinedValue param_msg; param_msg.type.data = UserDefinedValueType::DOUBLE; - param_msg.value = key_value.value; + param_msg.value = removeUnitString(key_value.value); return param_msg; } diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 6821388e174d4..63f3523503219 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -199,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")}], ) @@ -338,6 +343,7 @@ def add_launch_arg(name: str, default_value=None, description=None): 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 12ad512557cb1..210e5d049727c 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -6,6 +6,7 @@ + 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/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_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 70b87921ecca5..eb5fee271d662 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -109,7 +109,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/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