diff --git a/src/proto/parameters.proto b/src/proto/parameters.proto index 9ef2df8253..bd4cc23aa9 100644 --- a/src/proto/parameters.proto +++ b/src/proto/parameters.proto @@ -413,12 +413,19 @@ message ShootOrPassPlayConfig message RobotCapabilitiesConfig { - // Comma-separated list of numbers of robots with broken dribblers - required string broken_dribblers = 1; - // Comma-separated list of numbers of robots with broken chippers - required string broken_chippers = 2; - // Comma-separated list of numbers of robots with broken kickers - required string broken_kickers = 3; + required BrokenRobots broken_dribblers = 1; + required BrokenRobots broken_chippers = 2; + required BrokenRobots broken_kickers = 3; +} + +message BrokenRobots +{ + required bool robot_0 = 1; + required bool robot_1 = 2; + required bool robot_2 = 3; + required bool robot_3 = 4; + required bool robot_4 = 5; + required bool robot_5 = 6; } message SensorFusionConfig @@ -477,6 +484,10 @@ message SensorFusionConfig // considered touching the ball (in m) required double touching_ball_threshold_meters = 14 [default = 0.1, (bounds).min_double_value = 0.0, (bounds).max_double_value = 1.0]; + + // Object to represent which robots have broken dribblers, kickers, or chippers + // (updated via Thunderscope) + required RobotCapabilitiesConfig robot_capabilities_config = 15; } message EnemyBallPlacementPlayConfig diff --git a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic.cpp index cabb85a57e..4563bf8f73 100644 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic.cpp @@ -7,7 +7,7 @@ #include "software/world/ball.h" AttackerTactic::AttackerTactic(TbotsProto::AiConfig ai_config) - : Tactic({RobotCapability::Kick, RobotCapability::Chip, RobotCapability::Move}), + : Tactic({RobotCapability::Kick, RobotCapability::Move, RobotCapability::Dribble}), fsm_map(), best_pass_so_far(std::nullopt), pass_committed(false), diff --git a/src/software/ai/hl/stp/tactic/pass_defender/pass_defender_tactic.cpp b/src/software/ai/hl/stp/tactic/pass_defender/pass_defender_tactic.cpp index f1f57e1c4d..339cd77540 100644 --- a/src/software/ai/hl/stp/tactic/pass_defender/pass_defender_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/pass_defender/pass_defender_tactic.cpp @@ -5,7 +5,7 @@ #include "software/logger/logger.h" PassDefenderTactic::PassDefenderTactic(TbotsProto::AiConfig ai_config) - : Tactic({RobotCapability::Move, RobotCapability::Kick}), + : Tactic({RobotCapability::Move}), fsm_map(), control_params(PassDefenderFSM::ControlParams()), ai_config(ai_config) diff --git a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.cpp b/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.cpp index 3ce5327739..7dbd917326 100644 --- a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.cpp @@ -10,8 +10,7 @@ #include "software/logger/logger.h" PivotKickTactic::PivotKickTactic(TbotsProto::AiConfig ai_config) - : Tactic({RobotCapability::Move, RobotCapability::Kick, RobotCapability::Chip, - RobotCapability::Dribble}), + : Tactic({RobotCapability::Move, RobotCapability::Kick, RobotCapability::Dribble}), fsm_map(), control_params(PivotKickFSM::ControlParams()), ai_config(ai_config) diff --git a/src/software/ai/hl/stp/tactic/receiver/receiver_tactic.cpp b/src/software/ai/hl/stp/tactic/receiver/receiver_tactic.cpp index 34cd746b6b..5cf5d5ca25 100644 --- a/src/software/ai/hl/stp/tactic/receiver/receiver_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/receiver/receiver_tactic.cpp @@ -7,7 +7,7 @@ #include "software/logger/logger.h" ReceiverTactic::ReceiverTactic(const TbotsProto::ReceiverTacticConfig& receiver_config) - : Tactic({RobotCapability::Move}), + : Tactic({RobotCapability::Move, RobotCapability::Dribble, RobotCapability::Kick}), fsm_map(), control_params({ReceiverFSM::ControlParams{.pass = std::nullopt, .disable_one_touch_shot = false}}), diff --git a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic.cpp b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic.cpp index ebd70f7088..0145a166c0 100644 --- a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic.cpp @@ -1,7 +1,7 @@ #include "software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic.h" ShadowEnemyTactic::ShadowEnemyTactic() - : Tactic({RobotCapability::Move, RobotCapability::Kick}), + : Tactic({RobotCapability::Move}), fsm_map(), control_params{ShadowEnemyFSM::ControlParams{.enemy_threat = std::nullopt, .shadow_distance = 0}} diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index 261cbeb6a0..7d634b36fa 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -78,6 +78,11 @@ void SensorFusion::processSensorProto(const SensorProto &sensor_msg) } } +TbotsProto::SensorFusionConfig &SensorFusion::getConfig() +{ + return sensor_fusion_config; +} + void SensorFusion::updateWorld(const SSLProto::SSL_WrapperPacket &packet) { if (packet.has_geometry()) @@ -158,8 +163,25 @@ void SensorFusion::updateWorld( for (auto &robot_status_msg : robot_status_msgs) { RobotId robot_id = robot_status_msg.robot_id(); + TbotsProto::RobotCapabilitiesConfig capabilities_config = + sensor_fusion_config.robot_capabilities_config(); std::set unavailableCapabilities; + const google::protobuf::Reflection *broken_dribblers_reflection = + capabilities_config.broken_dribblers().GetReflection(); + const google::protobuf::Descriptor *broken_dribblers_descriptor = + capabilities_config.broken_dribblers().GetDescriptor(); + + const google::protobuf::Reflection *broken_chippers_reflection = + capabilities_config.broken_chippers().GetReflection(); + const google::protobuf::Descriptor *broken_chippers_descriptor = + capabilities_config.broken_chippers().GetDescriptor(); + + const google::protobuf::Reflection *broken_kickers_reflection = + capabilities_config.broken_kickers().GetReflection(); + const google::protobuf::Descriptor *broken_kickers_descriptor = + capabilities_config.broken_kickers().GetDescriptor(); + for (const auto &error_code_msg : robot_status_msg.error_code()) { if (error_code_msg == TbotsProto::ErrorCode::HIGH_CAP) @@ -172,6 +194,28 @@ void SensorFusion::updateWorld( unavailableCapabilities.insert(RobotCapability::Dribble); } } + + if (broken_dribblers_reflection->GetBool( + capabilities_config.broken_dribblers(), + broken_dribblers_descriptor->field(robot_id))) + { + unavailableCapabilities.insert(RobotCapability::Dribble); + } + + if (broken_chippers_reflection->GetBool( + capabilities_config.broken_chippers(), + broken_chippers_descriptor->field(robot_id))) + { + unavailableCapabilities.insert(RobotCapability::Chip); + } + + if (broken_kickers_reflection->GetBool( + capabilities_config.broken_kickers(), + broken_kickers_descriptor->field(robot_id))) + { + unavailableCapabilities.insert(RobotCapability::Kick); + } + friendly_team.setUnavailableRobotCapabilities(robot_id, unavailableCapabilities); if (robot_status_msg.has_power_status() && diff --git a/src/software/sensor_fusion/sensor_fusion.h b/src/software/sensor_fusion/sensor_fusion.h index b9c0f8196a..4b5c56702b 100644 --- a/src/software/sensor_fusion/sensor_fusion.h +++ b/src/software/sensor_fusion/sensor_fusion.h @@ -61,6 +61,13 @@ class SensorFusion // https://github.com/UBC-Thunderbots/Software/issues/3197 static constexpr double DISTANCE_THRESHOLD_FOR_BREAKBEAM_FAULT_DETECTION = 0.5; + /** + * Returns the SensorFusionConfig object + * + * @return the SensorFusionConfig object + */ + TbotsProto::SensorFusionConfig &getConfig(); + private: /** * Updates relevant components of world based on a new data diff --git a/src/software/sensor_fusion/sensor_fusion_test.cpp b/src/software/sensor_fusion/sensor_fusion_test.cpp index 3cb5dffdfb..69a6e88442 100644 --- a/src/software/sensor_fusion/sensor_fusion_test.cpp +++ b/src/software/sensor_fusion/sensor_fusion_test.cpp @@ -1010,3 +1010,81 @@ TEST_F(SensorFusionTest, breakbeam_fail_test_ssl) // did it not use robot position EXPECT_TRUE(ball_position != robot_state.position()); } + +TEST_F(SensorFusionTest, test_unavailable_robot_capabilities_dribblers) +{ + SensorProto sensor_msg; + auto ssl_wrapper_packet = + createSSLWrapperPacket(std::move(geom_data), initDetectionFrame()); + *(sensor_msg.mutable_ssl_vision_msg()) = *ssl_wrapper_packet; + *(sensor_msg.add_robot_status_msgs()) = *robot_status_msg_id_1; + + sensor_fusion.getConfig() + .mutable_robot_capabilities_config() + ->mutable_broken_dribblers() + ->set_robot_1(true); + + sensor_fusion.processSensorProto(sensor_msg); + + std::optional robot = + sensor_fusion.getWorld().value().friendlyTeam().getRobotById(1); + + std::set robot_unavailable_capabilities = + robot.value().getUnavailableCapabilities(); + + EXPECT_TRUE(robot_unavailable_capabilities.contains(RobotCapability::Dribble)); + EXPECT_FALSE(robot_unavailable_capabilities.contains(RobotCapability::Kick)); + EXPECT_FALSE(robot_unavailable_capabilities.contains(RobotCapability::Chip)); +} + +TEST_F(SensorFusionTest, test_unavailable_robot_capabilities_kickers) +{ + SensorProto sensor_msg; + auto ssl_wrapper_packet = + createSSLWrapperPacket(std::move(geom_data), initDetectionFrame()); + *(sensor_msg.mutable_ssl_vision_msg()) = *ssl_wrapper_packet; + *(sensor_msg.add_robot_status_msgs()) = *robot_status_msg_id_1; + + sensor_fusion.getConfig() + .mutable_robot_capabilities_config() + ->mutable_broken_kickers() + ->set_robot_1(true); + + sensor_fusion.processSensorProto(sensor_msg); + + std::optional robot = + sensor_fusion.getWorld().value().friendlyTeam().getRobotById(1); + + std::set robot_unavailable_capabilities = + robot.value().getUnavailableCapabilities(); + + EXPECT_FALSE(robot_unavailable_capabilities.contains(RobotCapability::Dribble)); + EXPECT_TRUE(robot_unavailable_capabilities.contains(RobotCapability::Kick)); + EXPECT_FALSE(robot_unavailable_capabilities.contains(RobotCapability::Chip)); +} + +TEST_F(SensorFusionTest, test_unavailable_robot_capabilities_chippers) +{ + SensorProto sensor_msg; + auto ssl_wrapper_packet = + createSSLWrapperPacket(std::move(geom_data), initDetectionFrame()); + *(sensor_msg.mutable_ssl_vision_msg()) = *ssl_wrapper_packet; + *(sensor_msg.add_robot_status_msgs()) = *robot_status_msg_id_1; + + sensor_fusion.getConfig() + .mutable_robot_capabilities_config() + ->mutable_broken_chippers() + ->set_robot_1(true); + + sensor_fusion.processSensorProto(sensor_msg); + + std::optional robot = + sensor_fusion.getWorld().value().friendlyTeam().getRobotById(1); + + std::set robot_unavailable_capabilities = + robot.value().getUnavailableCapabilities(); + + EXPECT_FALSE(robot_unavailable_capabilities.contains(RobotCapability::Dribble)); + EXPECT_FALSE(robot_unavailable_capabilities.contains(RobotCapability::Kick)); + EXPECT_TRUE(robot_unavailable_capabilities.contains(RobotCapability::Chip)); +}