From 57628011e9aca002eeddabe02093590c523ec2f9 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 15 Aug 2026 22:24:55 -0400 Subject: [PATCH] initial pass after ros bridge update --- .../src/message_conversions.cpp | 65 +++++++++++-------- .../src/message_conversions.hpp | 6 +- .../src/ssl_simulation_radio_bridge_node.cpp | 6 +- ssl_ros_bridge | 2 +- .../src/ateam_field_manager_node.cpp | 8 +-- .../src/message_conversions.cpp | 15 +++-- .../src/message_conversions.hpp | 12 ++-- .../scripts/calibrate_field.py | 30 +++++---- .../src/ateam_vision_filter_node.cpp | 8 +-- .../src/message_conversions.cpp | 42 +++++++----- .../src/message_conversions.hpp | 18 ++--- 11 files changed, 123 insertions(+), 89 deletions(-) diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp index 8718a1082..4b9134808 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp @@ -19,11 +19,10 @@ // THE SOFTWARE. #include "message_conversions.hpp" -#include -#include -#include +#include #include -#include + +#include namespace ateam_ssl_simulation_radio_bridge::message_conversions { @@ -96,17 +95,23 @@ SimulatorControl fromMsg(const ssl_league_msgs::msg::SimulatorControl & ros_msg) auto ball_msg = ros_msg.teleport_ball[0]; TeleportBall * proto_teleport_ball = sim_control.mutable_teleport_ball(); - proto_teleport_ball->set_x(ball_msg.pose.position.x); - proto_teleport_ball->set_y(ball_msg.pose.position.y); - proto_teleport_ball->set_z(ball_msg.pose.position.z); + proto_teleport_ball->set_x(ball_msg.pos.x); + proto_teleport_ball->set_y(ball_msg.pos.y); + proto_teleport_ball->set_z(ball_msg.pos.z); - proto_teleport_ball->set_vx(ball_msg.twist.linear.x); - proto_teleport_ball->set_vy(ball_msg.twist.linear.y); - proto_teleport_ball->set_vz(ball_msg.twist.linear.z); + proto_teleport_ball->set_vx(ball_msg.velocity.x); + proto_teleport_ball->set_vy(ball_msg.velocity.y); + proto_teleport_ball->set_vz(ball_msg.velocity.z); - proto_teleport_ball->set_teleport_safely(ball_msg.teleport_safely); - proto_teleport_ball->set_roll(ball_msg.roll); - proto_teleport_ball->set_by_force(ball_msg.by_force); + if (!ball_msg.teleport_safely.empty()) { + proto_teleport_ball->set_teleport_safely(ball_msg.teleport_safely.front()); + } + if (!ball_msg.roll.empty()) { + proto_teleport_ball->set_roll(ball_msg.roll.front()); + } + if (!ball_msg.by_force.empty()) { + proto_teleport_ball->set_by_force(ball_msg.by_force.front()); + } } if (ros_msg.teleport_robot.size() != 0) { @@ -121,31 +126,39 @@ SimulatorControl fromMsg(const ssl_league_msgs::msg::SimulatorControl & ros_msg) } if (robot_msg.id.team.size() != 0) { - proto_robot_id->set_team(static_cast(robot_msg.id.team[0].color)); + proto_robot_id->set_team(static_cast(robot_msg.id.team[0])); } else { throw std::invalid_argument("No robot team specified"); } - proto_teleport_robot->set_x(robot_msg.pose.position.x); - proto_teleport_robot->set_y(robot_msg.pose.position.y); + proto_teleport_robot->set_x(robot_msg.pos.x); + proto_teleport_robot->set_y(robot_msg.pos.y); - // Orientation - tf2::Quaternion tf2_quat; - tf2::fromMsg(robot_msg.pose.orientation, tf2_quat); - proto_teleport_robot->set_orientation(tf2::getYaw(tf2_quat)); + // orientation is yaw in radians directly (no quaternion round trip needed) + if (!robot_msg.orientation.empty()) { + proto_teleport_robot->set_orientation(robot_msg.orientation.front()); + } - proto_teleport_robot->set_v_x(robot_msg.twist.linear.x); - proto_teleport_robot->set_v_y(robot_msg.twist.linear.y); + proto_teleport_robot->set_v_x(robot_msg.velocity.x); + proto_teleport_robot->set_v_y(robot_msg.velocity.y); - proto_teleport_robot->set_v_angular(robot_msg.twist.angular.z); + if (!robot_msg.v_angular.empty()) { + proto_teleport_robot->set_v_angular(robot_msg.v_angular.front()); + } - proto_teleport_robot->set_present(robot_msg.present); + if (!robot_msg.present.empty()) { + proto_teleport_robot->set_present(robot_msg.present.front()); + } - proto_teleport_robot->set_by_force(robot_msg.by_force); + if (!robot_msg.by_force.empty()) { + proto_teleport_robot->set_by_force(robot_msg.by_force.front()); + } } } - sim_control.set_simulation_speed(ros_msg.simulation_speed); + if (!ros_msg.simulation_speed.empty()) { + sim_control.set_simulation_speed(ros_msg.simulation_speed.front()); + } return sim_control; } diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp index 4cd2e3afb..35cd619fb 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp @@ -21,9 +21,9 @@ #ifndef MESSAGE_CONVERSIONS_HPP_ #define MESSAGE_CONVERSIONS_HPP_ -#include -#include -#include +#include +#include +#include #include diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp index cef2b9f52..3efbbe0db 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp @@ -18,9 +18,9 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include -#include -#include +#include +#include +#include #include #include diff --git a/ssl_ros_bridge b/ssl_ros_bridge index 4a1373bc3..d75219f6f 160000 --- a/ssl_ros_bridge +++ b/ssl_ros_bridge @@ -1 +1 @@ -Subproject commit 4a1373bc30541013f15d76b0ea90503c0524288a +Subproject commit d75219f6f3cbccc329df1b47ca666a4a803b3717 diff --git a/state_tracking/ateam_field_manager/src/ateam_field_manager_node.cpp b/state_tracking/ateam_field_manager/src/ateam_field_manager_node.cpp index fcab6d987..8d119d0e3 100644 --- a/state_tracking/ateam_field_manager/src/ateam_field_manager_node.cpp +++ b/state_tracking/ateam_field_manager/src/ateam_field_manager_node.cpp @@ -58,7 +58,7 @@ class FieldManagerNode : public rclcpp::Node qos); ssl_vision_subscription_ = - create_subscription( + create_subscription( std::string(Topics::kVisionMessages), 10, std::bind(&FieldManagerNode::vision_callback, this, std::placeholders::_1)); @@ -75,7 +75,7 @@ class FieldManagerNode : public rclcpp::Node } void vision_callback( - const ssl_league_msgs::msg::VisionWrapper::SharedPtr vision_wrapper_msg) + const ssl_league_msgs::msg::WrapperPacket::SharedPtr vision_wrapper_msg) { const auto team_side = game_controller_listener_.GetTeamSide(); @@ -106,8 +106,8 @@ class FieldManagerNode : public rclcpp::Node const char * ignore_side_cache_filename_ = "ignore_side.txt"; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr field_publisher_; - rclcpp::Subscription::SharedPtr ssl_vision_subscription_; - rclcpp::Subscription::SharedPtr ssl_vision_subs_; + rclcpp::Subscription::SharedPtr ssl_vision_subscription_; + rclcpp::Subscription::SharedPtr ssl_vision_subs_; rclcpp::Service::SharedPtr set_ignore_field_side_service_; ateam_common::GameControllerListener game_controller_listener_; diff --git a/state_tracking/ateam_field_manager/src/message_conversions.cpp b/state_tracking/ateam_field_manager/src/message_conversions.cpp index 2f3bb9f97..f37cc7b5e 100644 --- a/state_tracking/ateam_field_manager/src/message_conversions.cpp +++ b/state_tracking/ateam_field_manager/src/message_conversions.cpp @@ -30,11 +30,11 @@ namespace ateam_field_manager::message_conversions { ateam_msgs::msg::FieldInfo fromMsg( - const ssl_league_msgs::msg::VisionGeometryData & vision_wrapper_msg, + const ssl_league_msgs::msg::GeometryData & vision_wrapper_msg, const ateam_common::TeamSide & team_side, const int ignore_side) { - const ssl_league_msgs::msg::VisionGeometryFieldSize & ros_msg = + const ssl_league_msgs::msg::GeometryFieldSize & ros_msg = vision_wrapper_msg.field; ateam_msgs::msg::FieldInfo field_info; @@ -44,8 +44,10 @@ ateam_msgs::msg::FieldInfo fromMsg( field_info.goal_width = ros_msg.goal_width; field_info.goal_depth = ros_msg.goal_depth; field_info.boundary_width = ros_msg.boundary_width; - field_info.defense_area_width = ros_msg.penalty_area_width; - field_info.defense_area_depth = ros_msg.penalty_area_depth; + field_info.defense_area_width = + ros_msg.penalty_area_width.empty() ? 0.0f : ros_msg.penalty_area_width.front(); + field_info.defense_area_depth = + ros_msg.penalty_area_depth.empty() ? 0.0f : ros_msg.penalty_area_depth.front(); field_info.field_corners.points = getPointsFromLines( ros_msg.field_lines, @@ -130,7 +132,8 @@ ateam_msgs::msg::FieldInfo fromMsg( }); if (circle_iter != ros_msg.field_arcs.end()) { - field_info.center_circle = circle_iter->center; + field_info.center_circle.x = circle_iter->center.x; + field_info.center_circle.y = circle_iter->center.y; field_info.center_circle_radius = circle_iter->radius; } @@ -195,7 +198,7 @@ int32_t mapIgnoredSide(const ateam_common::TeamSide & team_side, const int ignor } std::vector getPointsFromLines( - const std::vector & lines, + const std::vector & lines, const std::vector & line_names) { std::vector points; diff --git a/state_tracking/ateam_field_manager/src/message_conversions.hpp b/state_tracking/ateam_field_manager/src/message_conversions.hpp index 641eb76c7..fdcb7a457 100644 --- a/state_tracking/ateam_field_manager/src/message_conversions.hpp +++ b/state_tracking/ateam_field_manager/src/message_conversions.hpp @@ -27,16 +27,16 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace ateam_field_manager::message_conversions { ateam_msgs::msg::FieldInfo fromMsg( - const ssl_league_msgs::msg::VisionGeometryData & ros_msg, + const ssl_league_msgs::msg::GeometryData & ros_msg, const ateam_common::TeamSide & team_side, const int ignore_side); @@ -45,7 +45,7 @@ void invertFieldInfo(ateam_msgs::msg::FieldInfo & info); int32_t mapIgnoredSide(const ateam_common::TeamSide & team_side, const int ignore_side_raw); std::vector getPointsFromLines( - const std::vector & lines, + const std::vector & lines, const std::vector & line_names); } // namespace ateam_field_manager::message_conversions diff --git a/state_tracking/ateam_vision_filter/scripts/calibrate_field.py b/state_tracking/ateam_vision_filter/scripts/calibrate_field.py index 93b3b56dc..314f2475d 100755 --- a/state_tracking/ateam_vision_filter/scripts/calibrate_field.py +++ b/state_tracking/ateam_vision_filter/scripts/calibrate_field.py @@ -40,7 +40,7 @@ import rosbag2_py -from ssl_league_msgs.msg import VisionWrapper +from ssl_league_msgs.msg import WrapperPacket @dataclass @@ -125,7 +125,7 @@ def _main(): if topic.startswith('/yellow_team'): yellow_robots_pub[topic].publish(msg_ser) if topic == '/vision_messages': - msg = deserialize_message(msg_ser, VisionWrapper) + msg = deserialize_message(msg_ser, WrapperPacket) if len(msg.detection) > 0: detections = msg.detection[0] for i in range(len(detections.balls)): @@ -138,12 +138,14 @@ def _main(): robot_detections = ( detections.robots_blue if color == 'blue' - else detections.robots_yello + else detections.robots_yellow ) for detect_robot in robot_detections: - robot = robots[detect_robot.robot_id] - robot.xs.append(detect_robot.pose.position.x) - robot.ys.append(detect_robot.pose.position.y) + if not detect_robot.robot_id: + continue + robot = robots[detect_robot.robot_id[0]] + robot.xs.append(detect_robot.pos.x) + robot.ys.append(detect_robot.pos.y) if len(msg.geometry) > 0: field_geometry = msg.geometry[0].field @@ -153,7 +155,13 @@ def _main(): half_field_len = field_geometry.field_length / 2 half_field_wid = field_geometry.field_width / 2 - def_area_front_abs = half_field_len - field_geometry.penalty_area_depth + penalty_area_depth = ( + field_geometry.penalty_area_depth[0] if field_geometry.penalty_area_depth else 0.0 + ) + penalty_area_width = ( + field_geometry.penalty_area_width[0] if field_geometry.penalty_area_width else 0.0 + ) + def_area_front_abs = half_field_len - penalty_area_depth field_points = [ FieldPoint('Center', 0.0, 0.0), @@ -170,22 +178,22 @@ def _main(): FieldPoint( 'NegDefAreaNegCorner', -def_area_front_abs, - -field_geometry.penalty_area_width / 2, + -penalty_area_width / 2, ), FieldPoint( 'NegDefAreaPosCorner', -def_area_front_abs, - field_geometry.penalty_area_width / 2, + penalty_area_width / 2, ), FieldPoint( 'PosDefAreaNegCorner', def_area_front_abs, - -field_geometry.penalty_area_width / 2, + -penalty_area_width / 2, ), FieldPoint( 'PosDefAreaPosCorner', def_area_front_abs, - field_geometry.penalty_area_width / 2, + penalty_area_width / 2, ), ] diff --git a/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp b/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp index 518880533..e888c7b69 100644 --- a/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp +++ b/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp @@ -77,7 +77,7 @@ class VisionFilterNode : public rclcpp::Node rclcpp::SystemDefaultsQoS()); ssl_vision_subscription_ = - create_subscription( + create_subscription( std::string(Topics::kVisionMessages), 10, std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1)); @@ -90,7 +90,7 @@ class VisionFilterNode : public rclcpp::Node } void vision_callback( - const ssl_league_msgs::msg::VisionWrapper::SharedPtr vision_wrapper_msg) + const ssl_league_msgs::msg::WrapperPacket::SharedPtr vision_wrapper_msg) { const auto team_side = game_controller_listener_.GetTeamSide(); if (!vision_wrapper_msg->detection.empty()) { @@ -141,8 +141,8 @@ class VisionFilterNode : public rclcpp::Node std::array::SharedPtr, 16> yellow_robots_publisher_; rclcpp::Publisher::SharedPtr vision_state_publisher_; - rclcpp::Subscription::SharedPtr ssl_vision_subscription_; - rclcpp::Subscription::SharedPtr ssl_vision_subs_; + rclcpp::Subscription::SharedPtr ssl_vision_subscription_; + rclcpp::Subscription::SharedPtr ssl_vision_subs_; rclcpp::Subscription::SharedPtr field_subscription_; // We might be able to get rid of this since the field manager is handling some of it now diff --git a/state_tracking/ateam_vision_filter/src/message_conversions.cpp b/state_tracking/ateam_vision_filter/src/message_conversions.cpp index d071ff580..202f0becd 100644 --- a/state_tracking/ateam_vision_filter/src/message_conversions.cpp +++ b/state_tracking/ateam_vision_filter/src/message_conversions.cpp @@ -83,7 +83,7 @@ ateam_msgs::msg::VisionStateRobot toMsg(const std::optional & maybe_robot } CameraMeasurement fromMsg( - const ssl_league_msgs::msg::VisionDetectionFrame & ros_msg, + const ssl_league_msgs::msg::DetectionFrame & ros_msg, const ateam_common::TeamSide & team_side) { CameraMeasurement cameraFrame; @@ -92,12 +92,18 @@ CameraMeasurement fromMsg( } for (const auto & yellow_robot_detection : ros_msg.robots_yellow) { - std::size_t robot_id = yellow_robot_detection.robot_id; + if (yellow_robot_detection.robot_id.empty()) { + continue; + } + std::size_t robot_id = yellow_robot_detection.robot_id.front(); cameraFrame.yellow_robots.at(robot_id).push_back(fromMsg(yellow_robot_detection)); } for (const auto & blue_robot_detection : ros_msg.robots_blue) { - std::size_t robot_id = blue_robot_detection.robot_id; + if (blue_robot_detection.robot_id.empty()) { + continue; + } + std::size_t robot_id = blue_robot_detection.robot_id.front(); cameraFrame.blue_robots.at(robot_id).push_back(fromMsg(blue_robot_detection)); } @@ -108,18 +114,19 @@ CameraMeasurement fromMsg( return cameraFrame; } -RobotMeasurement fromMsg(const ssl_league_msgs::msg::VisionDetectionRobot & ros_msg) +RobotMeasurement fromMsg(const ssl_league_msgs::msg::DetectionRobot & ros_msg) { - tf2::Quaternion tf2_quat; RobotMeasurement robotDetection; - robotDetection.position.x() = ros_msg.pose.position.x; - robotDetection.position.y() = ros_msg.pose.position.y; - tf2::fromMsg(ros_msg.pose.orientation, tf2_quat); - robotDetection.theta = tf2::getYaw(tf2_quat); + robotDetection.position.x() = ros_msg.pos.x; + robotDetection.position.y() = ros_msg.pos.y; + // orientation is yaw in radians directly (no quaternion round trip needed) + if (!ros_msg.orientation.empty()) { + robotDetection.theta = ros_msg.orientation.front(); + } return robotDetection; } -BallMeasurement fromMsg(const ssl_league_msgs::msg::VisionDetectionBall & ros_msg) +BallMeasurement fromMsg(const ssl_league_msgs::msg::DetectionBall & ros_msg) { BallMeasurement ballDetection; ballDetection.position.x() = ros_msg.pos.x; @@ -128,10 +135,10 @@ BallMeasurement fromMsg(const ssl_league_msgs::msg::VisionDetectionBall & ros_ms } ateam_msgs::msg::FieldInfo fromMsg( - const ssl_league_msgs::msg::VisionGeometryData & vision_wrapper_msg, + const ssl_league_msgs::msg::GeometryData & vision_wrapper_msg, const ateam_common::TeamSide & team_side) { - const ssl_league_msgs::msg::VisionGeometryFieldSize & ros_msg = + const ssl_league_msgs::msg::GeometryFieldSize & ros_msg = vision_wrapper_msg.field; ateam_msgs::msg::FieldInfo field_info; @@ -141,8 +148,10 @@ ateam_msgs::msg::FieldInfo fromMsg( field_info.goal_width = ros_msg.goal_width; field_info.goal_depth = ros_msg.goal_depth; field_info.boundary_width = ros_msg.boundary_width; - field_info.defense_area_width = ros_msg.penalty_area_width; - field_info.defense_area_depth = ros_msg.penalty_area_depth; + field_info.defense_area_width = + ros_msg.penalty_area_width.empty() ? 0.0f : ros_msg.penalty_area_width.front(); + field_info.defense_area_depth = + ros_msg.penalty_area_depth.empty() ? 0.0f : ros_msg.penalty_area_depth.front(); field_info.field_corners.points = getPointsFromLines( ros_msg.field_lines, @@ -227,7 +236,8 @@ ateam_msgs::msg::FieldInfo fromMsg( }); if (circle_iter != ros_msg.field_arcs.end()) { - field_info.center_circle = circle_iter->center; + field_info.center_circle.x = circle_iter->center.x; + field_info.center_circle.y = circle_iter->center.y; field_info.center_circle_radius = circle_iter->radius; } @@ -265,7 +275,7 @@ void invertFieldInfo(ateam_msgs::msg::FieldInfo & info) } std::vector getPointsFromLines( - const std::vector & lines, + const std::vector & lines, const std::vector & line_names) { std::vector points; diff --git a/state_tracking/ateam_vision_filter/src/message_conversions.hpp b/state_tracking/ateam_vision_filter/src/message_conversions.hpp index 0ad9e5c14..67256793a 100644 --- a/state_tracking/ateam_vision_filter/src/message_conversions.hpp +++ b/state_tracking/ateam_vision_filter/src/message_conversions.hpp @@ -29,10 +29,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include "types/ball.hpp" #include "types/ball_measurement.hpp" @@ -47,17 +47,17 @@ ateam_msgs::msg::VisionStateBall toMsg(const std::optional & maybe_ball); ateam_msgs::msg::VisionStateRobot toMsg(const std::optional & maybe_robot); CameraMeasurement fromMsg( - const ssl_league_msgs::msg::VisionDetectionFrame & ros_msg, + const ssl_league_msgs::msg::DetectionFrame & ros_msg, const ateam_common::TeamSide & team_side); -RobotMeasurement fromMsg(const ssl_league_msgs::msg::VisionDetectionRobot & ros_msg); -BallMeasurement fromMsg(const ssl_league_msgs::msg::VisionDetectionBall & ros_msg); +RobotMeasurement fromMsg(const ssl_league_msgs::msg::DetectionRobot & ros_msg); +BallMeasurement fromMsg(const ssl_league_msgs::msg::DetectionBall & ros_msg); ateam_msgs::msg::FieldInfo fromMsg( - const ssl_league_msgs::msg::VisionGeometryData & ros_msg, + const ssl_league_msgs::msg::GeometryData & ros_msg, const ateam_common::TeamSide & team_side); void invertFieldInfo(ateam_msgs::msg::FieldInfo & info); std::vector getPointsFromLines( - const std::vector & lines, + const std::vector & lines, const std::vector & line_names); } // namespace ateam_vision_filter::message_conversions