Skip to content

Fix Code Style On common_utils #2526

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
rhkoulen merged 2 commits into from
Apr 8, 2026
Merged

Fix Code Style On common_utils #2526

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e0c3439
automated style fixes
rhkoulen Apr 8, 2026
7edbe5c
I don't like the way it styled this
rkoulen3 Apr 8, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 8 additions & 7 deletions src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,14 +33,15 @@ class SoloOffense : public Position {

private:
enum State {
IDLE, // The nothing doer
MARKER, // Aggressively sit between ball and goal pos
TO_BALL, // Collect
ROTATE, // After successful collect, aim and fire
KICK // The more naive line kick
IDLE, // The nothing doer
MARKER, // Aggressively sit between ball and goal pos
TO_BALL, // Collect
ROTATE, // After successful collect, aim and fire
KICK // The more naive line kick
};
State kick_strategy_ = TO_BALL; // set to TO_BALL for collect kicking, set to KICK for line kicking
planning::LinearMotionInstant kick_target_; // aiming point
State kick_strategy_ =
TO_BALL; // set to TO_BALL for collect kicking, set to KICK for line kicking
planning::LinearMotionInstant kick_target_; // aiming point

State current_state_ = IDLE;

Expand Down
136 changes: 79 additions & 57 deletions src/rj_strategy/include/rj_strategy/agent/position_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#pragma once

#include <cstdlib>
#include <cmath>
#include <cstdlib>
#include <limits>
#include <string>
#include <unordered_map>
Expand All @@ -23,23 +23,25 @@
/**
* This file is just to collect a bunch of common utilities.
* Longterm TODO:
* - a lot functions in here do pairwise distance between various objects. can we have a coordinator that does a grand pairwise distance we can just reference?
* - a lot functions in here do pairwise distance between various objects. can we have a
* coordinator that does a grand pairwise distance we can just reference?
* Rules:
* - these functions have to be stateless and context-free
* - try to keep big object args as const references (to minimize overhead)
* - try to keep the execution time minimal (to minimize overhead)
* - try to keep function names unnecessarily verbose and accurate (to minimize headache)
* - remember that if you change something, other people may be using that; don't fundamentally change invariants
* if you're doing something a little different, do it in your own file, or add a new util, avoid revising where possible
* - if you love me, do not use auto. figure out what type you want
* - remember that if you change something, other people may be using that; don't fundamentally
* change invariants if you're doing something a little different, do it in your own file,
* or add a new util, avoid revising where possible
* - please don't use auto. figure out what type you want :)
*/

// namespace strategy {

// Example
/**
* @brief Euclidean distance between two points.
*
*
* @param a The first point.
* @param b The second point.
* @return the Euclidean distance between a and b
Expand All @@ -48,12 +50,10 @@ inline double distance(const rj_geometry::Point& a, const rj_geometry::Point& b)
return (a - b).mag();
}



// Geometry confirmation
/**
* @brief Determines whether the ball is in bounds (whole field rectangle).
*
*
* @param world_state (often named last_world_state_ in Position subclasses)
* @param field_dimensions (often named field_dimensions_ in Position subclasses)
* @return if ball.position is in bounds
Expand All @@ -65,76 +65,85 @@ inline bool ball_on_field(const WorldState* world_state, const FieldDimensions&

/**
* @brief Determines whether the ball is in our defense area (red region around goalie box).
*
*
* @param world_state (often named last_world_state_ in Position subclasses)
* @param field_dimensions (often named field_dimensions_ in Position subclasses)
* @return if ball.position is in our goalie box
*/
inline bool ball_in_our_defense_area(const WorldState* world_state, const FieldDimensions& field_dimensions) {
inline bool ball_in_our_defense_area(const WorldState* world_state,
const FieldDimensions& field_dimensions) {
const rj_geometry::Point& ball_point = world_state->ball.position;
return field_dimensions.our_defense_area().contains_point(ball_point);
}

/**
* @brief Determines whether the ball is in our defense area (red region around goalie box).
*
*
* @param world_state (often named last_world_state_ in Position subclasses)
* @param field_dimensions (often named field_dimensions_ in Position subclasses)
* @return if ball.position is in their goalie box
*/
inline bool ball_in_their_defense_area(const WorldState* world_state, const FieldDimensions& field_dimensions) {
inline bool ball_in_their_defense_area(const WorldState* world_state,
const FieldDimensions& field_dimensions) {
const rj_geometry::Point& ball_point = world_state->ball.position;
return field_dimensions.their_defense_area().contains_point(ball_point);
}

// There is also "penalty area". This refers to the actual white lines of the goalie box.
// Annoyingly, this is what is called "defense area" in the rules document.
// I have chosen not to expose this, since we have a widened red region around the box lines for rules compliance.
// We're not yet at the skill level to bother with tussles at the defense lines.
// I have chosen not to expose this, since we have a widened red region around the box lines for
// rules compliance. We're not yet at the skill level to bother with tussles at the defense lines.

/**
* @return Determines whether the ball is in an area that non-goalies can reach.
*
*
* @param world_state (often named last_world_state_ in Position subclasses)
* @param field_dimensions (often named field_dimensions_ in Position subclasses)
* @return if ball.position is in playable space
*/
inline bool ball_in_play_area(const WorldState* world_state, const FieldDimensions& field_dimensions) {
inline bool ball_in_play_area(const WorldState* world_state,
const FieldDimensions& field_dimensions) {
return ball_on_field(world_state, field_dimensions) &&
!ball_in_our_defense_area(world_state, field_dimensions) &&
!ball_in_their_defense_area(world_state, field_dimensions);
}


// Shot calculation
/**
* @brief Gets the shortest angular distance from a proposed shotline to an opponent.
* Can be thought of as half of the the arc of the largest circular sector centered around the shot that has no opponent bits in it.
*
* Can be thought of as half of the the arc of the largest circular sector centered around the shot
* that has no opponent bits in it.
*
* The idea is that we want to take shots that are far away from opponents.
* Currently we pick the one that is furthest angularly from an opponent, to minimize the chance of them blocking.
* 0 means that there is an opponent directly on the shotline.
* pi/2 means that there is no opponent bot in the forward FOV.
*
* Currently we pick the one that is furthest angularly from an opponent, to minimize the chance of
* them blocking. 0 means that there is an opponent directly on the shotline. pi/2 means that there
* is no opponent bot in the forward FOV.
*
* @param origin where the ball starts
* @param shot where the shot is aimed
* @param world_state (often named last_world_state_ in Position subclasses)
* @return the size of the smallest angle to defender
*/
inline double shot_clearance(const rj_geometry::Point& origin, const rj_geometry::Point& shot, const WorldState* world_state) {
inline double shot_clearance(const rj_geometry::Point& origin, const rj_geometry::Point& shot,
const WorldState* world_state) {
rj_geometry::Point shot_vec = shot - origin;
const std::vector<RobotState>& their_robots = world_state->their_robots;
double min_angle = M_PI_2;
for (const RobotState& enemy : their_robots) { // TODO: our robots are not programmed to dodge our own shots, frankly, we may need to consider them opponents
for (const RobotState& enemy :
their_robots) { // TODO: our robots are not programmed to dodge our own shots, frankly, we
// may need to consider them opponents
rj_geometry::Point enemy_vec = enemy.pose.position() - origin;
if (enemy_vec.dot(shot_vec) < 0) {
continue; // if the enemy is behind us, ignore them
continue; // if the enemy is behind us, ignore them
}

double projection = enemy_vec.dot(shot_vec) / shot_vec.dot(shot_vec);
double dist_to_block = (enemy_vec - projection * shot_vec).mag(); // magnitude of the perpendicular component
dist_to_block = std::max(0.0, dist_to_block - kRobotRadius - kBallRadius); // (robot positions are the center, but even glancing a shot will usually block)
double dist_along_shot = projection * shot_vec.mag(); // magnitude of the tangent component
double dist_to_block =
(enemy_vec - projection * shot_vec).mag(); // magnitude of the perpendicular component
dist_to_block = std::max(0.0, dist_to_block - kRobotRadius -
kBallRadius); // (robot positions are the center, but
// even glancing a shot will usually block)
double dist_along_shot = projection * shot_vec.mag(); // magnitude of the tangent component

double clearance_angle = std::atan2(dist_to_block, dist_along_shot);
min_angle = std::min(min_angle, clearance_angle);
Expand All @@ -145,50 +154,58 @@ inline double shot_clearance(const rj_geometry::Point& origin, const rj_geometry
/**
* @brief Gets a good shot worth taking, given our pratical realities.
* Use this function and rest assured it will go in on an undefended goal.
*
*
* @param world_state (often named last_world_state_ in Position subclasses)
* @param field_dimensions (often named field_dimensions_ in Position subclasses)
* @return a point to aim at for the good shot
* * currently just returns the center of the goal
*/
inline rj_geometry::Point calculate_a_shot([[maybe_unused]] const WorldState* world_state, const FieldDimensions& field_dimensions) {
inline rj_geometry::Point calculate_a_shot([[maybe_unused]] const WorldState* world_state,
const FieldDimensions& field_dimensions) {
// TODO: could be a fun collab with hardware!
// Can we quantify our uncertainty and aim closer to the goalposts when close enough?
return field_dimensions.their_goal_loc();
}

/**
* @brief Gets a the best shot available, given excellent aim.
* The definition of "best" may need refining, but I think my approach is sound.
*
* The definition of "best" may need refining, but I think my approach is sound.
*
* @param world_state (often named last_world_state_ in Position subclasses)
* @param field_dimensions (often named field_dimensions_ in Position subclasses)
* @param granularity the spacing between considered points on the goal line (meters)
* @param ignore_posts whether to count the goalposts as candidate shots (you should use True, aiming for the goalposts is stupid)
* @param ignore_posts whether to count the goalposts as candidate shots (you should use True,
* aiming for the goalposts is stupid)
* @return a point to aim at for the best shot
*/
inline rj_geometry::Point calculate_best_shot(const WorldState* world_state, const FieldDimensions& field_dimensions, double granularity, bool ignore_posts) {
inline rj_geometry::Point calculate_best_shot(const WorldState* world_state,
const FieldDimensions& field_dimensions,
double granularity, bool ignore_posts) {
// Geometry
rj_geometry::Point their_goal_pos = field_dimensions.their_goal_loc(); // returns center of goal
if (granularity <= 0.0) { // protection
SPDLOG_ERROR("Invalid granularity value passed into calculate_best_shot, must use a positive float.");
rj_geometry::Point their_goal_pos =
field_dimensions.their_goal_loc(); // returns center of goal
if (granularity <= 0.0) { // protection
SPDLOG_ERROR(
"Invalid granularity value passed into calculate_best_shot, must use a positive "
"float.");
return their_goal_pos;
}
double goal_width = field_dimensions.goal_width();
rj_geometry::Point negXmost_shot = their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0.0);
rj_geometry::Point posXmost_shot = their_goal_pos + rj_geometry::Point(goal_width / 2.0, 0.0);
rj_geometry::Point increment(granularity, 0.0);
if (ignore_posts) {
negXmost_shot = negXmost_shot + increment;
negXmost_shot = negXmost_shot + increment;
posXmost_shot = posXmost_shot - increment;
}
rj_geometry::Point ball_pos = world_state->ball.position;

// Argmaxxing
rj_geometry::Point best_shot(0.0, 0.0);
double curr_clearance;
double best_clearance = -1.0; // clearance is in [0,pi/2], so this will be immediately overwritten

double best_clearance =
-1.0; // clearance is in [0,pi/2], so this will be immediately overwritten

rj_geometry::Point curr_shot = negXmost_shot;
while (curr_shot.x() <= posXmost_shot.x()) {
curr_clearance = shot_clearance(ball_pos, curr_shot, world_state);
Expand All @@ -208,11 +225,13 @@ inline rj_geometry::Point calculate_best_shot(const WorldState* world_state, con
}

// Possession calculation
// TODO: these functions should also account for rotation. a robot doesn't have possession if the ball is sitting at its rear motors
// TODO: overload these functions so that we can have some default possession_radius, such that people using it don't need to turn on they brain
// TODO: these functions should also account for rotation. a robot doesn't have possession if the
// ball is sitting at its rear motors
// TODO: overload these functions so that we can have some default possession_radius, such that
// people using it don't need to turn on they brain
/**
* @brief Determines whether the enemy has possession of the ball.
*
*
* @param world_state (often named last_world_state_ in Position subclasses)
* @param possession_radius the distance at which a robot is defined to "have" the ball (m)
* @return do they have ball
Expand All @@ -231,7 +250,7 @@ inline bool they_have_ball(const WorldState* world_state, double possession_radi

/**
* @brief Determines whether our team has possession of the ball.
*
*
* @param world_state (often named last_world_state_ in Position subclasses)
* @param possession_radius the distance at which a robot is defined to "have" the ball (m)
* @return do we have ball
Expand All @@ -250,12 +269,13 @@ inline bool we_have_ball(const WorldState* world_state, double possession_radius

/**
* @brief Determines whether a specific robot has possession of the ball.
*
*
* @param world_state (often named last_world_state_ in Position subclasses)
* @param possession_radius the distance at which a robot is defined to "have" the ball (m)
* @return does it have ball
*/
inline bool robot_has_ball(const WorldState* world_state, const RobotState& robot, double possession_radius) {
inline bool robot_has_ball(const WorldState* world_state, const RobotState& robot,
double possession_radius) {
rj_geometry::Point ball_pos = world_state->ball.position;
rj_geometry::Point robot_pos = robot.pose.position();
if (distance(ball_pos, robot_pos) < possession_radius) {
Expand All @@ -268,18 +288,20 @@ inline bool robot_has_ball(const WorldState* world_state, const RobotState& robo
// Kick speed calculation
/**
* @brief Provides a good suggestion for kick speed, designed for passing.
*
*
* Motion intents take an integer from [0,15]. This provides that.
*
*
* @param distance_to_target distance from kicker to target (m)
* @param intended_velo_at_target the velocity you want the ball to be going when it reaches the target (m/s)
* @param intended_velo_at_target the velocity you want the ball to be going when it reaches the
* target (m/s)
* @return an int to shove in the motion command
*/
inline int calculate_kick_speed(double distance_to_target, [[maybe_unused]] double intended_velo_at_target) {
inline int calculate_kick_speed(double distance_to_target,
[[maybe_unused]] double intended_velo_at_target) {
// TODO: this could be a fun collab with hardware!
// v^2 = v_initial^2 - 2ad, where a is a measure of the constant deceleration applied from the ground
// Frankly, that changes from pitch to pitch, so it should probably be parametrized in rqt.
// We would also then need a map from v_initial to power[0,15]
// v^2 = v_initial^2 - 2ad, where a is a measure of the constant deceleration applied from the
// ground Frankly, that changes from pitch to pitch, so it should probably be parametrized in
// rqt. We would also then need a map from v_initial to power[0,15]

// The below code is very dumb. I stole it from Offense.
if (distance_to_target < 0.6) {
Expand All @@ -293,9 +315,9 @@ inline int calculate_kick_speed(double distance_to_target, [[maybe_unused]] doub

/**
* @brief Provides the max allowed kick speed, designed for shooting.
*
*
* Motion intents take an integer from [0,15]. This provides that.
*
*
* @return 7
*/
inline int max_kick_speed() {
Expand Down
17 changes: 11 additions & 6 deletions src/rj_strategy/src/agent/position/offense.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -170,7 +170,8 @@ Offense::State Offense::next_state() {
}

case SHOOTING: {
if (!ball_in_play_area(last_world_state_, field_dimensions_) || check_is_done() || !has_open_shot() || !can_steal_ball()) {
if (!ball_in_play_area(last_world_state_, field_dimensions_) || check_is_done() ||
!has_open_shot() || !can_steal_ball()) {
return DEFAULT;
}
// if (distance_to_ball() > kOwnBallRadius) {
Expand Down Expand Up @@ -275,13 +276,14 @@ std::optional<RobotIntent> Offense::state_to_task(RobotIntent intent) {

case SHOOTING: {
// link kick because collect is garbage
planning::LinearMotionInstant target{calculate_best_shot(last_world_state_, field_dimensions_, 0.04, true)};
planning::LinearMotionInstant target{
calculate_best_shot(last_world_state_, field_dimensions_, 0.04, true)};

auto shoot_cmd =
planning::MotionCommand{"line_kick", target, planning::FaceTarget{}, true};
intent.motion_command = shoot_cmd;
intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM;
intent.kick_speed = max_kick_speed(); // Integer value in [0,15]
intent.kick_speed = max_kick_speed(); // Integer value in [0,15]
return intent;
}
}
Expand Down Expand Up @@ -415,9 +417,12 @@ void Offense::derived_acknowledge_ball_in_transit() {
}

bool Offense::has_open_shot() const {
rj_geometry::Point best_shot = calculate_best_shot(last_world_state_, field_dimensions_, 0.04, true);
double clearance_angle = shot_clearance(last_world_state_->ball.position, best_shot, last_world_state_);
return clearance_angle >= 0.05; // if there's >= 3 degrees (0.05 radians) of clearance, that's an open shot
rj_geometry::Point best_shot =
calculate_best_shot(last_world_state_, field_dimensions_, 0.04, true);
double clearance_angle =
shot_clearance(last_world_state_->ball.position, best_shot, last_world_state_);
return clearance_angle >=
0.05; // if there's >= 3 degrees (0.05 radians) of clearance, that's an open shot
}


Expand Down
Loading
Loading