diff --git a/src/software/sensor_fusion/filter/robot_filter.cpp b/src/software/sensor_fusion/filter/robot_filter.cpp index 7a797c4270..4f156d1039 100644 --- a/src/software/sensor_fusion/filter/robot_filter.cpp +++ b/src/software/sensor_fusion/filter/robot_filter.cpp @@ -16,7 +16,8 @@ RobotFilter::RobotFilter(RobotDetection current_robot_state, } std::optional RobotFilter::getFilteredData( - const std::vector &new_robot_data) + const std::vector &new_robot_data, + const std::optional breakbeam_tripped_id) { int data_num = 0; Timestamp latest_timestamp = Timestamp().fromSeconds(0); @@ -88,11 +89,14 @@ std::optional RobotFilter::getFilteredData( (filtered_data.timestamp.toSeconds() - current_robot_state.timestamp().toSeconds()); + // find breakbeam_status + bool breakbeam_tripped = breakbeam_tripped_id == getRobotId(); + // update current_robot_state this->current_robot_state = Robot(this->getRobotId(), filtered_data.position, filtered_data.velocity, filtered_data.orientation, filtered_data.angular_velocity, - filtered_data.timestamp); + filtered_data.timestamp, breakbeam_tripped); return std::make_optional(this->current_robot_state); } diff --git a/src/software/sensor_fusion/filter/robot_filter.h b/src/software/sensor_fusion/filter/robot_filter.h index 2842bbed9e..5568dfb669 100644 --- a/src/software/sensor_fusion/filter/robot_filter.h +++ b/src/software/sensor_fusion/filter/robot_filter.h @@ -44,10 +44,15 @@ class RobotFilter * The data does not all have to be for a particular Robot, the filter will only use * the new Robot data that matches the robot id the filter was constructed with. * + * @param breakbeam_tripped_id The id of the robot with the tripped breakbeam + * according to sensor fusion filtering logic (or none if no robot has a tripped + * beam). + * * @return The filtered data for the robot */ std::optional getFilteredData( - const std::vector& new_robot_data); + const std::vector& new_robot_data, + const std::optional breakbeam_tripped_id = std::nullopt); /** * Returns the id of the Robot that this filter is filtering for diff --git a/src/software/sensor_fusion/filter/robot_team_filter.cpp b/src/software/sensor_fusion/filter/robot_team_filter.cpp index fafacd9729..9a09aee931 100644 --- a/src/software/sensor_fusion/filter/robot_team_filter.cpp +++ b/src/software/sensor_fusion/filter/robot_team_filter.cpp @@ -8,7 +8,8 @@ RobotTeamFilter::RobotTeamFilter() {} Team RobotTeamFilter::getFilteredData( const Team ¤t_team_state, - const std::vector &new_robot_detections) + const std::vector &new_robot_detections, + const std::optional breakbeam_tripped_id) { // Add filters for any robot we haven't seen before for (auto detection : new_robot_detections) @@ -28,7 +29,8 @@ Team RobotTeamFilter::getFilteredData( std::vector new_filtered_robot_data; for (auto it = robot_filters.begin(); it != robot_filters.end(); it++) { - auto data = it->second.getFilteredData(new_robot_detections); + auto data = + it->second.getFilteredData(new_robot_detections, breakbeam_tripped_id); if (data) { new_filtered_robot_data.emplace_back(*data); diff --git a/src/software/sensor_fusion/filter/robot_team_filter.h b/src/software/sensor_fusion/filter/robot_team_filter.h index de59efe145..7ad293dab0 100644 --- a/src/software/sensor_fusion/filter/robot_team_filter.h +++ b/src/software/sensor_fusion/filter/robot_team_filter.h @@ -20,11 +20,15 @@ class RobotTeamFilter * * @param current_team_state The current state of the Team * @param new_robot_detections A list of new SSL Robot detections + * @param breakbeam_tripped_id The id of the robot with the tripped breakbeam + * according to sensor fusion * * @return The updated state of the team given the new data */ - Team getFilteredData(const Team& current_team_state, - const std::vector& new_robot_detections); + Team getFilteredData( + const Team& current_team_state, + const std::vector& new_robot_detections, + const std::optional breakbeam_tripped_id = std::nullopt); // A map used to store a separate robot filter for each robot on this team, so diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index dced5047ee..b99b7438e6 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -358,8 +358,8 @@ std::optional SensorFusion::createBall( Team SensorFusion::createFriendlyTeam(const std::vector &robot_detections) { - Team new_friendly_team = - friendly_team_filter.getFilteredData(friendly_team, robot_detections); + Team new_friendly_team = friendly_team_filter.getFilteredData( + friendly_team, robot_detections, friendly_robot_id_with_ball_in_dribbler); return new_friendly_team; } @@ -427,7 +427,8 @@ void SensorFusion::updateDribbleDisplacement() Team SensorFusion::createEnemyTeam(const std::vector &robot_detections) { - Team new_enemy_team = enemy_team_filter.getFilteredData(enemy_team, robot_detections); + Team new_enemy_team = + enemy_team_filter.getFilteredData(enemy_team, robot_detections, false); return new_enemy_team; } diff --git a/src/software/sensor_fusion/sensor_fusion_test.cpp b/src/software/sensor_fusion/sensor_fusion_test.cpp index 3cb5dffdfb..cd980c036a 100644 --- a/src/software/sensor_fusion/sensor_fusion_test.cpp +++ b/src/software/sensor_fusion/sensor_fusion_test.cpp @@ -1010,3 +1010,88 @@ TEST_F(SensorFusionTest, breakbeam_fail_test_ssl) // did it not use robot position EXPECT_TRUE(ball_position != robot_state.position()); } + +TEST_F(SensorFusionTest, breakbeam_in_robot_test) +{ + // Check that breakbeam status is propagated to the Robot + SensorProto sensor_msg; + + std::unique_ptr frame = initDetectionFrame(); + std::unique_ptr frame_2 = + initDetectionFrameWithFutureTime(); + // creating robot status + auto robot_msg = std::make_unique(); + robot_msg->set_robot_id(2); + + auto power_status_msg = std::make_unique(); + power_status_msg->set_breakbeam_tripped(true); + *(robot_msg->mutable_power_status()) = *power_status_msg; + + auto geometry_data = initSSLDivBGeomData(); + + // use frame 1 to create sensor_msg + auto ssl_wrapper_packet = + createSSLWrapperPacket(std::move(geometry_data), std::move(frame)); + *(sensor_msg.mutable_ssl_vision_msg()) = *ssl_wrapper_packet; + *(sensor_msg.add_robot_status_msgs()) = *robot_msg; + sensor_fusion.processSensorProto(sensor_msg); + + // use frame 2 that is at a future time to give sensor fusion + // a chance to use the breakbeam id it got from the previous sensor proto + auto ssl_wrapper_packet_2 = + createSSLWrapperPacket(std::move(geometry_data), std::move(frame_2)); + *(sensor_msg.mutable_ssl_vision_msg()) = *ssl_wrapper_packet_2; + sensor_fusion.processSensorProto(sensor_msg); + + + std::optional current_world = sensor_fusion.getWorld(); + bool breakbeam_tripped = + current_world.value().friendlyTeam().getRobotById(2)->breakbeamTripped(); + + // is the breakbeam_tripped on the robot + EXPECT_TRUE(breakbeam_tripped); +} + + +TEST_F(SensorFusionTest, breakbeam_not_in_robot_test) +{ + // Check that breakbeam status is false when propagated to the Robot + SensorProto sensor_msg; + + std::unique_ptr frame = initDetectionFrame(); + std::unique_ptr frame_2 = + initDetectionFrameWithFutureTime(); + // creating robot status + auto robot_msg = std::make_unique(); + robot_msg->set_robot_id(2); + + auto power_status_msg = std::make_unique(); + power_status_msg->set_breakbeam_tripped(false); + *(robot_msg->mutable_power_status()) = *power_status_msg; + + // create ssl wrapper packet + auto geometry_data = initSSLDivBGeomData(); + + + // use frame 1 to create sensor_msg + auto ssl_wrapper_packet = + createSSLWrapperPacket(std::move(geometry_data), std::move(frame)); + *(sensor_msg.mutable_ssl_vision_msg()) = *ssl_wrapper_packet; + *(sensor_msg.add_robot_status_msgs()) = *robot_msg; + sensor_fusion.processSensorProto(sensor_msg); + + // use frame 2 that is at a future time to give sensor fusion + // a chance to use the breakbeam id it got from the previous sensor proto + auto ssl_wrapper_packet_2 = + createSSLWrapperPacket(std::move(geometry_data), std::move(frame_2)); + *(sensor_msg.mutable_ssl_vision_msg()) = *ssl_wrapper_packet_2; + sensor_fusion.processSensorProto(sensor_msg); + + + std::optional current_world = sensor_fusion.getWorld(); + bool breakbeam_tripped = + current_world.value().friendlyTeam().getRobotById(2)->breakbeamTripped(); + + // is the break_beam correct + EXPECT_FALSE(breakbeam_tripped); +} diff --git a/src/software/world/robot.cpp b/src/software/world/robot.cpp index fd9efd3acd..1b0dece191 100644 --- a/src/software/world/robot.cpp +++ b/src/software/world/robot.cpp @@ -4,13 +4,27 @@ #include "software/ai/evaluation/time_to_travel.h" #include "software/logger/logger.h" +Robot::Robot(RobotId id, const Point &position, const Vector &velocity, + const Angle &orientation, const AngularVelocity &angular_velocity, + const Timestamp ×tamp, bool breakbeam_tripped, + const std::set &unavailable_capabilities, + const RobotConstants_t &robot_constants) + : id_(id), + current_state_(position, velocity, orientation, angular_velocity, + breakbeam_tripped), + timestamp_(timestamp), + unavailable_capabilities_(unavailable_capabilities), + robot_constants_(robot_constants) +{ +} + Robot::Robot(RobotId id, const Point &position, const Vector &velocity, const Angle &orientation, const AngularVelocity &angular_velocity, const Timestamp ×tamp, const std::set &unavailable_capabilities, const RobotConstants_t &robot_constants) : id_(id), - current_state_(position, velocity, orientation, angular_velocity), + current_state_(position, velocity, orientation, angular_velocity, false), timestamp_(timestamp), unavailable_capabilities_(unavailable_capabilities), robot_constants_(robot_constants) @@ -90,6 +104,11 @@ Angle Robot::orientation() const return current_state_.orientation(); } +bool Robot::breakbeamTripped() const +{ + return current_state_.breakbeamTripped(); +} + AngularVelocity Robot::angularVelocity() const { return current_state_.angularVelocity(); @@ -99,17 +118,19 @@ bool Robot::isNearDribbler(const Point &test_point, double TOLERANCE) const { const double POSSESSION_THRESHOLD_METERS = DIST_TO_FRONT_OF_ROBOT_METERS + TOLERANCE; + bool breakbeam = this->breakbeamTripped(); Vector vector_to_test_point = test_point - position(); if (vector_to_test_point.length() > POSSESSION_THRESHOLD_METERS) { - return false; + return breakbeam; } else { // check that ball is in a 90-degree cone in front of the robot auto ball_to_robot_angle = orientation().minDiff(vector_to_test_point.orientation()); - return (ball_to_robot_angle < Angle::fromDegrees(45.0)); + bool vision_confirm_ball = (ball_to_robot_angle < Angle::fromDegrees(45.0)); + return breakbeam || vision_confirm_ball; } } diff --git a/src/software/world/robot.h b/src/software/world/robot.h index d317c1c8de..e6a509591e 100644 --- a/src/software/world/robot.h +++ b/src/software/world/robot.h @@ -26,16 +26,38 @@ class Robot * per second * @param timestamp The timestamp at which the robot was observed to be in the given * state + * @param breakbeam_tripped The new status of the breakbeam * @param unavailable_capabilities The set of unavailable capabilities for this robot * @param robot_constants The robot constants for this robot */ explicit Robot(RobotId id, const Point &position, const Vector &velocity, const Angle &orientation, const AngularVelocity &angular_velocity, - const Timestamp ×tamp, + const Timestamp ×tamp, bool breakbeam_tripped = false, const std::set &unavailable_capabilities = std::set(), const RobotConstants_t &robot_constants = DEFAULT_ROBOT_CONSTANTS); + /** + * Creates a new robot given robot data + * + * @param id The id of the robot to create + * @param position the new position of the robot. Coordinates are in metres. + * @param velocity the new velocity of the robot, in metres / second. + * @param orientation the new orientation of the robot, in Radians. + * @param angular_velocity the new angular velocity of the robot, in Radians + * per second + * @param timestamp The timestamp at which the robot was observed to be in the given + * state + * @param unavailable_capabilities The set of unavailable capabilities for this robot + * @param robot_constants The robot constants for this robot + */ + explicit Robot(RobotId id, const Point &position, const Vector &velocity, + const Angle &orientation, const AngularVelocity &angular_velocity, + const Timestamp ×tamp, + const std::set &unavailable_capabilities, + const RobotConstants_t &robot_constants); + + /** * Creates a new robot with the given initial state * @@ -114,6 +136,13 @@ class Robot */ Angle orientation() const; + /** + * Returns the current breakbeam status of the robot + * + * @return the current breakbeam status of the robot + */ + bool breakbeamTripped() const; + /** * Returns the current angular velocity of the robot * diff --git a/src/software/world/robot_state.cpp b/src/software/world/robot_state.cpp index 886e222ecc..a312c9a693 100644 --- a/src/software/world/robot_state.cpp +++ b/src/software/world/robot_state.cpp @@ -1,11 +1,13 @@ #include "software/world/robot_state.h" RobotState::RobotState(const Point &position, const Vector &velocity, - const Angle &orientation, const AngularVelocity &angular_velocity) + const Angle &orientation, const AngularVelocity &angular_velocity, + const bool breakbeam_tripped) : position_(position), velocity_(velocity), orientation_(orientation), - angular_velocity_(angular_velocity) + angular_velocity_(angular_velocity), + breakbeam_tripped_(breakbeam_tripped) { } @@ -16,7 +18,8 @@ RobotState::RobotState(const TbotsProto::RobotState &robot_state_proto) robot_state_proto.global_velocity().y_component_meters())), orientation_(Angle::fromRadians(robot_state_proto.global_orientation().radians())), angular_velocity_(AngularVelocity::fromRadians( - robot_state_proto.global_angular_velocity().radians_per_second())) + robot_state_proto.global_angular_velocity().radians_per_second())), + breakbeam_tripped_(false) { } @@ -40,6 +43,11 @@ AngularVelocity RobotState::angularVelocity() const return angular_velocity_; } +bool RobotState::breakbeamTripped() const +{ + return breakbeam_tripped_; +} + bool RobotState::operator==(const RobotState &other) const { return this->position() == other.position() && this->velocity() == other.velocity() && diff --git a/src/software/world/robot_state.h b/src/software/world/robot_state.h index 39d74d8945..80aa2d73ce 100644 --- a/src/software/world/robot_state.h +++ b/src/software/world/robot_state.h @@ -26,10 +26,11 @@ class RobotState * @param velocity The velocity of the robot, in metres per second * @param orientation The orientation of the robot * @param angular_velocity The angular velocity of the robot + * @param breakbeam_tripped The breakbeam status of the robot */ explicit RobotState(const Point &position, const Vector &velocity, - const Angle &orientation, - const AngularVelocity &angular_velocity); + const Angle &orientation, const AngularVelocity &angular_velocity, + const bool breakbeam_tripped = false); /** * Creates a new robot state based on the TbotsProto::RobotState protobuf @@ -68,6 +69,13 @@ class RobotState */ AngularVelocity angularVelocity() const; + /** + * Returns the breakbeam status of the robot represented by this state + * + * @return the breakbeam status of the robot represented by this state + */ + bool breakbeamTripped() const; + /** * Defines the equality operator for a RobotState. RobotStates are equal if * all their members are equal @@ -94,6 +102,7 @@ class RobotState Vector velocity_; Angle orientation_; AngularVelocity angular_velocity_; + bool breakbeam_tripped_; }; /** diff --git a/src/software/world/robot_state_test.cpp b/src/software/world/robot_state_test.cpp index c77c9ebee4..e23ff62121 100644 --- a/src/software/world/robot_state_test.cpp +++ b/src/software/world/robot_state_test.cpp @@ -43,6 +43,20 @@ TEST(RobotStateTest, get_angular_velocity) EXPECT_EQ(AngularVelocity::half(), state.angularVelocity()); } +TEST(RobotStateTest, get_breakbeam_status) +{ + RobotState state(Point(1.1, -0.5), Vector(3, 0), Angle::quarter(), + AngularVelocity::half(), false); + EXPECT_EQ(false, state.breakbeamTripped()); +} + +TEST(RobotStateTest, get_breakbeam_status_tripped) +{ + RobotState state(Point(1.1, -0.5), Vector(3, 0), Angle::quarter(), + AngularVelocity::half(), true); + EXPECT_EQ(true, state.breakbeamTripped()); +} + TEST(RobotStateTest, compare_identical_states) { RobotState state_1(Point(1.1, -0.5), Vector(3, 0), Angle::quarter(), diff --git a/src/software/world/robot_test.cpp b/src/software/world/robot_test.cpp index 312dd4d886..e3bda4b593 100644 --- a/src/software/world/robot_test.cpp +++ b/src/software/world/robot_test.cpp @@ -28,7 +28,7 @@ class RobotTest : public ::testing::Test TEST_F(RobotTest, construct_with_all_params) { Robot robot = Robot(3, Point(1, 1), Vector(-0.3, 0), Angle::fromRadians(2.2), - AngularVelocity::fromRadians(-0.6), current_time); + AngularVelocity::fromRadians(-0.6), current_time, false); EXPECT_EQ(3, robot.id()); EXPECT_EQ(Point(1, 1), robot.position()); @@ -36,6 +36,7 @@ TEST_F(RobotTest, construct_with_all_params) EXPECT_EQ(Angle::fromRadians(2.2), robot.orientation()); EXPECT_EQ(AngularVelocity::fromRadians(-0.6), robot.angularVelocity()); EXPECT_EQ(current_time, robot.timestamp()); + EXPECT_EQ(false, robot.breakbeamTripped()); } TEST_F(RobotTest, construct_with_initial_state) @@ -53,12 +54,28 @@ TEST_F(RobotTest, construct_with_initial_state) EXPECT_EQ(current_time, robot.timestamp()); } +TEST_F(RobotTest, construct_with_initial_state_breakbeam) +{ + Robot robot = Robot(3, + RobotState(Point(1, 1), Vector(-0.3, 0), Angle::fromRadians(2.2), + AngularVelocity::fromRadians(-0.6), true), + current_time); + + EXPECT_EQ(3, robot.id()); + EXPECT_EQ(Point(1, 1), robot.position()); + EXPECT_EQ(Vector(-0.3, 0), robot.velocity()); + EXPECT_EQ(Angle::fromRadians(2.2), robot.orientation()); + EXPECT_EQ(AngularVelocity::fromRadians(-0.6), robot.angularVelocity()); + EXPECT_EQ(current_time, robot.timestamp()); + EXPECT_EQ(true, robot.breakbeamTripped()); +} + TEST_F(RobotTest, construct_with_protobuf) { Robot original_robot = Robot(3, RobotState(Point(1, 1), Vector(-0.3, 0), Angle::fromRadians(2.2), - AngularVelocity::fromRadians(-0.6)), + AngularVelocity::fromRadians(-0.6), false), current_time, std::set{RobotCapability::Chip, RobotCapability::Move, RobotCapability::Kick, RobotCapability::Dribble}); @@ -230,6 +247,16 @@ TEST(RobotIsNearDribblerTest, ball_near_dribbler_ball_to_side_of_robot) EXPECT_FALSE(robot.isNearDribbler(ball_position)); } +TEST(RobotIsNearDribblerTest, ball_near_dribbler_according_to_breakbeam) +{ + Point ball_position = Point(0.07, 0); + Timestamp timestamp = Timestamp::fromSeconds(0); + + Robot robot = Robot(0, Point(0, 0), Vector(), Angle::half(), AngularVelocity::zero(), + timestamp, true); + EXPECT_TRUE(robot.isNearDribbler(ball_position)); +} + TEST(RobotIsNearDribblerTest, ball_near_dribbler_robot_moving_ball_in_dribbler) { Point ball_position = Point(0.07, 0); @@ -270,9 +297,9 @@ TEST_F(RobotTest, get_unavailable_capabilities) RobotCapability::Chip, }; - Robot robot = - Robot(0, Point(3, 1.2), Vector(-3, 1), Angle::fromDegrees(0), - AngularVelocity::fromDegrees(25), current_time, unavailable_capabilities); + Robot robot = Robot(0, Point(3, 1.2), Vector(-3, 1), Angle::fromDegrees(0), + AngularVelocity::fromDegrees(25), current_time, false, + unavailable_capabilities); EXPECT_EQ(unavailable_capabilities, robot.getUnavailableCapabilities()); } @@ -284,9 +311,9 @@ TEST_F(RobotTest, get_available_capabilities) RobotCapability::Chip, }; - Robot robot = - Robot(0, Point(3, 1.2), Vector(-3, 1), Angle::fromDegrees(0), - AngularVelocity::fromDegrees(25), current_time, unavailable_capabilities); + Robot robot = Robot(0, Point(3, 1.2), Vector(-3, 1), Angle::fromDegrees(0), + AngularVelocity::fromDegrees(25), current_time, false, + unavailable_capabilities); // available capabilities = all capabilities - unavailable capabilities std::set all_capabilities = allRobotCapabilities();