Return trajectory early over threshold

docs/code-style-guide.md

@@ -19,23 +19,6 @@
 - [Protobuf](#protobuf)
 
 <!--TOC-->
-
-Our C++ coding style is based off of [Google's C++ Style Guide](https://google.github.io/styleguide/cppguide.html). We use [clang-format](https://clang.llvm.org/docs/ClangFormat.html) to enforce most of the nit-picky parts of the style, such as brackets and alignment, so this document highlights the important rules to follow that clang-format cannot enforce.
-
-If you want to know more about our coding style you can take a look at our [clang-format configuration file](https://github.com/UBC-Thunderbots/Software/blob/master/.clang-format).
-
-
-### Names and Variables
-
-* Classes, structures, namespaces, unions, enumerates, "typename"-type template parameters, and typedefs names uses `CamelCase` with a capital letter.
-
-  ```cpp
-  // Incorrect
-  class eventHandler;
-
-  // Incorrect
-  class event_handler;
-
   // Correct
   class EventHandler;
   ```

docs/robot-software-architecture.md

@@ -5,25 +5,11 @@
 <!--TOC-->
 
 - [Table of Contents](#table-of-contents)
-- [Robot Software Diagram](#robot-software-diagram)
-- [Tools](#tools)
-  - [Ansible](#ansible)
   - [Systemd](#systemd)
   - [Redis](#redis)
 - [Thunderloop](#thunderloop)
 
 <!--TOC-->
-
-# Robot Software Diagram
-
-![Robot Software Diagram](images/robot_software_diagram.svg)
-
-# Tools
-
-## Ansible
-
-[Ansible](https://www.ansible.com/overview/how-ansible-works) allows us to run actions on multiple robots at once. Actions are communicated through YAML files called playbooks. Playbooks contain a series of tasks (eg. move a file, run this script, output this command) and logic dictating dependencies between tasks. When playbooks are run, Ansible establishes an SSH connection between the user's computer and robot, allowing it to run the tasks in the playbook. Output from each task, and any other requested output, is displayed on the console
-
 For a more detailed look at how Ansible works, [see the RFC](https://docs.google.com/document/d/1hN3Us2Vjr8z6ihqUVp_3L7rrjKc-EZ-l2hZJc31gNOc/edit)
 
 Example command: `bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot --//software/embedded:host_platform=<platform> -- --playbook deploy_robot_software.yml --hosts <robot_ip> --ssh_pass <robot_password>`

docs/software-architecture-and-design.md

@@ -5,16 +5,6 @@
 <!--TOC-->
 
 - [Table of Contents](#table-of-contents)
-- [Architecture Overview](#architecture-overview)
-  - [League-Maintained Software](#league-maintained-software)
-    - [SSL Vision](#ssl-vision)
-    - [SSL Gamecontroller](#ssl-gamecontroller)
-- [Protobuf](#protobuf)
-  - [Important Protobuf Messages](#important-protobuf-messages)
-    - [Primitives](#primitives)
-    - [Robot Status](#robot-status)
-- [Conventions](#conventions)
-  - [Coordinates](#coordinates)
   - [Angles](#angles)
   - [Convention Diagram](#convention-diagram)
 - [Fullsystem](#fullsystem)
@@ -75,89 +65,6 @@
   - [C++ Templating](#c-templating)
 
 <!--TOC-->
-
-# Architecture Overview
-
-![High-Level Architecture Diagram](images/high_level_architecture.svg)
-
-At a high-level, our system is split into several independent processes that [communicate with each other](#inter-process-communication). Our architecture is designed in this manner to promote decoupling of different features, making our software easier to expand, maintain, and test.
-
-- [**Fullsystem**](#fullsystem) is the program that processes data and makes decisions for a [team](#team) of [robots](#robot). It manages [**Sensor Fusion**](#sensor-fusion), which is responsible for processing and filtering raw data, and the [**AI**](#ai) that makes gameplay decisions.
-
-- [**Thunderscope**](#thunderscope) is an application that provides a GUI for visualizing and interacting with our software.
-
-- The [**Simulator**](#simulator) provides a physics simulation of the world (robots, ball, and field), enabling testing of our gameplay when we don't have access to a real field. This process is optional and used only for development and testing purposes; in a real match, our system will receive data from [SSL-Vision](#ssl-vision).
-
-- [**Thunderloop**](/docs/robot-software-architecture.md#thunderloop) is the software that runs onboard our robots. It is responsible for coordinating communication between our [AI](#ai) computer and the motor and power boards in our robots. It is part our robot software architecture, which is documented [here](/docs/robot-software-architecture.md).
-
-## League-Maintained Software
-
-Our software is designed to interact with the following software developed and maintained by the RoboCup Small Size League:
-
-### SSL Vision
-* This is the shared vision system used by the Small Size League. It is what connects to the cameras above the field, does the vision processing, and transmits the positional data of everything on the field to our [AI](#ai) computers.
-* The GitHub repository can be found [here](https://github.com/RoboCup-SSL/ssl-vision)
-
-### SSL Gamecontroller
-* Sometimes referred to as the "Referee", this is another shared piece of Small Size League software that is used to send gamecontroller and referee commands to the teams. A human and/or [computer auto-referee](#https://ssl.robocup.org/league-software/#auto-referees) controls this application during the games to send the appropriate commands to the robots. For example, some of these commands are what stage the gameplay is in, such as `HALT`, `STOP`, `READY`, or `PLAY`.
-* The GitHub repository can be found [here](https://github.com/RoboCup-SSL/ssl-game-controller)
-
-# Protobuf
-[Protobufs or protocol buffers](https://protobuf.dev/) are used to pass messages between components in our system.
-After building using Bazel, the `.proto` files are generated into `.pb.h` and `.pb.cc` files, which are found in `bazel-out/k8-fastbuild/bin/proto`.
-To include these files in our code, we simply include `proto/<protobuf_filename>.pb.h`.
-
-## Important Protobuf Messages
-These are [protobuf](https://developers.google.com/protocol-buffers/docs/cpptutorial) messages that we define and that are important for understanding how the [AI](#ai) works.
-
-### Primitives
-
-**Primitives** represent the low-level actions that a robot can execute. They are sent to the robots and can be "blindly" executed without knowledge of the [World](#world) and/or the high-level gameplay [strategy](#strategy). Primitives are understood directly by our robot software, which will translate the primitive into motor and power board inputs to make the robot move, dribble, kick, and/or chip as instructed.
-
-Each Primitive is a C++ class that generates an associated `TbotsProto::Primitive` protobuf message that can be sent to the robots.
-
-Primitives act as the abstraction between our [AI](#ai) and our robot software. It splits the responsibility such that the [AI](#ai) is responsible for sending a Primitive to a robot telling it what it wants it to do, and the robot is responsible for making sure it does what it's told.
-
-### Robot Status
-The `TbotsProto::RobotStatus` protobuf message contains information about the status of a single robot. Examples of the information they include are:
-* Robot battery voltage
-* Whether or not the robot senses the ball in the breakbeam
-* The capacitor charge on the robot
-* The temperature of the dribbler motor
-
-Information about the robot status is communicated and stored as `RobotStatus` protobuf messages. [Thunderscope](#thunderscope) displays warnings from incoming `RobotStatus` messages so we can take appropriate action. For example, during a game we may get a "low battery warning" for a certain robot, so we know to substitute it and replace the battery before it dies on the field.
-
-
-# Conventions
-
-Below documents various conventions we use and follow in our software.
-
-## Coordinates
-We use a slightly custom coordinate convention to make it easier to write our code in a consistent and understandable way. This is particularly important for any code handling gameplay logic and positions on the field.
-
-The coordinate system is a simple 2D x-y plane. The x-dimension runs between the friendly and enemy goals, along the longer dimension of the field. The y-dimension runs perpendicular to the x-dimension, along the short dimension of the field.
-
-Because we have to be able to play on either side of a field during a game, this means the "friendly half of the field" will not always be in the positive or negative x part of the coordinate plane. This inconsistency is a problem when we want to specify points like "the friendly net", or "the enemy corner". We can't simple say the friendly net is `(-4.5, 0)` all the time, because this would not be the case if we were defending the other side of the field where the friendly net would be `(4.5, 0)`.
-
-In order to overcome this, our convention is that:
-* The **friendly half** of the field is **always negative x**, and the **enemy half** of the field is **always positive x**
-* `y` is positive to the "left" of someone looking at the enemy goal from the friendly goal
-* The center of the field (inside the center-circle) is the origin / `(0, 0)`
-
-This is easiest to understand in the [diagram](#convention-diagram) below.
-
-Based on what side we are defending, [Sensor Fusion](#sensor-fusion) will transform all the coordinates of incoming data so that it will match our convention. This means that from the perspective of the rest of the system, the friendly half of the field is always negative x and the enemy half is always positive x. Now when we want to tell a robot to move to the friendly goal, we can simply tell it so move to `(-4.5, 0)` and we know this will _always_ be the friendly side. All of our code is written with the assumption in mind.
-
-## Angles
-Going along with our coordinate convention, we have a convention for angles as well. An Angle of `0` is along the positive x-axis (facing the enemy goal), and positive rotation is counter-clockwise (from a perspective above the field, looking at it like a regular x-y plane where +y is "up"). See the [diagram](#convention-diagram) below.
-
-Because of our [Coordinate Conventions](#coordinates), this means that an angle of `0` will always face the enemy net regardless of which side of the field we are actually defending.
-
-## Convention Diagram
-![Coordinate Convention Diagram](images/coordinate_and_angle_convention_diagram.svg)
-
-# Fullsystem
-
 **Fullsystem** processes data and makes decisions for a [team](#team) of [robots](#robot). It manages [Sensor Fusion](#sensor-fusion), which is responsible for processing and filtering raw data, and the [AI](#ai) that makes gameplay decisions. 
 
 Data within Fullsystem is shared between components using the [observer pattern](#observer-pattern); for instance, [Sensor Fusion](#sensor-fusion) and the [Backend](#backend) are `Subject`s that the [AI](#ai) observes.

src/software/ai/navigator/trajectory/collision_evaluator.cpp

@@ -8,16 +8,20 @@ CollisionEvaluator::CollisionEvaluator(const std::vector<ObstaclePtr> &obstacles
 TrajectoryPathWithCost CollisionEvaluator::evaluate(
     const TrajectoryPath &trajectory,
     const std::optional<TrajectoryPathWithCost> &sub_traj_with_cost,
-    const std::optional<double> sub_traj_duration_s)
+    const std::optional<double> sub_traj_duration_s, const std::optional<double> max_cost)
 {
     TrajectoryPathWithCost traj_with_cost(trajectory);
 
+    const double traj_time = trajectory.getTotalTime();
     const double search_end_time_s =
         std::min(trajectory.getTotalTime(), MAX_FUTURE_COLLISION_CHECK_SEC);
+    const Point destination = traj_with_cost.traj_path.getDestination();
 
-    /**
-     * Find the start duration before the trajectory leaves all obstacles
-     */
+    // Initialize total cost as trajectory time
+    double total_cost = traj_time;
+
+
+    // Find the start duration before the trajectory leaves all obstacles
     double first_non_collision_time;
     // Avoid finding the first non-collision time if the cache sub-trajectory
     // has a collision time.
@@ -33,18 +37,35 @@ TrajectoryPathWithCost CollisionEvaluator::evaluate(
     }
     traj_with_cost.collision_duration_front_s = first_non_collision_time;
 
-    /**
-     * Find the duration we're within an obstacle before search_end_time_s
-     */
+    // Add first front collision to total cost
+    total_cost += FRONT_COLLISION_COST_CONST * first_non_collision_time;
+
+    // Return early if current cost already higher than max cost
+    if (total_cost >= max_cost)
+    {
+        traj_with_cost.cost = total_cost;
+        return traj_with_cost;
+    }
+
+    // Find the duration we're within an obstacle before search_end_time_s
     double last_non_collision_time =
         getLastNonCollisionTime(trajectory, search_end_time_s);
     traj_with_cost.collision_duration_back_s =
         search_end_time_s - last_non_collision_time;
 
-    /**
-     * Get the first collision time, excluding the time at the start and end of path
-     * that we may be in an obstacle for.
-     */
+    // Add back collision to total cost
+    total_cost += BACK_COLLISION_COST_CONST * traj_with_cost.collision_duration_back_s;
+
+    // Return early if current cost already higher than max cost
+    if (total_cost >= max_cost)
+    {
+        traj_with_cost.cost = total_cost;
+        return traj_with_cost;
+    }
+
+
+    // Get the first collision time, excluding the time at the start and end of path
+    // that we may be in an obstacle for.
     if (sub_traj_with_cost.has_value() &&
         sub_traj_with_cost->first_collision_time_s < sub_traj_duration_s)
     {
@@ -60,6 +81,22 @@ TrajectoryPathWithCost CollisionEvaluator::evaluate(
         traj_with_cost.colliding_obstacle     = collision.second;
     }
 
+    // Add penalty if mid-trajectory collision exists
+    if (traj_with_cost.colliding_obstacle != nullptr)
+    {
+        total_cost += MID_TRAJ_COST_CONST;
+    }
+
+    //  Add distance from first collision to destination
+    Point first_collision_position =
+        trajectory.getPosition(traj_with_cost.first_collision_time_s);
+    total_cost += (first_collision_position - destination).length();
+
+    //  Add early collision penalty
+    total_cost += std::max(
+        0.0, MAX_FUTURE_COLLISION_CHECK_SEC - traj_with_cost.first_collision_time_s);
+
+    traj_with_cost.cost = total_cost;
     return traj_with_cost;
 }
 

src/software/ai/navigator/trajectory/collision_evaluator.h

@@ -12,6 +12,13 @@
  **/
 class CollisionEvaluator
 {
+    static constexpr double COLLISION_CHECK_STEP_INTERVAL_SEC         = 0.1;
+    static constexpr double FORWARD_COLLISION_CHECK_STEP_INTERVAL_SEC = 0.05;
+    static constexpr double MAX_FUTURE_COLLISION_CHECK_SEC            = 2.0;
+    static constexpr double FRONT_COLLISION_COST_CONST                = 3.0;
+    static constexpr double BACK_COLLISION_COST_CONST                 = 1.0;
+    static constexpr double MID_TRAJ_COST_CONST                       = 6.0;
+
    public:
     /**
      * Constructor
@@ -27,22 +34,20 @@ class CollisionEvaluator
      * If a prefix trajectory is provided, collision information that occurs entirely
      * before the given prefix duration may be reused to avoid redundant collision
      * checking. Collision checks beyond the prefix duration are computed normally.
-     * @param trajectory
-     *        The trajectory to evaluate for collisions and cost.
-     * @param sub_traj_with_cost
-     *        An optional previously evaluated prefix trajectory whose collision
-     *        information may be reused if the prefix fully covers the relevant
-     *        collision interval.
-     * @param sub_traj_duration_s
-     *        The duration (in seconds) of the prefix trajectory within trajectory.
-     * @return
-     *         A TrajectoryPathWithCost< containing the trajectory along with
+     * @param trajectory The trajectory to evaluate for collisions and cost.
+     * @param sub_traj_with_cost An optional previously evaluated prefix trajectory whose
+     * collision information may be reused if the prefix fully covers the relevant
+     * collision interval.
+     * @param sub_traj_duration_s The duration (in seconds) of the prefix trajectory
+     * within trajectory.
+     * @param max_cost Current maximum cost of best path
+     * @return A TrajectoryPathWithCost< containing the trajectory along with
      *         computed collision timing information and total cost.
      */
     TrajectoryPathWithCost evaluate(
         const TrajectoryPath &trajectory,
         const std::optional<TrajectoryPathWithCost> &sub_traj_with_cost,
-        std::optional<double> sub_traj_duration_s);
+        std::optional<double> sub_traj_duration_s, const std::optional<double> max_cost);
 
    private:
     std::vector<ObstaclePtr> obstacles;
@@ -89,7 +94,3 @@ class CollisionEvaluator
     double getLastNonCollisionTime(const TrajectoryPath &traj_path,
                                    const double search_end_time_s) const;
 };
-
-const double COLLISION_CHECK_STEP_INTERVAL_SEC         = 0.1;
-const double FORWARD_COLLISION_CHECK_STEP_INTERVAL_SEC = 0.05;
-const double MAX_FUTURE_COLLISION_CHECK_SEC            = 2.0;

src/software/ai/navigator/trajectory/trajectory_planner.cpp

@@ -109,8 +109,9 @@ std::optional<TrajectoryPath> TrajectoryPlanner::findTrajectory(
                 break;
             }
 
-            TrajectoryPathWithCost full_traj_with_cost = getTrajectoryWithCost(
-                traj_path_to_dest, obstacles, sub_trajectory, connection_time);
+            TrajectoryPathWithCost full_traj_with_cost =
+                getTrajectoryWithCost(traj_path_to_dest, obstacles, sub_trajectory,
+                                      connection_time, best_traj_with_cost.cost);
             full_traj_with_cost.cost += cost_offset;
             if (full_traj_with_cost.cost < best_traj_with_cost.cost)
             {
@@ -140,51 +141,23 @@ TrajectoryPathWithCost TrajectoryPlanner::getDirectTrajectoryWithCost(
     const Point &start, const Point &destination, const Vector &initial_velocity,
     const KinematicConstraints &constraints, const std::vector<ObstaclePtr> &obstacles)
 {
+    // Calculate full new cost regardless by passing in maximum max cost
     return getTrajectoryWithCost(
         TrajectoryPath(std::make_shared<BangBangTrajectory2D>(
                            start, destination, initial_velocity, constraints),
                        BangBangTrajectory2D::generator),
-        obstacles, std::nullopt, std::nullopt);
+        obstacles, std::nullopt, std::nullopt, std::numeric_limits<double>::max());
 }
 
 TrajectoryPathWithCost TrajectoryPlanner::getTrajectoryWithCost(
     const TrajectoryPath &trajectory, const std::vector<ObstaclePtr> &obstacles,
     const std::optional<TrajectoryPathWithCost> &sub_traj_with_cost,
-    const std::optional<double> sub_traj_duration_s)
+    const std::optional<double> sub_traj_duration_s, const std::optional<double> max_cost)
 {
     CollisionEvaluator evaluator(obstacles);
-    TrajectoryPathWithCost traj_with_cost(
-        evaluator.evaluate(trajectory, sub_traj_with_cost, sub_traj_duration_s));
-    traj_with_cost.cost = calculateCost(traj_with_cost);
+    TrajectoryPathWithCost traj_with_cost(evaluator.evaluate(
+        trajectory, sub_traj_with_cost, sub_traj_duration_s, max_cost));
 
-    return traj_with_cost;
-}
-
-double TrajectoryPlanner::calculateCost(
-    const TrajectoryPathWithCost &traj_with_cost) const
-{
-    double total_cost = traj_with_cost.traj_path.getTotalTime();
-
-    // Add a large cost if the trajectory collides with an obstacle
-    // Note that this ignores collisions that may be in at the
-    // start of the trajectory as those are unavoidable by all trajectories.
-    if (traj_with_cost.colliding_obstacle != nullptr)
-    {
-        total_cost += 6.0;
-    }
-
-    // The closer the collision is to the destination, the lower its cost will be
-    Point first_collision_position =
-        traj_with_cost.traj_path.getPosition(traj_with_cost.first_collision_time_s);
-    Point destination = traj_with_cost.traj_path.getDestination();
-    total_cost += (first_collision_position - destination).length();
-
-    total_cost += std::max(
-        0.0, (MAX_FUTURE_COLLISION_CHECK_SEC - traj_with_cost.first_collision_time_s));
 
-    total_cost += 3 * traj_with_cost.collision_duration_front_s;
-
-    total_cost += 1 * traj_with_cost.collision_duration_back_s;
-
-    return total_cost;
+    return traj_with_cost;
 }