Skip to content

Commit 75a08b1

Browse files
pre-commit-ci-lite[bot]pre-commit-ci-lite[bot]
authored
[pre-commit.ci lite] apply automatic fixes
1 parent 4991a83 commit 75a08b1

5 files changed

Lines changed: 54 additions & 50 deletions

File tree

‎src/software/ai/hl/stp/tactic/move_primitive.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -113,7 +113,7 @@ MovePrimitive::generatePrimitiveProtoMessage(
113113

114114
if (!traj_path.has_value())
115115
{
116-
LOG(WARNING) << "Could not find trajectory path for robot " << robot.id()
116+
LOG(WARNING) << "Could not find trajectory path for robot " << robot.id()
117117
<< " to move to " << destination;
118118
return std::make_pair(std::nullopt, std::move(createStopPrimitiveProto()));
119119
}

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

Lines changed: 37 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,8 @@
11
#include "software/ai/navigator/trajectory/collision_evaluator.h"
22

33
const double FRONT_COLLISION_COST_CONST = 3.0;
4-
const double BACK_COLLISION_COST_CONST = 1.0;
5-
const double MID_TRAJ_COST_CONST = 6.0;
4+
const double BACK_COLLISION_COST_CONST = 1.0;
5+
const double MID_TRAJ_COST_CONST = 6.0;
66
CollisionEvaluator::CollisionEvaluator(const std::vector<ObstaclePtr> &obstacles)
77
: obstacles(obstacles)
88
{
@@ -11,18 +11,17 @@ CollisionEvaluator::CollisionEvaluator(const std::vector<ObstaclePtr> &obstacles
1111
TrajectoryPathWithCost CollisionEvaluator::evaluate(
1212
const TrajectoryPath &trajectory,
1313
const std::optional<TrajectoryPathWithCost> &sub_traj_with_cost,
14-
const std::optional<double> sub_traj_duration_s,
15-
const std::optional<double> max_cost)
14+
const std::optional<double> sub_traj_duration_s, const std::optional<double> max_cost)
1615
{
17-
TrajectoryPathWithCost traj_with_cost(trajectory);
16+
TrajectoryPathWithCost traj_with_cost(trajectory);
1817

19-
const double traj_time = trajectory.getTotalTime();
18+
const double traj_time = trajectory.getTotalTime();
2019
const double search_end_time_s =
2120
std::min(trajectory.getTotalTime(), MAX_FUTURE_COLLISION_CHECK_SEC);
2221
const Point destination = traj_with_cost.traj_path.getDestination();
2322

24-
// Initialize total cost as trajectory time
25-
double total_cost = traj_time;
23+
// Initialize total cost as trajectory time
24+
double total_cost = traj_time;
2625

2726

2827
// Find the start duration before the trajectory leaves all obstacles
@@ -41,29 +40,31 @@ TrajectoryPathWithCost CollisionEvaluator::evaluate(
4140
}
4241
traj_with_cost.collision_duration_front_s = first_non_collision_time;
4342

44-
// Add first front collision to total cost
45-
total_cost += FRONT_COLLISION_COST_CONST * first_non_collision_time;
43+
// Add first front collision to total cost
44+
total_cost += FRONT_COLLISION_COST_CONST * first_non_collision_time;
4645

4746
// Return early if current cost already higher than max cost
48-
if (total_cost >= max_cost){
49-
traj_with_cost.cost = total_cost;
50-
return traj_with_cost;
51-
}
47+
if (total_cost >= max_cost)
48+
{
49+
traj_with_cost.cost = total_cost;
50+
return traj_with_cost;
51+
}
5252

5353
// Find the duration we're within an obstacle before search_end_time_s
5454
double last_non_collision_time =
5555
getLastNonCollisionTime(trajectory, search_end_time_s);
5656
traj_with_cost.collision_duration_back_s =
5757
search_end_time_s - last_non_collision_time;
5858

59-
// Add back collision to total cost
60-
total_cost += BACK_COLLISION_COST_CONST * traj_with_cost.collision_duration_back_s;
59+
// Add back collision to total cost
60+
total_cost += BACK_COLLISION_COST_CONST * traj_with_cost.collision_duration_back_s;
6161

6262
// Return early if current cost already higher than max cost
63-
if (total_cost >= max_cost){
64-
traj_with_cost.cost = total_cost;
65-
return traj_with_cost;
66-
}
63+
if (total_cost >= max_cost)
64+
{
65+
traj_with_cost.cost = total_cost;
66+
return traj_with_cost;
67+
}
6768

6869

6970
// Get the first collision time, excluding the time at the start and end of path
@@ -82,20 +83,23 @@ TrajectoryPathWithCost CollisionEvaluator::evaluate(
8283
traj_with_cost.first_collision_time_s = collision.first;
8384
traj_with_cost.colliding_obstacle = collision.second;
8485
}
85-
86+
8687
// Add 6.0 to collision if mid-trajectory collision exist
87-
if (traj_with_cost.colliding_obstacle != nullptr){
88-
total_cost += MID_TRAJ_COST_CONST;
89-
}
90-
91-
// Add distance from first collision to destination
92-
Point first_collision_position = trajectory.getPosition(traj_with_cost.first_collision_time_s);
93-
total_cost += (first_collision_position - destination).length();
94-
95-
// Add early collision penalty
96-
total_cost += std::max(0.0, MAX_FUTURE_COLLISION_CHECK_SEC - traj_with_cost.first_collision_time_s);
97-
98-
traj_with_cost.cost = total_cost;
88+
if (traj_with_cost.colliding_obstacle != nullptr)
89+
{
90+
total_cost += MID_TRAJ_COST_CONST;
91+
}
92+
93+
// Add distance from first collision to destination
94+
Point first_collision_position =
95+
trajectory.getPosition(traj_with_cost.first_collision_time_s);
96+
total_cost += (first_collision_position - destination).length();
97+
98+
// Add early collision penalty
99+
total_cost += std::max(
100+
0.0, MAX_FUTURE_COLLISION_CHECK_SEC - traj_with_cost.first_collision_time_s);
101+
102+
traj_with_cost.cost = total_cost;
99103
return traj_with_cost;
100104
}
101105

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

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -28,11 +28,12 @@ class CollisionEvaluator
2828
* before the given prefix duration may be reused to avoid redundant collision
2929
* checking. Collision checks beyond the prefix duration are computed normally.
3030
* @param trajectory The trajectory to evaluate for collisions and cost.
31-
* @param sub_traj_with_cost An optional previously evaluated prefix trajectory whose collision
32-
* information may be reused if the prefix fully covers the relevant
31+
* @param sub_traj_with_cost An optional previously evaluated prefix trajectory whose
32+
* collision information may be reused if the prefix fully covers the relevant
3333
* collision interval.
34-
* @param sub_traj_duration_s The duration (in seconds) of the prefix trajectory within trajectory.
35-
* @param max_cost Current maximum cost of best path
34+
* @param sub_traj_duration_s The duration (in seconds) of the prefix trajectory
35+
* within trajectory.
36+
* @param max_cost Current maximum cost of best path
3637
* @return A TrajectoryPathWithCost< containing the trajectory along with
3738
* computed collision timing information and total cost.
3839
*/

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

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -109,8 +109,9 @@ std::optional<TrajectoryPath> TrajectoryPlanner::findTrajectory(
109109
break;
110110
}
111111

112-
TrajectoryPathWithCost full_traj_with_cost = getTrajectoryWithCost(
113-
traj_path_to_dest, obstacles, sub_trajectory, connection_time, best_traj_with_cost.cost);
112+
TrajectoryPathWithCost full_traj_with_cost =
113+
getTrajectoryWithCost(traj_path_to_dest, obstacles, sub_trajectory,
114+
connection_time, best_traj_with_cost.cost);
114115
full_traj_with_cost.cost += cost_offset;
115116
if (full_traj_with_cost.cost < best_traj_with_cost.cost)
116117
{
@@ -133,14 +134,14 @@ std::optional<TrajectoryPath> TrajectoryPlanner::findTrajectory(
133134
}
134135
}
135136

136-
return best_traj_with_cost.traj_path;
137+
return best_traj_with_cost.traj_path;
137138
}
138139

139140
TrajectoryPathWithCost TrajectoryPlanner::getDirectTrajectoryWithCost(
140141
const Point &start, const Point &destination, const Vector &initial_velocity,
141142
const KinematicConstraints &constraints, const std::vector<ObstaclePtr> &obstacles)
142143
{
143-
// Calculate full new cost regardless by passing in maximum max cost
144+
// Calculate full new cost regardless by passing in maximum max cost
144145
return getTrajectoryWithCost(
145146
TrajectoryPath(std::make_shared<BangBangTrajectory2D>(
146147
start, destination, initial_velocity, constraints),
@@ -151,14 +152,12 @@ TrajectoryPathWithCost TrajectoryPlanner::getDirectTrajectoryWithCost(
151152
TrajectoryPathWithCost TrajectoryPlanner::getTrajectoryWithCost(
152153
const TrajectoryPath &trajectory, const std::vector<ObstaclePtr> &obstacles,
153154
const std::optional<TrajectoryPathWithCost> &sub_traj_with_cost,
154-
const std::optional<double> sub_traj_duration_s,
155-
const std::optional<double> max_cost)
155+
const std::optional<double> sub_traj_duration_s, const std::optional<double> max_cost)
156156
{
157157
CollisionEvaluator evaluator(obstacles);
158-
TrajectoryPathWithCost traj_with_cost(
159-
evaluator.evaluate(trajectory, sub_traj_with_cost, sub_traj_duration_s, max_cost));
160-
158+
TrajectoryPathWithCost traj_with_cost(evaluator.evaluate(
159+
trajectory, sub_traj_with_cost, sub_traj_duration_s, max_cost));
160+
161161

162162
return traj_with_cost;
163163
}
164-

‎src/software/ai/navigator/trajectory/trajectory_planner.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -59,14 +59,14 @@ class TrajectoryPlanner
5959
* @param sub_traj_with_cost Optional cached trajectory path with cost of the sub
6060
* trajectory
6161
* @param sub_traj_duration_s Optional duration of the cached sub_traj_with_cost
62-
* @param max_cost Current maximum cost among calculated trajectories
62+
* @param max_cost Current maximum cost among calculated trajectories
6363
* @return The trajectory path with its cost
6464
*/
6565
TrajectoryPathWithCost getTrajectoryWithCost(
6666
const TrajectoryPath &trajectory, const std::vector<ObstaclePtr> &obstacles,
6767
const std::optional<TrajectoryPathWithCost> &sub_traj_with_cost,
6868
const std::optional<double> sub_traj_duration_s,
69-
const std::optional<double> max_cost);
69+
const std::optional<double> max_cost);
7070

7171

7272

0 commit comments

Comments
 (0)