diff --git a/.github/workflows/gtest.yml b/.github/workflows/gtest.yml index 85289b6..5e143f7 100644 --- a/.github/workflows/gtest.yml +++ b/.github/workflows/gtest.yml @@ -2,10 +2,18 @@ name: Run Test on: push: - branches: [ main ] pull_request: workflow_dispatch: +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + +env: + RMCS_REPO: https://github.com/Alliance-Algorithm/RMCS.git + RMCS_REF: main + HIKCAMERA_REPO: https://github.com/Alliance-Algorithm/ros2-hikcamera.git + jobs: test: runs-on: ubuntu-latest @@ -14,45 +22,79 @@ jobs: options: --user 0 env: - WS_DIR: ${{ github.workspace }}/ws - SRC_DIR: ${{ github.workspace }}/ws/src TEST_ASSETS_ROOT: /tmp/auto_aim steps: - - name: Checkout repository into ROS2 workspace + - name: Checkout rmcs_auto_aim_v2 uses: actions/checkout@v4 with: - repository: Alliance-Algorithm/rmcs_auto_aim_v2 - path: ws/src/rmcs_auto_aim_v2 + path: rmcs_auto_aim_v2 + + - name: Clone RMCS workspace + shell: bash + run: | + set -euo pipefail + git clone --depth 1 --recurse-submodules --branch "${RMCS_REF}" "${RMCS_REPO}" /tmp/RMCS + git clone --depth 1 "${HIKCAMERA_REPO}" /tmp/RMCS/rmcs_ws/src/hikcamera + mkdir -p /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2 + tar -C "${GITHUB_WORKSPACE}/rmcs_auto_aim_v2" --exclude=".git" -cf - . \ + | tar -C /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2 -xf - - name: Install yq dependency run: | apt-get update apt-get install -y yq + - name: Build RMCS workspace + shell: bash + working-directory: /tmp/RMCS/rmcs_ws + run: | + set -euo pipefail + set +u + source /opt/ros/jazzy/setup.bash + set -u + colcon build --packages-up-to rmcs_auto_aim_v2 + - name: Download test assets shell: bash run: | - cd "$SRC_DIR/rmcs_auto_aim_v2/test" + set -euo pipefail + set +u + source /opt/ros/jazzy/setup.bash + source /tmp/RMCS/rmcs_ws/install/setup.bash + set -u + cd /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2/test TEST_ASSETS_ROOT="$TEST_ASSETS_ROOT" ./download_assets.sh - name: Configure test project shell: bash run: | + set -euo pipefail + set +u source /opt/ros/jazzy/setup.bash - cd "$SRC_DIR/rmcs_auto_aim_v2/test" + source /tmp/RMCS/rmcs_ws/install/setup.bash + set -u + cd /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2/test cmake -S . -B build -DCMAKE_BUILD_TYPE=Release - name: Build test project shell: bash run: | + set -euo pipefail + set +u source /opt/ros/jazzy/setup.bash - cd "$SRC_DIR/rmcs_auto_aim_v2/test" + source /tmp/RMCS/rmcs_ws/install/setup.bash + set -u + cd /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2/test cmake --build build --parallel - name: Run gtests shell: bash run: | + set -euo pipefail + set +u source /opt/ros/jazzy/setup.bash - cd "$SRC_DIR/rmcs_auto_aim_v2/test" + source /tmp/RMCS/rmcs_ws/install/setup.bash + set -u + cd /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2/test ctest --test-dir build --output-on-failure diff --git a/CMakeLists.txt b/CMakeLists.txt index 7630051..26598de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,6 +32,7 @@ find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) find_package(visualization_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(rmcs_msgs REQUIRED) find_package(rmcs_executor REQUIRED) find_package(rmcs_description REQUIRED) @@ -47,10 +48,12 @@ include_directories( ${ament_index_cpp_INCLUDE_DIRS} ${visualization_msgs_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} + ${rmcs_msgs_INCLUDE_DIRS} ${rmcs_executor_INCLUDE_DIRS} ${rmcs_description_INCLUDE_DIRS} ${fast_tf_INCLUDE_DIRS} + ${hikcamera_INCLUDE_DIRS} ) ## 代码资源搜索 diff --git a/config/config.yaml b/config/config.yaml index 1ab56fc..4dabc4f 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -76,14 +76,14 @@ pose_estimator: [-0.064232403853946, -0.087667493884102, 0, 0, 0.792381808294582] fire_control: - initial_bullet_speed: 24.2 # m/s - shoot_delay: 0.1 # s + initial_bullet_speed: 21.4 # m/s + shoot_delay: 0.07 # s mpc_enable: true yaw_offset: 0.0 # degree pitch_offset: 0.0 # degree aim_point_chooser: - coming_angle: 60.0 # degree + coming_angle: 70.0 # degree leaving_angle: 20.0 # degree outpost_coming_angle: 70.0 # degree outpost_leaving_angle: 30.0 # degree diff --git a/package.xml b/package.xml index 5534fd1..c986fdd 100644 --- a/package.xml +++ b/package.xml @@ -15,6 +15,9 @@ rmcs_executor + rmcs_description + fast_tf + rmcs_msgs hikcamera diff --git a/src/component.cpp b/src/component.cpp index 4a8fd68..4481a56 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -4,7 +4,7 @@ #pragma GCC diagnostic ignored "-Wdeprecated-declarations" #endif -#include "adapter/sentry.hpp" +#include "adapter/adapter.hpp" #include "kernel/feishu.hpp" #include "utility/rclcpp/node.hpp" #include "utility/shared/context.hpp" @@ -41,7 +41,7 @@ class AutoAimComponent final : public rmcs_executor::Component { InputInterface robot_id; auto make_context() const { - auto context = SystemContext { }; + auto context = SystemContext {}; context.timestamp = Clock::now(); diff --git a/src/kernel/fire_control.cpp b/src/kernel/fire_control.cpp index b091aac..cad394a 100644 --- a/src/kernel/fire_control.cpp +++ b/src/kernel/fire_control.cpp @@ -29,11 +29,16 @@ struct FireControl::Impl { double pitch_offset; constexpr static std::tuple metas { - &Config::initial_bullet_speed, "initial_bullet_speed", - &Config::shoot_delay, "shoot_delay", - &Config::mpc_enable, "mpc_enable", - &Config::yaw_offset, "yaw_offset", - &Config::pitch_offset, "pitch_offset", + &Config::initial_bullet_speed, + "initial_bullet_speed", + &Config::shoot_delay, + "shoot_delay", + &Config::mpc_enable, + "mpc_enable", + &Config::yaw_offset, + "yaw_offset", + &Config::pitch_offset, + "pitch_offset", }; } config; @@ -89,9 +94,8 @@ struct FireControl::Impl { auto yaw_acc = std::numeric_limits::quiet_NaN(); auto feedforward_valid = false; - if (config.mpc_enable) { - auto sample_attitude = - [&](TimePoint t) -> std::expected { + if (config.mpc_enable && snapshot.device_id() != DeviceId::OUTPOST) { + auto sample_attitude = [&](TimePoint t) -> std::expected { auto sample = AimPointSampler::sample_at( snapshot, aim_point_chooser, t, config.initial_bullet_speed); if (!sample.has_value()) return std::unexpected { sample.error() }; @@ -111,11 +115,11 @@ struct FireControl::Impl { yaw_acc = planned->yaw_acc; feedforward_valid = true; } else { - constexpr int kYawRow = 0; - constexpr int kPitchRow = 2; + constexpr int kYawRow = 0; + constexpr int kPitchRow = 2; constexpr int kCenterCol = kMpcAxisHorizon / 2; - pitch = (*reference)(kPitchRow, kCenterCol); - yaw = util::normalize_angle((*reference)(kYawRow, kCenterCol)); + pitch = (*reference)(kPitchRow, kCenterCol); + yaw = util::normalize_angle((*reference)(kYawRow, kCenterCol)); } } } diff --git a/src/module/fire_control/aim_point_chooser.cpp b/src/module/fire_control/aim_point_chooser.cpp index beea8d0..237ca71 100644 --- a/src/module/fire_control/aim_point_chooser.cpp +++ b/src/module/fire_control/aim_point_chooser.cpp @@ -57,7 +57,8 @@ struct AimPointChooser::Impl { if (!(config.coming_angle > 0.0) || !(config.leaving_angle > 0.0) || !(config.outpost_coming_angle > 0.0) || !(config.outpost_leaving_angle > 0.0)) { return std::unexpected { - "coming_angle, leaving_angle, outpost_coming_angle and outpost_leaving_angle must be > 0", + "coming_angle, leaving_angle, outpost_coming_angle and outpost_leaving_angle must " + "be > 0", }; } @@ -95,7 +96,7 @@ struct AimPointChooser::Impl { }; for (size_t index = 0; index < armors.size(); ++index) { - const auto delta_yaw = util::normalize_angle(yaw(index) - center_yaw); + const auto delta_yaw = util::normalize_angle(yaw(index) - center_yaw); candidate_evals[index] = { .delta_yaw = delta_yaw, .in_window = in_window(delta_yaw), @@ -103,11 +104,17 @@ struct AimPointChooser::Impl { } const auto priority_key = [&](size_t index) { - const auto abs_delta = std::abs(candidate_evals[index].delta_yaw); - const auto id = armors[index].id; - const auto is_last = last_chosen_armor_id.has_value() && (id == *last_chosen_armor_id); + const auto preferred_incoming = [&] { + const auto delta = candidate_evals[index].delta_yaw; + if (angular_velocity > 0.0) return (delta > 0.0) ? 1 : 0; + if (angular_velocity < 0.0) return (delta < 0.0) ? 1 : 0; + return 0; + }(); + const auto abs_delta = std::abs(candidate_evals[index].delta_yaw); + const auto id = armors[index].id; + const auto is_last = last_chosen_armor_id.has_value() && (id == *last_chosen_armor_id); const auto last_penalty = is_last ? 0 : 1; - return std::tuple { abs_delta, last_penalty, id, index }; + return std::tuple { preferred_incoming, abs_delta, last_penalty, id, index }; }; auto best_idx = std::optional {}; diff --git a/src/module/fire_control/solver/aim_point_sampling.cpp b/src/module/fire_control/solver/aim_point_sampling.cpp index d1ed612..7bca483 100644 --- a/src/module/fire_control/solver/aim_point_sampling.cpp +++ b/src/module/fire_control/solver/aim_point_sampling.cpp @@ -12,27 +12,27 @@ auto rmcs::fire_control::AimPointSampler::sample_at( -> std::expected { auto aim_point = sample_aim_point_at(snapshot, chooser, t); if (!aim_point.has_value()) { - return std::unexpected { "aim point sample failed" }; + return std::unexpected { aim_point.error() }; } - auto attitude = solve_aim_attitude(*aim_point, bullet_speed); + auto attitude = solve_aim_attitude(aim_point.value(), bullet_speed); if (!attitude.has_value()) { return std::unexpected { attitude.error() }; } return AimSample { .attitude = *attitude, - .aim_point = *aim_point, + .aim_point = aim_point.value(), }; } auto rmcs::fire_control::AimPointSampler::sample_aim_point_at(predictor::Snapshot const& snapshot, - AimPointChooser& chooser, TimePoint t) -> std::optional { + AimPointChooser& chooser, TimePoint t) -> std::expected { auto predicted_armors = snapshot.predicted_armors(t); auto predicted_kinematics = snapshot.kinematics_at(t); auto chosen_armor = chooser.choose_armor(predicted_armors, predicted_kinematics.center_position, predicted_kinematics.angular_velocity); - if (!chosen_armor.has_value()) return std::nullopt; + if (!chosen_armor.has_value()) return std::unexpected { "choose_armor returned nullopt" }; auto aim_point = Eigen::Vector3d {}; chosen_armor->translation.copy_to(aim_point); diff --git a/src/module/fire_control/solver/aim_point_sampling.hpp b/src/module/fire_control/solver/aim_point_sampling.hpp index fdaa8a6..c2cec11 100644 --- a/src/module/fire_control/solver/aim_point_sampling.hpp +++ b/src/module/fire_control/solver/aim_point_sampling.hpp @@ -29,7 +29,7 @@ class AimPointSampler { private: static auto sample_aim_point_at(predictor::Snapshot const& snapshot, AimPointChooser& chooser, - TimePoint t) -> std::optional; + TimePoint t) -> std::expected; static auto solve_aim_attitude(Eigen::Vector3d const& aim_point, double bullet_speed) -> std::expected; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0385080..5574edd 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp REQUIRED) find_package(visualization_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(rmcs_msgs REQUIRED) find_package(yaml-cpp REQUIRED) find_package(OpenVINO REQUIRED) @@ -38,6 +39,7 @@ include_directories( ${rclcpp_INCLUDE_DIRS} ${visualization_msgs_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} + ${rmcs_msgs_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) @@ -143,12 +145,6 @@ ament_add_gtest( ${TEST_DIR}/transform_communication.cpp ) -# Feishu IPC -ament_add_gtest( - test_feishu - ${TEST_DIR}/feishu_test.cpp -) - # Aim point chooser ament_add_gtest( test_aim_point_chooser diff --git a/test/aim_point_chooser.cpp b/test/aim_point_chooser.cpp index 3f321e1..0501e88 100644 --- a/test/aim_point_chooser.cpp +++ b/test/aim_point_chooser.cpp @@ -106,3 +106,21 @@ TEST(AimPointChooser, SingleArmorHighSpeedScanBySpinDirection) { } } } + +TEST(AimPointChooser, PreferIncomingArmorWhenSpinPositive) { + auto const armors = make_armors({ -30.0, 5.0 }); + auto const chosen = choose_once(armors, 0.0, kFastPositive); + EXPECT_EQ(chosen, std::optional { 0 }); +} + +TEST(AimPointChooser, PreferIncomingArmorWhenSpinNegative) { + auto const armors = make_armors({ -5.0, 30.0 }); + auto const chosen = choose_once(armors, 0.0, kFastNegative); + EXPECT_EQ(chosen, std::optional { 1 }); +} + +TEST(AimPointChooser, KeepAbsoluteDeltaPriorityWhenSpinZero) { + auto const armors = make_armors({ -15.0, 5.0 }); + auto const chosen = choose_once(armors, 0.0, 0.0); + EXPECT_EQ(chosen, std::optional { 1 }); +} diff --git a/test/feishu_test.cpp b/test/feishu_test.cpp deleted file mode 100644 index beb300a..0000000 --- a/test/feishu_test.cpp +++ /dev/null @@ -1,72 +0,0 @@ -#include - -#include -#include -#include -#include - -#include "kernel/feishu.hpp" -#include "utility/shared/context.hpp" - -using namespace std::chrono_literals; -using namespace rmcs; -using namespace rmcs::kernel; - -TEST(FeishuIntegration, BidirectionalCommunication) { - const auto pid = ::fork(); - ASSERT_NE(pid, -1); - - if (pid == 0) { - auto feishu_child = Feishu { }; - std::this_thread::sleep_for(50ms); - - auto deadline = Clock::now() + 500ms; - auto ctrl = std::optional { }; - while (!ctrl && Clock::now() < deadline) { - if (feishu_child.heartbeat()) { - ctrl = feishu_child.latest(); - } else { - std::this_thread::sleep_for(10ms); - } - } - ASSERT_TRUE(ctrl.has_value()); - - auto auto_state = AutoAimState { }; - auto_state.should_control = true; - auto_state.should_shoot = true; - auto_state.yaw = 1.23; - - feishu_child.send(auto_state); - exit(0); - } - - auto feishu_parent = Feishu { }; - std::this_thread::sleep_for(50ms); - - feishu_parent.heartbeat(); - - auto ctrl = SystemContext { }; - ctrl.timestamp = Clock::now(); - - feishu_parent.send(ctrl); - - auto deadline = Clock::now() + 500ms; - auto auto_state = std::optional { }; - while (!auto_state && Clock::now() < deadline) { - if (feishu_parent.heartbeat()) { - auto_state = feishu_parent.latest(); - } else { - std::this_thread::sleep_for(10ms); - } - } - - ASSERT_TRUE(auto_state.has_value()); - EXPECT_TRUE(auto_state->should_control); - EXPECT_TRUE(auto_state->should_shoot); - EXPECT_DOUBLE_EQ(auto_state->yaw, 1.23); - - int status = 0; - ASSERT_EQ(::waitpid(pid, &status, 0), pid); - ASSERT_TRUE(WIFEXITED(status)); - EXPECT_EQ(WEXITSTATUS(status), 0); -}