Brakebream availability in gameplay (UBC-Thunderbots#3462)

Author: zuperzane

src/software/sensor_fusion/filter/robot_filter.cpp

@@ -16,7 +16,8 @@ RobotFilter::RobotFilter(RobotDetection current_robot_state,
 }
 
 std::optional<Robot> RobotFilter::getFilteredData(
-    const std::vector<RobotDetection> &new_robot_data)
+    const std::vector<RobotDetection> &new_robot_data,
+    const std::optional<RobotId> breakbeam_tripped_id)
 {
     int data_num               = 0;
     Timestamp latest_timestamp = Timestamp().fromSeconds(0);
@@ -88,11 +89,14 @@ std::optional<Robot> 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);
     }

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<Robot> getFilteredData(
-        const std::vector<RobotDetection>& new_robot_data);
+        const std::vector<RobotDetection>& new_robot_data,
+        const std::optional<RobotId> breakbeam_tripped_id = std::nullopt);
 
     /**
      * Returns the id of the Robot that this filter is filtering for

src/software/sensor_fusion/filter/robot_team_filter.cpp

@@ -8,7 +8,8 @@ RobotTeamFilter::RobotTeamFilter() {}
 
 Team RobotTeamFilter::getFilteredData(
     const Team &current_team_state,
-    const std::vector<RobotDetection> &new_robot_detections)
+    const std::vector<RobotDetection> &new_robot_detections,
+    const std::optional<RobotId> 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<Robot> 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);

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<RobotDetection>& new_robot_detections);
+    Team getFilteredData(
+        const Team& current_team_state,
+        const std::vector<RobotDetection>& new_robot_detections,
+        const std::optional<RobotId> breakbeam_tripped_id = std::nullopt);
 
 
     // A map used to store a separate robot filter for each robot on this team, so

src/software/sensor_fusion/sensor_fusion.cpp

@@ -362,8 +362,8 @@ std::optional<Ball> SensorFusion::createBall(
 
 Team SensorFusion::createFriendlyTeam(const std::vector<RobotDetection> &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;
 }
 
@@ -431,7 +431,8 @@ void SensorFusion::updateDribbleDisplacement()
 
 Team SensorFusion::createEnemyTeam(const std::vector<RobotDetection> &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;
 }
 

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<SSLProto::SSL_DetectionFrame> frame = initDetectionFrame();
+    std::unique_ptr<SSLProto::SSL_DetectionFrame> frame_2 =
+        initDetectionFrameWithFutureTime();
+    // creating robot status
+    auto robot_msg = std::make_unique<TbotsProto::RobotStatus>();
+    robot_msg->set_robot_id(2);
+
+    auto power_status_msg = std::make_unique<TbotsProto::PowerStatus>();
+    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<World> 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<SSLProto::SSL_DetectionFrame> frame = initDetectionFrame();
+    std::unique_ptr<SSLProto::SSL_DetectionFrame> frame_2 =
+        initDetectionFrameWithFutureTime();
+    // creating robot status
+    auto robot_msg = std::make_unique<TbotsProto::RobotStatus>();
+    robot_msg->set_robot_id(2);
+
+    auto power_status_msg = std::make_unique<TbotsProto::PowerStatus>();
+    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<World> current_world = sensor_fusion.getWorld();
+    bool breakbeam_tripped =
+        current_world.value().friendlyTeam().getRobotById(2)->breakbeamTripped();
+
+    // is the break_beam correct
+    EXPECT_FALSE(breakbeam_tripped);
+}

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 &timestamp, bool breakbeam_tripped,
+             const std::set<RobotCapability> &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 &timestamp,
              const std::set<RobotCapability> &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;
     }
 }
 

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 &timestamp,
+                   const Timestamp &timestamp, bool breakbeam_tripped = false,
                    const std::set<RobotCapability> &unavailable_capabilities =
                        std::set<RobotCapability>(),
                    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 &timestamp,
+                   const std::set<RobotCapability> &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
      *

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() &&

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_;
 };
 
 /**