Skip to content

Comp merge: ateam_game_state #424

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
barulicm merged 4 commits into from
Jul 7, 2026
Merged

Comp merge: ateam_game_state #424

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
d10fd2d
Imports ateam_game_state changes from comp branch
barulicm Jul 6, 2026
2b19d10
Fixes build failures.
barulicm Jul 6, 2026
00c653b
Fixes style errors
barulicm Jul 7, 2026
63e2d19
Merge branch 'main' into dev/barulicm/comp/game_state
barulicm Jul 7, 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
4 changes: 2 additions & 2 deletions ateam_kenobi/src/core/path_planning/obstacles.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -172,11 +172,11 @@ bool IsPointInBounds(
ateam_geometry::Rectangle pathable_region(ateam_geometry::Point(-x_bound, -y_bound),
ateam_geometry::Point(x_bound, y_bound));

if (world.field.ignore_side == ateam_msgs::msg::FieldInfo::IGNORE_SIDE_THEIRS) {
if (world.field.ignore_side == ateam_game_state::IgnoreSide::Theirs) {
pathable_region = ateam_geometry::Rectangle(
ateam_geometry::Point(-x_bound, -y_bound),
ateam_geometry::Point(0, y_bound));
} else if (world.field.ignore_side == ateam_msgs::msg::FieldInfo::IGNORE_SIDE_OURS) {
} else if (world.field.ignore_side == ateam_game_state::IgnoreSide::Ours) {
pathable_region = ateam_geometry::Rectangle(
ateam_geometry::Point(0, y_bound),
ateam_geometry::Point(x_bound, -y_bound));
Expand Down
19 changes: 17 additions & 2 deletions ateam_kenobi/test/robot_assignment_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,8 @@ TEST(RobotAssignmentTest, OneRobotOneGoal)
{
std::vector<Robot> robots {
{1, true, true, ateam_geometry::Point(1, 2), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false}
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0, ateam_geometry::Vector{}, 0.0,
false, true, false}
};
std::vector<ateam_geometry::Point> goals = {
ateam_geometry::Point(3, 4)
Expand All @@ -68,9 +68,11 @@ TEST(RobotAssignmentTest, TwoRobotsOneGoal)
{
std::vector<Robot> robots {
{1, true, true, ateam_geometry::Point(1, 2), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false},
{2, true, true, ateam_geometry::Point(3, 4), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false}
};
Expand All @@ -85,9 +87,11 @@ TEST(RobotAssignmentTest, TwoRobotsTwoGoals)
{
std::vector<Robot> robots {
{1, true, true, ateam_geometry::Point(1, 2), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false},
{2, true, true, ateam_geometry::Point(3, 4), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false}
};
Expand All @@ -106,9 +110,11 @@ TEST(RobotAssignmentTest, TwoRobotsSameDistance)
{
std::vector<Robot> robots {
{1, true, true, ateam_geometry::Point(0, 1), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false},
{2, true, true, ateam_geometry::Point(0, -1), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false}
};
Expand All @@ -127,6 +133,7 @@ TEST(RobotAssignmentTest, DisallowAssigningDisallowedRobots)
{
std::vector<Robot> robots {
{1, true, true, ateam_geometry::Point(0, 0), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false}
};
Expand All @@ -143,18 +150,23 @@ TEST(RobotAssignmentTest, DisallowAssigningDisallowedRobots)
TEST(GroupAssignmentTest, ThreeGroups) {
std::vector<Robot> robots {
{0, true, true, ateam_geometry::Point(0, 0), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false},
{1, true, true, ateam_geometry::Point(1, 1), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false},
{2, true, true, ateam_geometry::Point(2, 2), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false},
{3, true, true, ateam_geometry::Point(3, 3), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false},
{4, true, true, ateam_geometry::Point(4, 4), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false}
};
Expand All @@ -181,12 +193,15 @@ TEST(GroupAssignmentTest, ThreeGroups) {
TEST(GroupAssignmentTest, DisallowedIds) {
std::vector<Robot> robots {
{0, true, true, ateam_geometry::Point(0, 0), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false},
{1, true, true, ateam_geometry::Point(1, 1), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false},
{2, true, true, ateam_geometry::Point(2, 2), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false, true,
false}
};
Expand Down
1 change: 1 addition & 0 deletions ateam_kenobi/test/window_evaluation_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ TEST(WindowEvaluationTest, OneRobot)
{
std::vector<Robot> robots = {
{1, true, true, ateam_geometry::Point(4.2, 0.0), 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Point{}, 0.0, ateam_geometry::Vector{}, 0.0,
ateam_geometry::Vector{}, 0.0, false,
true,
false}
Expand Down
1 change: 1 addition & 0 deletions state_tracking/ateam_game_state/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ rclcpp_components_register_node(
)

ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(ateam_common tf2 tf2_geometry_msgs)

install(
DIRECTORY include/
Expand Down
9 changes: 8 additions & 1 deletion state_tracking/ateam_game_state/include/ateam_game_state/field.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,12 @@ struct FieldSidedInfo
std::array<ateam_geometry::Point, 4> defense_area_corners;
std::array<ateam_geometry::Point, 4> goal_corners;
};

enum class IgnoreSide {
None = 0,
Ours = 1,
Theirs = 2
};
struct Field
{
float field_length = 0.f;
Expand All @@ -43,7 +49,8 @@ struct Field
ateam_geometry::Point center_circle_center;
float center_circle_radius;
std::array<ateam_geometry::Point, 4> field_corners;
int ignore_side = 0;
IgnoreSide ignore_side = IgnoreSide::None;
int ignore_side_raw = 0;
FieldSidedInfo ours;
FieldSidedInfo theirs;
};
Expand Down
5 changes: 5 additions & 0 deletions state_tracking/ateam_game_state/include/ateam_game_state/robot.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,11 @@ struct Robot
ateam_geometry::Vector vel;
double omega = 0.0;

ateam_geometry::Point firmware_pos;
double firmware_theta = 0.0;
ateam_geometry::Vector firmware_vel;
double firmware_omega = 0.0;

ateam_geometry::Vector prev_command_vel;
double prev_command_omega = 0.0;

Expand Down
17 changes: 14 additions & 3 deletions state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -202,9 +202,20 @@ class GameStateTracker : public rclcpp::Node {

void RobotFeedbackCallback(const ateam_radio_msgs::msg::BasicTelemetry::SharedPtr msg, int id)
{
world_.our_robots[id].breakbeam_ball_detected = msg->breakbeam_ball_detected;
world_.our_robots[id].kicker_available = msg->kicker_available;
world_.our_robots[id].chipper_available = msg->chipper_available;
auto & robot = world_.our_robots[id];
robot.breakbeam_ball_detected = msg->breakbeam_ball_detected;
robot.kicker_available = msg->kicker_available;
robot.chipper_available = msg->chipper_available;
robot.firmware_pos = ateam_geometry::Point{
msg->kf_body_pos_estimate[0] / 1e3,
msg->kf_body_pos_estimate[1] / 1e3
};
robot.firmware_theta = msg->kf_body_pos_estimate[2] / 1e3;
robot.firmware_vel = ateam_geometry::Vector{
msg->kf_body_vel_estimate[0] / 1e3,
msg->kf_body_vel_estimate[1] / 1e3
};
robot.firmware_omega = msg->kf_body_vel_estimate[2] / 1e3;
}

void RobotConnectionCallback(const ateam_radio_msgs::msg::ConnectionStatus::SharedPtr msg, int id)
Expand Down
26 changes: 22 additions & 4 deletions state_tracking/ateam_game_state/src/type_adapters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,9 @@ void rclcpp::TypeAdapter<ateam_game_state::World,
if (robot.visible || robot.radio_connected) {
RobotTA::convert_to_ros_message(robot, ros_msg.our_robots.emplace_back());
} else {
ros_msg.our_robots.push_back(ateam_msgs::msg::GameStateRobot());
auto default_bot = ateam_msgs::msg::GameStateRobot();
default_bot.id = robot.id;
ros_msg.our_robots.push_back(default_bot);
}
}

Expand All @@ -59,7 +61,9 @@ void rclcpp::TypeAdapter<ateam_game_state::World,
if (robot.visible) {
RobotTA::convert_to_ros_message(robot, ros_msg.their_robots.emplace_back());
} else {
ros_msg.their_robots.push_back(ateam_msgs::msg::GameStateRobot());
auto default_bot = ateam_msgs::msg::GameStateRobot();
default_bot.id = robot.id;
ros_msg.their_robots.push_back(default_bot);
}
}

Expand Down Expand Up @@ -149,6 +153,13 @@ void rclcpp::TypeAdapter<ateam_game_state::Robot,
ros_msg.velocity.linear.y = robot.vel.y();
ros_msg.velocity.angular.z = robot.omega;

ros_msg.firmware_pose.position.x = robot.firmware_pos.x();
ros_msg.firmware_pose.position.y = robot.firmware_pos.y();
ros_msg.firmware_pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), robot.firmware_theta));
ros_msg.firmware_velocity.linear.x = robot.firmware_vel.x();
ros_msg.firmware_velocity.linear.y = robot.firmware_vel.y();
ros_msg.firmware_velocity.angular.z = robot.firmware_omega;

ros_msg.prev_command_velocity.linear.x = robot.prev_command_vel.x();
ros_msg.prev_command_velocity.linear.y = robot.prev_command_vel.y();
ros_msg.prev_command_velocity.angular.z = robot.prev_command_omega;
Expand Down Expand Up @@ -176,6 +187,11 @@ void rclcpp::TypeAdapter<ateam_game_state::Robot,
robot.vel = ateam_geometry::Vector(ros_msg.velocity.linear.x, ros_msg.velocity.linear.y);
robot.omega = ros_msg.velocity.angular.z;

robot.firmware_pos = ateam_geometry::Point(ros_msg.firmware_pose.position.x, ros_msg.firmware_pose.position.y);
tf2::Quaternion firm_quat;
tf2::fromMsg(ros_msg.firmware_pose.orientation, firm_quat);
robot.firmware_theta = tf2::getYaw(firm_quat);

robot.prev_command_vel = ateam_geometry::Vector(ros_msg.prev_command_velocity.linear.x,
ros_msg.prev_command_velocity.linear.y);
robot.prev_command_omega = ros_msg.prev_command_velocity.angular.z;
Expand All @@ -196,7 +212,8 @@ void rclcpp::TypeAdapter<ateam_game_state::Field,
ros_msg.goal_width = field.goal_width;
ros_msg.goal_depth = field.goal_depth;
ros_msg.boundary_width = field.boundary_width;
ros_msg.ignore_side = field.ignore_side;
ros_msg.ignore_side = static_cast<int>(field.ignore_side);
ros_msg.ignore_side_raw = field.ignore_side_raw;
ros_msg.defense_area_width = field.defense_area_width;
ros_msg.defense_area_depth = field.defense_area_depth;
ros_msg.center_circle.x = field.center_circle_center.x();
Expand Down Expand Up @@ -228,7 +245,8 @@ void rclcpp::TypeAdapter<ateam_game_state::Field, ateam_msgs::msg::FieldInfo>::c
field.goal_width = ros_msg.goal_width;
field.goal_depth = ros_msg.goal_depth;
field.boundary_width = ros_msg.boundary_width;
field.ignore_side = ros_msg.ignore_side;
field.ignore_side = static_cast<ateam_game_state::IgnoreSide>(ros_msg.ignore_side);
field.ignore_side_raw = ros_msg.ignore_side_raw;
field.defense_area_width = ros_msg.defense_area_width;
field.defense_area_depth = ros_msg.defense_area_depth;
field.center_circle_center = ateam_geometry::Point(ros_msg.center_circle.x,
Expand Down
Loading