Robot Substitution by Mr-Anyone · Pull Request #3429 · UBC-Thunderbots/Software

Mr-Anyone · 2025-02-20T07:30:48Z

Description

Robot substitution

Testing Done

TBD, maybe need a field potentially!

Length Justification and Key Files to Review

N/A

Review Checklist

Function & Class comments: All function definitions (usually in the .h file) should have a javadoc style comment at the start of them. For examples, see the functions defined in thunderbots/software/geom. Similarly, all classes should have an associated Javadoc comment explaining the purpose of the class.
Remove all commented out code
Remove extra print statements: for example, those just used for testing
Resolve all TODO's: All TODO (or similar) statements should either be completed or associated with a github issue

Taken mostly from @jameschuang2002 in his pr.

itsarune · 2025-03-19T10:19:49Z

+
+        // move to middle of court and to positive y boundary and stop
+        auto_sub_tactic->updateControlParams(
+            Point(0, world_ptr->field().totalYLength() / 2), Angle::zero(),


we need to be able to control which boundary to go to. Ideally, via dynamic params

bump @Mr-Anyone

itsarune · 2025-03-19T10:20:32Z

+            TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT,
+            TbotsProto::ObstacleAvoidanceMode::SAFE);
+
+        for (auto robot : injured_robots)


we need to limit the number of robots that will simultaneously substitute (ideally a max number set via dynamic params)

bump @Mr-Anyone

itsarune · 2025-03-19T10:22:24Z

        asst));
 }
+
+TEST_F(TacticAssignmentTest, test_assigning_stop_tactics_to_unassigned_non_goalie_robots)


can you add this test file to the BUILD

bump @Mr-Anyone

itsarune · 2025-03-19T10:25:29Z


    virtual ~SensorFusion() = default;

+    const static int BREAK_BEAM_TOLERANCE = 2;


please suffix units to all constants

bump @Mr-Anyone

itsarune · 2025-03-19T10:29:21Z

+TEST_F(SensorFusionTest, test_detect_front_right_motor_fault)
+{
+    std::vector<TbotsProto::RobotStatus> robot_status_msgs;
+    auto robot_msg = std::make_unique<TbotsProto::RobotStatus>();
+    robot_msg->set_robot_id(1);
+    robot_msg->add_error_code(TbotsProto::ErrorCode::NO_ERROR);
+    robot_msg->mutable_motor_status()->mutable_front_right()->add_motor_faults(
+        TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE);
+    robot_status_msgs.push_back(*robot_msg);
+
+    auto robot_msg2 = std::make_unique<TbotsProto::RobotStatus>();
+    robot_msg2->add_error_code(TbotsProto::ErrorCode::NO_ERROR);
+    robot_msg2->set_robot_id(2);
+    robot_status_msgs.push_back(*robot_msg2);
+
+    SensorProto sensor_msg;
+
+    auto ssl_wrapper_packet =
+        createSSLWrapperPacket(std::move(geom_data), initDetectionFrame());
+    // set vision msg so that world is valid
+    *(sensor_msg.mutable_ssl_vision_msg()) = *ssl_wrapper_packet;
+
+    sensor_fusion.processSensorProto(sensor_msg);
+
+    sensor_fusion.detectInjuredRobots(robot_status_msgs);
+
+    std::vector<Robot> injured_robots =
+        sensor_fusion.getWorld().value().friendlyTeam().getInjuredRobots();
+    EXPECT_EQ(injured_robots.size(), 1);
+
+    if (injured_robots.size() == 1)
+    {
+        EXPECT_EQ(injured_robots[0].id(), 1);
+    }
+}
+
+TEST_F(SensorFusionTest, test_detect_front_left_motor_fault)
+{
+    std::vector<TbotsProto::RobotStatus> robot_status_msgs;
+    auto robot_msg = std::make_unique<TbotsProto::RobotStatus>();
+    robot_msg->set_robot_id(1);
+    robot_msg->add_error_code(TbotsProto::ErrorCode::NO_ERROR);
+    robot_msg->mutable_motor_status()->mutable_front_left()->add_motor_faults(
+        TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE_PREWARNING);
+    robot_status_msgs.push_back(*robot_msg);
+
+    auto robot_msg2 = std::make_unique<TbotsProto::RobotStatus>();
+    robot_msg2->add_error_code(TbotsProto::ErrorCode::NO_ERROR);
+    robot_msg2->set_robot_id(2);
+    robot_status_msgs.push_back(*robot_msg2);
+
+    SensorProto sensor_msg;
+
+    auto ssl_wrapper_packet =
+        createSSLWrapperPacket(std::move(geom_data), initDetectionFrame());
+    // set vision msg so that world is valid
+    *(sensor_msg.mutable_ssl_vision_msg()) = *ssl_wrapper_packet;
+
+    sensor_fusion.processSensorProto(sensor_msg);
+
+    sensor_fusion.detectInjuredRobots(robot_status_msgs);
+
+    std::vector<Robot> injured_robots =
+        sensor_fusion.getWorld().value().friendlyTeam().getInjuredRobots();
+    EXPECT_EQ(injured_robots.size(), 1);
+
+    if (injured_robots.size() == 1)
+    {
+        EXPECT_EQ(injured_robots[0].id(), 1);
+    }
+}
+
+TEST_F(SensorFusionTest, test_detect_back_right_motor_fault)
+{
+    std::vector<TbotsProto::RobotStatus> robot_status_msgs;
+    auto robot_msg = std::make_unique<TbotsProto::RobotStatus>();
+    robot_msg->set_robot_id(1);
+    robot_msg->add_error_code(TbotsProto::ErrorCode::NO_ERROR);
+    robot_msg->mutable_motor_status()->mutable_front_right()->add_motor_faults(
+        TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE);
+    robot_status_msgs.push_back(*robot_msg);
+
+    auto robot_msg2 = std::make_unique<TbotsProto::RobotStatus>();
+    robot_msg2->add_error_code(TbotsProto::ErrorCode::NO_ERROR);
+    robot_msg2->set_robot_id(2);
+    robot_msg2->mutable_motor_status()->mutable_back_right()->add_motor_faults(
+        TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE);
+    robot_status_msgs.push_back(*robot_msg2);
+
+    SensorProto sensor_msg;
+
+    auto ssl_wrapper_packet =
+        createSSLWrapperPacket(std::move(geom_data), initDetectionFrame());
+    // set vision msg so that world is valid
+    *(sensor_msg.mutable_ssl_vision_msg()) = *ssl_wrapper_packet;
+
+    sensor_fusion.processSensorProto(sensor_msg);
+
+    sensor_fusion.detectInjuredRobots(robot_status_msgs);
+
+    std::vector<Robot> injured_robots =
+        sensor_fusion.getWorld().value().friendlyTeam().getInjuredRobots();
+    EXPECT_EQ(injured_robots.size(), 2);
+
+    if (injured_robots.size() == 2)
+    {
+        EXPECT_EQ(injured_robots[0].id(), 1);
+        EXPECT_EQ(injured_robots[1].id(), 2);
+    }
+}
+
+TEST_F(SensorFusionTest, test_detect_back_left_motor_fault)
+{
+    std::vector<TbotsProto::RobotStatus> robot_status_msgs;
+    auto robot_msg = std::make_unique<TbotsProto::RobotStatus>();
+    robot_msg->set_robot_id(1);
+    robot_msg->add_error_code(TbotsProto::ErrorCode::NO_ERROR);
+    robot_msg->mutable_motor_status()->mutable_front_left()->add_motor_faults(
+        TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE_PREWARNING);
+    robot_status_msgs.push_back(*robot_msg);
+
+    auto robot_msg2 = std::make_unique<TbotsProto::RobotStatus>();
+    robot_msg2->add_error_code(TbotsProto::ErrorCode::NO_ERROR);
+    robot_msg2->set_robot_id(2);
+    robot_msg2->mutable_motor_status()->mutable_back_left()->add_motor_faults(
+        TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE_PREWARNING);
+    robot_status_msgs.push_back(*robot_msg2);
+
+    SensorProto sensor_msg;
+
+    auto ssl_wrapper_packet =
+        createSSLWrapperPacket(std::move(geom_data), initDetectionFrame());
+    // set vision msg so that world is valid
+    *(sensor_msg.mutable_ssl_vision_msg()) = *ssl_wrapper_packet;
+
+    sensor_fusion.processSensorProto(sensor_msg);
+
+    sensor_fusion.detectInjuredRobots(robot_status_msgs);
+
+    std::vector<Robot> injured_robots =
+        sensor_fusion.getWorld().value().friendlyTeam().getInjuredRobots();
+    EXPECT_EQ(injured_robots.size(), 2);
+
+    if (injured_robots.size() == 2)
+    {
+        EXPECT_EQ(injured_robots[0].id(), 1);
+        EXPECT_EQ(injured_robots[1].id(), 2);
+    }
+}


these tests are all very similar. consider making this into a parametrized test (TEST_P)

example:

Software/src/software/logger/csv_sink_test.cpp

Line 36 in e0ca810

TEST_P(CSVSinkTest, test_csv_log_levels_not_logging)

bump @Mr-Anyone

itsarune · 2025-03-19T10:31:25Z

    dribble_displacement = std::nullopt;
 }
+
+// This detects injured robots, but isn't called anywhere?


looks like you can call this function in:

Software/src/software/sensor_fusion/sensor_fusion.cpp

Line 164 in e0ca810

void SensorFusion::updateWorld(

bump @Mr-Anyone

github-actions · 2025-04-19T03:08:06Z

This PR is stale because it has been open for 30 days with no activity. Remove stale label or comment or this will be closed in 5 days.

github-actions · 2025-04-24T03:24:27Z

This PR was closed because it has been stalled for 5 days with no activity.

itsarune · 2025-06-04T22:54:25Z

+    if (injured_robots.size() > 0)
+    {
+        std::map<RobotId, TrajectoryPath> robot_trajectories = {};
+        unsigned int num_injured = (unsigned int)injured_robots.size();


don't use C-style casts. static_cast<unsigned int>(injured_robots.size())

itsarune · 2025-06-04T22:57:24Z

+void SensorFusion::detectInjuredRobots(
+    const std::vector<TbotsProto::RobotStatus> &robot_status_msgs)
+{
+    /* potential improvement: add short circuit logic when an error is detected */


make an issue for this

itsarune · 2025-06-04T22:58:25Z

+        RobotId robot_id = robot_status_msg.robot_id();
+        bool has_error   = false;
+
+        // Note: each check is not continuing when the robot is identified as injured


Suggested change

// Note: each check is not continuing when the robot is identified as injured

// Note: check stops once a robot has been identified as injured

itsarune · 2025-06-04T22:59:14Z

+            drive_unit_msgs.push_back(motor_status_msgs.front_left());
+            drive_unit_msgs.push_back(motor_status_msgs.front_right());
+            drive_unit_msgs.push_back(motor_status_msgs.back_left());
+            drive_unit_msgs.push_back(motor_status_msgs.back_right());


you can also add the dribbler as well

itsarune · 2025-06-04T23:00:00Z

    virtual ~SensorFusion() = default;

+    const static int BREAK_BEAM_TOLERANCE = 2;
+    void detectInjuredRobots(


also this function can be private

itsarune · 2025-06-04T23:10:30Z

+
+    std::vector<Robot> injured_robots =
+        sensor_fusion.getWorld().value().friendlyTeam().getInjuredRobots();
+    EXPECT_EQ(injured_robots.size(), 1);


kinda pedantic but if you use ASSERT_EQ here, then below you can remove the if because the assertion will stop execution at this point if it fails

itsarune · 2025-06-04T23:10:44Z

+
+    std::vector<Robot> injured_robots =
+        sensor_fusion.getWorld().value().friendlyTeam().getInjuredRobots();
+    EXPECT_EQ(injured_robots.size(), 2);


same note as above

itsarune · 2025-06-04T23:11:03Z

+        sensor_fusion.getWorld().value().friendlyTeam().getInjuredRobots();
+    EXPECT_EQ(injured_robots.size(), 1);
+
+    if (injured_robots.size() == 1)


same note as above

itsarune · 2025-06-04T23:11:17Z

+
+    std::vector<Robot> injured_robots =
+        sensor_fusion.getWorld().value().friendlyTeam().getInjuredRobots();
+    EXPECT_EQ(injured_robots.size(), 1);


itsarune · 2025-06-04T23:11:28Z

+        sensor_fusion.getWorld().value().friendlyTeam().getInjuredRobots();
+    EXPECT_EQ(injured_robots.size(), 1);
+
+    if (injured_robots.size() == 1)


itsarune

great work with the tests! left some comments

Mr-Anyone · 2025-06-05T00:17:17Z

great work with the tests! left some comments

It turns out that the code taken previously doesn't really work after running the test case. I will work on this in a few days.

itsarune · 2025-07-07T21:57:58Z

+    std::vector<RobotSubstituionParam> substitution_robot_params
+        = trySubstituteInjuredRobots(world_ptr);
+    // applying robot substitution result
+    num_tactics -= static_cast<unsigned int>(substitution_robot_params.size());


prone to underflow:

num_tactics = std::max(0, static_cast<int>(num_tactics) - substitution_robot_params.size());

itsarune · 2025-07-07T22:00:21Z

+std::vector<Play::RobotSubstituionParam> Play::trySubstituteInjuredRobots(const WorldPtr& world_ptr){
+    // Don't do anything if there aren't any injured robots  
+    const std::vector<Robot>& injured_robots = world_ptr->friendlyTeam().getInjuredRobots();
+    if(injured_robots.size()  == 0)


using explicit { } is a part of our code style guide:

Software/docs/code-style-guide.md

Line 307 in bd3317f

* Always use curly braces around code blocks, even if the braces surround a single statement.

itsarune · 2025-07-07T22:01:15Z

+
+    // move to middle of court and to positive y boundary and stop
+    double y_coordinate = world_ptr->field().totalYLength() / 2.0; 
+    if(!ai_config.robot_substitution_config().substitute_at_positive_y())


using explicit { } is a part of our code style guide:

Software/docs/code-style-guide.md

Line 307 in bd3317f

* Always use curly braces around code blocks, even if the braces surround a single statement.

itsarune · 2025-07-07T22:02:08Z


   private:
+    // For the implementation of `trySubstituteInjuredRobots`
+    struct RobotSubstituionParam{


Suggested change

struct RobotSubstituionParam{

struct RobotSubstitutionParam{

itsarune · 2025-07-07T22:02:38Z

+      * Remove the substituted robot from robots
+      *
+      * @param world_ptr: the current world_ptr with the robot
+      * @param robots substituted robots are removed from this list. There is side effect


this param doesn't exist

itsarune · 2025-07-07T22:06:35Z

    virtual void updateTactics(const PlayUpdate& play_update);

   private:
+    // For the implementation of `trySubstituteInjuredRobots`


Suggested change

// For the implementation of `trySubstituteInjuredRobots`

/**

* RobotSubstitutionParam contains the replacement tactics for robot substitutions.

itsarune · 2025-07-07T22:07:54Z

+    EXPECT_EQ(expected_pos, injured_robot_pos);
+}
+
+/**


you should also check that we don't exceed the limit we've set in the maximum_robot_substitution_count()

itsarune · 2025-07-07T22:12:29Z

+
+    // substitution tactic is just a move tactic to a decided location
+    std::shared_ptr<MoveTactic> auto_sub_tactic = std::make_shared<MoveTactic>();
+


Robot Substitution rules

We should also check that the ball is > 0.5m away from the robot substitution location before committing to the substitution

itsarune · 2025-07-07T22:13:36Z

@@ -0,0 +1,71 @@
+
+TEST_F(TacticAssignmentTest, test_assigning_stop_tactics_to_unassigned_non_goalie_robots)


why did you make a new test file and not just put it here: https://github.com/UBC-Thunderbots/Software/blob/master/src/software/ai/hl/stp/stp_tactic_assignment_test.cpp

itsarune

left some feedback

Andrewyx · 2026-04-07T23:28:46Z

@Mr-Anyone What the status of this issue? If it is not currently in progress, could you create an issue, note your progress then link this PR? That way I can assign this issue to someone/prune this ticket

Mr-Anyone · 2026-04-07T23:59:49Z

@Mr-Anyone What the status of this issue? If it is not currently in progress, could you create an issue, note your progress then link this PR? That way I can assign this issue to someone/prune this ticket

I just took a look at this PR, and it seems that if you call setInjuredRobots, the robot will be put in substitution by removing it from the play assignment?

Nobodies calls detectInjuredRobots, so this PR has no functional change. I think this PR just adds the ability for robot substitution (by calling setInjuredRobots?), but never use it in anywhere? Also, I am pretty sure breakbeam change in this PR is a duplicate of #3206. The original robot substitution did not land before we made it to the Netherland. You probably want to remove the detectInjuredRobots function.

tbh, it may be easier to redo this PR than to build from it (I think you can do it more elegantly than this). Feel free to assign it to people. I took most of the code from #3091 .

Mr-Anyone · 2026-04-08T00:19:49Z

@Andrewyx @williamckha btw, feel free to take over #3455 if you think it is urgent or just want to assign it for someone to do.

Mr-Anyone requested review from itsarune, sauravbanna and williamckha as code owners February 20, 2025 07:30

Mr-Anyone closed this Feb 20, 2025

Mr-Anyone reopened this Feb 20, 2025

itsarune reviewed Mar 19, 2025

View reviewed changes

github-actions Bot added the Stale Inactive pull requests label Apr 19, 2025

github-actions Bot closed this Apr 24, 2025

itsarune reopened this Apr 29, 2025

github-actions Bot removed the Stale Inactive pull requests label Apr 30, 2025

Mr-Anyone marked this pull request as draft May 15, 2025 22:11

Mr-Anyone and others added 3 commits May 31, 2025 14:46

made some changes

75f12a9

added additional test

8dc616a

[pre-commit.ci lite] apply automatic fixes

f2b8a13

Mr-Anyone force-pushed the robot_substitution branch from fc6997b to f2b8a13 Compare May 31, 2025 06:46

itsarune reviewed Jun 4, 2025

View reviewed changes

address some stuff

2430eac

itsarune reviewed Jul 7, 2025

View reviewed changes

Mr-Anyone closed this Apr 7, 2026


		virtual ~SensorFusion() = default;

		const static int BREAK_BEAM_TOLERANCE = 2;

	// Note: each check is not continuing when the robot is identified as injured
	// Note: check stops once a robot has been identified as injured

	// For the implementation of `trySubstituteInjuredRobots`
	/**
	* RobotSubstitutionParam contains the replacement tactics for robot substitutions.


		// substitution tactic is just a move tactic to a decided location
		std::shared_ptr<MoveTactic> auto_sub_tactic = std::make_shared<MoveTactic>();

		@@ -0,0 +1,71 @@

		TEST_F(TacticAssignmentTest, test_assigning_stop_tactics_to_unassigned_non_goalie_robots)

Uh oh!

Conversation

Mr-Anyone commented Feb 20, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Testing Done

Length Justification and Key Files to Review

Review Checklist

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

github-actions Bot commented Apr 19, 2025

Uh oh!

github-actions Bot commented Apr 24, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

itsarune left a comment

Choose a reason for hiding this comment

Uh oh!

Mr-Anyone commented Jun 5, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Mr-Anyone commented Feb 20, 2025 •

edited

Loading

Mr-Anyone commented Apr 8, 2026 •

edited

Loading