Update Software to Match Ros Bridge Auto-Generated Names

radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp

@@ -19,11 +19,10 @@
 // THE SOFTWARE.
 
 #include "message_conversions.hpp"
-#include <tf2/convert.h>
-#include <tf2/utils.h>
-#include <tf2/LinearMath/Quaternion.h>
+#include <cmath>
 #include <stdexcept>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <rclcpp/logging.hpp>
 
 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<Team>(robot_msg.id.team[0].color));
+        proto_robot_id->set_team(static_cast<Team>(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;
 }

radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp

@@ -21,9 +21,9 @@
 #ifndef MESSAGE_CONVERSIONS_HPP_
 #define MESSAGE_CONVERSIONS_HPP_
 
-#include <ssl_league_protobufs/ssl_simulation_robot_control.pb.h>
-#include <ssl_league_protobufs/ssl_simulation_robot_feedback.pb.h>
-#include <ssl_league_protobufs/ssl_simulation_control.pb.h>
+#include <ssl_league_protobufs/simulation/ssl_simulation_robot_control.pb.h>
+#include <ssl_league_protobufs/simulation/ssl_simulation_robot_feedback.pb.h>
+#include <ssl_league_protobufs/simulation/ssl_simulation_control.pb.h>
 
 #include <rclcpp/logger.hpp>
 

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 <ssl_league_protobufs/ssl_simulation_robot_control.pb.h>
-#include <ssl_league_protobufs/ssl_simulation_robot_feedback.pb.h>
-#include <ssl_league_protobufs/ssl_simulation_control.pb.h>
+#include <ssl_league_protobufs/simulation/ssl_simulation_robot_control.pb.h>
+#include <ssl_league_protobufs/simulation/ssl_simulation_robot_feedback.pb.h>
+#include <ssl_league_protobufs/simulation/ssl_simulation_control.pb.h>
 
 #include <array>
 #include <string>

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<ssl_league_msgs::msg::VisionWrapper>(
+      create_subscription<ssl_league_msgs::msg::WrapperPacket>(
       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<ateam_msgs::msg::FieldInfo>::SharedPtr field_publisher_;
-  rclcpp::Subscription<ssl_league_msgs::msg::VisionWrapper>::SharedPtr ssl_vision_subscription_;
-  rclcpp::Subscription<ssl_league_msgs::msg::VisionWrapper>::SharedPtr ssl_vision_subs_;
+  rclcpp::Subscription<ssl_league_msgs::msg::WrapperPacket>::SharedPtr ssl_vision_subscription_;
+  rclcpp::Subscription<ssl_league_msgs::msg::WrapperPacket>::SharedPtr ssl_vision_subs_;
   rclcpp::Service<ateam_msgs::srv::SetIgnoreFieldSide>::SharedPtr set_ignore_field_side_service_;
   ateam_common::GameControllerListener game_controller_listener_;
 

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<geometry_msgs::msg::Point32> getPointsFromLines(
-  const std::vector<ssl_league_msgs::msg::VisionFieldLineSegment> & lines,
+  const std::vector<ssl_league_msgs::msg::FieldLineSegment> & lines,
   const std::vector<std::string> & line_names)
 {
   std::vector<geometry_msgs::msg::Point32> points;

state_tracking/ateam_field_manager/src/message_conversions.hpp

@@ -27,16 +27,16 @@
 #include <ateam_msgs/msg/field_info.hpp>
 #include <ateam_msgs/msg/field_sided_info.hpp>
 #include <ateam_common/game_controller_listener.hpp>
-#include <ssl_league_msgs/msg/vision_detection_ball.hpp>
-#include <ssl_league_msgs/msg/vision_detection_robot.hpp>
-#include <ssl_league_msgs/msg/vision_detection_frame.hpp>
-#include <ssl_league_msgs/msg/vision_wrapper.hpp>
+#include <ssl_league_msgs/msg/detection_ball.hpp>
+#include <ssl_league_msgs/msg/detection_robot.hpp>
+#include <ssl_league_msgs/msg/detection_frame.hpp>
+#include <ssl_league_msgs/msg/wrapper_packet.hpp>
 
 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<geometry_msgs::msg::Point32> getPointsFromLines(
-  const std::vector<ssl_league_msgs::msg::VisionFieldLineSegment> & lines,
+  const std::vector<ssl_league_msgs::msg::FieldLineSegment> & lines,
   const std::vector<std::string> & line_names);
 
 }  // namespace ateam_field_manager::message_conversions

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,
         ),
     ]
 

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<ssl_league_msgs::msg::VisionWrapper>(
+      create_subscription<ssl_league_msgs::msg::WrapperPacket>(
       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<rclcpp::Publisher<ateam_msgs::msg::VisionStateRobot>::SharedPtr,
     16> yellow_robots_publisher_;
   rclcpp::Publisher<ateam_msgs::msg::VisionStateCameraArray>::SharedPtr vision_state_publisher_;
-  rclcpp::Subscription<ssl_league_msgs::msg::VisionWrapper>::SharedPtr ssl_vision_subscription_;
-  rclcpp::Subscription<ssl_league_msgs::msg::VisionWrapper>::SharedPtr ssl_vision_subs_;
+  rclcpp::Subscription<ssl_league_msgs::msg::WrapperPacket>::SharedPtr ssl_vision_subscription_;
+  rclcpp::Subscription<ssl_league_msgs::msg::WrapperPacket>::SharedPtr ssl_vision_subs_;
   rclcpp::Subscription<ateam_msgs::msg::FieldInfo>::SharedPtr field_subscription_;
 
   // We might be able to get rid of this since the field manager is handling some of it now