Merge branch 'master' of github.com:UBC-Thunderbots/Software into fix_wifi_driver_task_setup_pi_ansible

suchirss · suchirss · commit 7ac617f404e0 · 2026-02-18T23:47:03.000-08:00
diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml
@@ -43,30 +43,53 @@ jobs:
       - name: Check for recent commits
         id: check
         run: |
+          git fetch --tags
+          
+          LATEST_TAG=$(git describe --tags --match "v[0-9]*" --abbrev=0 2>/dev/null || echo "v0.0.0")
+          echo "Latest tag: $LATEST_TAG"
+          
+          # Prevent double release on the same day (unless manual dispatch)
+          if [ "${{ github.event_name }}" != "workflow_dispatch" ] && [ "$LATEST_TAG" != "v0.0.0" ]; then
+            TAG_DATE=$(git log -1 --format=%as "$LATEST_TAG")
+            TODAY=$(date +%Y-%m-%d)
+            echo "Tag Date: $TAG_DATE, Today: $TODAY"
+            
+            if [ "$TAG_DATE" = "$TODAY" ]; then
+              echo "Already released $LATEST_TAG today. Skipping."
+              echo "count=0" >> $GITHUB_OUTPUT
+              exit 0
+            fi
+          fi
+
           if [ "${{ github.event_name }}" = "workflow_dispatch" ]; then
             echo "count=1" >> $GITHUB_OUTPUT
           else
-            COUNT=$(git log --since="1 week ago" --oneline | wc -l)
+            if [ "$LATEST_TAG" = "v0.0.0" ]; then
+               COUNT=$(git log --oneline | wc -l)
+            else
+               COUNT=$(git log "$LATEST_TAG"..HEAD --oneline | wc -l)
+            fi
             echo "count=$COUNT" >> $GITHUB_OUTPUT
           fi
           
       - name: Determine Version Bump
         if: steps.check.outputs.count > 0 
         id: bump_logic
         run: |
-          if [ "${{ github.event.inputs.release_tag }}" = "" ]; then
+          LATEST_TAG=$(git describe --tags --match "v[0-9]*" --abbrev=0 2>/dev/null || echo "v1.0.0")
+          BASE_VERSION=${LATEST_TAG#v}
+          
+          IFS='.' read -r major minor patch <<< "$BASE_VERSION"
+          
+          if [ "${{ github.event.inputs.release_tag }}" != "" ]; then
+            NEW_TAG="${{ github.event.inputs.release_tag }}"
+          else 
             BUMP="patch"
             if [ "$(date +%d)" = "01" ]; then BUMP="minor"; fi
             if [ "${{ github.event_name }}" = "workflow_dispatch" ]; then
               BUMP="${{ github.event.inputs.version_type }}"
             fi
             
-            LATEST_TAG=$(git describe --tags --abbrev=0 2>/dev/null || echo "v1.0.0")
-            # Strip the 'v' prefix
-            BASE_VERSION=${LATEST_TAG#v}
-            
-            IFS='.' read -r major minor patch <<< "$BASE_VERSION"
-            
             if [ "$BUMP" = "major" ]; then
               major=$((major + 1)); minor=0; patch=0
             elif [ "$BUMP" = "minor" ]; then
@@ -76,8 +99,6 @@ jobs:
             fi
             
             NEW_TAG="v$major.$minor.$patch"
-          else 
-            NEW_TAG="${{ github.event.inputs.release_tag }}"
           fi
           echo "tag=$NEW_TAG" >> $GITHUB_OUTPUT
           echo "Using version: $NEW_TAG"
@@ -90,16 +111,6 @@ jobs:
           version: ${{ steps.bump_logic.outputs.tag }}
           tag: ${{ steps.bump_logic.outputs.tag }}
 
-      - name: Create GitHub Release
-        if: steps.check.outputs.count > 0
-        env:
-          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
-        run: |
-          gh release create "${{ steps.bump_logic.outputs.tag }}" \
-            --title "${{ steps.bump_logic.outputs.tag }}" \
-            --generate-notes \
-            --draft 
-
   upload_assets:
     needs: prepare_release
     if: needs.prepare_release.outputs.should_release == 'true'
@@ -128,10 +139,13 @@ jobs:
         run: | 
           cd src
           TAG="${{ needs.prepare_release.outputs.tag }}"
+          
           bazel build --show_timestamps --copt=-O3 --verbose_failures \
           -- //software:unix_full_system_tar_gen
-          mv bazel-bin/software/unix_full_system_tar_gen.tar.gz "${{ runner.temp }}/unix_full_system_${{ needs.prepare_release.outputs.tag }}_${{ matrix.platform }}.tar.gz" 
-          gh release upload "$TAG" "${{ runner.temp }}/unix_full_system_${{ needs.prepare_release.outputs.tag }}_${{ matrix.platform }}.tar.gz"
+          
+          ARTIFACT_NAME="unix_full_system_${TAG}_${{ matrix.platform }}.tar.gz"
+          mv bazel-bin/software/unix_full_system_tar_gen.tar.gz "${{ runner.temp }}/$ARTIFACT_NAME" 
+          gh release upload "$TAG" "${{ runner.temp }}/$ARTIFACT_NAME"
 
   publish_release:
     needs: [prepare_release, upload_assets]
diff --git a/src/cli/cli_params.py b/src/cli/cli_params.py
@@ -89,3 +89,5 @@ class Platform(str, Enum):
 EnableThunderscopeOption = Annotated[bool, Option("-t", "--enable_thunderscope")]
 EnableVisualizerOption = Annotated[bool, Option("-v", "--enable_visualizer")]
 StopAIOnStartOption = Annotated[bool, Option("-t", "--stop_ai_on_start")]
+
+JobsOption = Annotated[str, Option("-j", "--jobs")]
diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp
@@ -123,8 +123,6 @@ MovePrimitive::generatePrimitiveProtoMessage(
 
     if (!traj_path.has_value())
     {
-        LOG(WARNING) << "Could not find trajectory path for robot " << robot.id()
-                     << " to move to " << destination;
         return std::make_pair(std::nullopt, std::move(createStopPrimitiveProto()));
     }
 
diff --git a/src/software/ai/navigator/trajectory/collision_evaluator.cpp b/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 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;
 }
 
diff --git a/src/software/ai/navigator/trajectory/collision_evaluator.h b/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, 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;
diff --git a/src/software/ai/navigator/trajectory/trajectory_path_with_cost.h b/src/software/ai/navigator/trajectory/trajectory_path_with_cost.h
@@ -32,9 +32,9 @@ class TrajectoryPathWithCost
     // 0 if the trajectory does not end within an obstacle.
     double collision_duration_back_s = 0.0;
 
-    // The time and obstacle at which the trajectory first collides with an obstacle
-    // first_collision_time_s is set to infinity and colliding_obstacle is nullptr
-    // if the trajectory does not collide.
+    // The time and obstacle at which the trajectory first collides with an
+    // obstacle first_collision_time_s is set to infinity and colliding_obstacle is
+    // nullopt if the trajectory does not collide.
     // Note that collisions that the trajectory starts or ends in are not considered as
     // those are unavoidable + they are handled by collision_duration_front_s and
     // collision_duration_back_s.
diff --git a/src/software/ai/navigator/trajectory/trajectory_planner.cpp b/src/software/ai/navigator/trajectory/trajectory_planner.cpp
@@ -4,6 +4,7 @@
 #include "software/geom/algorithms/contains.h"
 #include "software/geom/algorithms/distance.h"
 
+
 TrajectoryPlanner::TrajectoryPlanner()
     : relative_sub_destinations(getRelativeSubDestinations())
 {
@@ -109,8 +110,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)
             {
@@ -133,58 +135,47 @@ std::optional<TrajectoryPath> TrajectoryPlanner::findTrajectory(
         }
     }
 
-    return best_traj_with_cost.traj_path;
+    // In move primitive, a stop primitive is created when trajectory path is null.
+    // Check if there is an unavoidable collision, and return a null opt if such
+    // collision exist on best path
+    double collision_velocity =
+        best_traj_with_cost.traj_path
+            .getVelocity(best_traj_with_cost.first_collision_time_s)
+            .length();
+    if (best_traj_with_cost.collides() &&
+        best_traj_with_cost.first_collision_time_s <
+            UNAVOIDABLE_COLLISION_TIME_THRESHOLD_S &&
+        collision_velocity > UNAVOIDABLE_COLLISION_VELOCITY_THRESHOLD_M_S)
+    {
+        return std::nullopt;
+    }
+    else
+    {
+        return best_traj_with_cost.traj_path;
+    }
 }
 
 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, 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;
 }
diff --git a/src/software/ai/navigator/trajectory/trajectory_planner.h b/src/software/ai/navigator/trajectory/trajectory_planner.h
diff --git a/src/tbots.py b/src/tbots.py

Original file line number	Diff line number	Diff line change
`@@ -123,8 +123,6 @@ MovePrimitive::generatePrimitiveProtoMessage(`
`123`	`123`
`124`	`124`	`if (!traj_path.has_value())`
`125`	`125`	`{`
`126`		`- LOG(WARNING) << "Could not find trajectory path for robot " << robot.id()`
`127`		`- << " to move to " << destination;`
`128`	`126`	`return std::make_pair(std::nullopt, std::move(createStopPrimitiveProto()));`
`129`	`127`	`}`
`130`	`128`