Splits vision and game state messages

ateam_kenobi/src/core/fps_tracker.hpp

@@ -36,7 +36,7 @@ class FpsTracker
     std::fill(buffer_.begin(), buffer_.end(), average_fps_ / kWindowWidth);
   }
 
-  void update(World & world)
+  double Update(const World & world)
   {
     const auto dt = ateam_common::TimeDiffSeconds(world.current_time, prev_time_);
     prev_time_ = world.current_time;
@@ -45,7 +45,7 @@ class FpsTracker
     buffer_[buffer_index_] = current_fps / kWindowWidth;
     average_fps_ += buffer_[buffer_index_];
     buffer_index_ = (buffer_index_ + 1) % kWindowWidth;
-    world.fps = average_fps_;
+    return average_fps_;
   }
 
 private:

ateam_kenobi/src/core/motion/motion_controller.cpp

@@ -28,7 +28,6 @@
 #include <vector>
 #include <utility>
 #include <ateam_msgs/msg/robot_motion_command.hpp>
-#include <ateam_msgs/msg/robot_state.hpp>
 #include "ateam_geometry/ateam_geometry.hpp"
 #include "ateam_common/robot_constants.hpp"
 #include "pid.hpp"

ateam_kenobi/src/core/types/message_conversions.cpp

@@ -21,9 +21,11 @@
 #include "core/types/message_conversions.hpp"
 
 #include <tf2/LinearMath/Quaternion.h>
-
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
+#include "core/types/field.hpp"
+#include "core/types/referee_info.hpp"
+
 namespace ateam_kenobi::message_conversions
 {
 ateam_msgs::msg::FieldInfo toMsg(const Field & obj)
@@ -81,9 +83,9 @@ ateam_msgs::msg::RefereeInfo toMsg(const RefereeInfo & obj)
 }
 
 
-ateam_msgs::msg::BallState toMsg(const Ball & obj)
+ateam_msgs::msg::GameStateBall toMsg(const Ball & obj)
 {
-  ateam_msgs::msg::BallState ball_state_msg;
+  ateam_msgs::msg::GameStateBall ball_state_msg;
   ball_state_msg.pose.position.x = obj.pos.x();
   ball_state_msg.pose.position.y = obj.pos.y();
   ball_state_msg.twist.linear.x = obj.vel.x();
@@ -93,9 +95,9 @@ ateam_msgs::msg::BallState toMsg(const Ball & obj)
   return ball_state_msg;
 }
 
-ateam_msgs::msg::RobotState toMsg(const Robot & obj)
+ateam_msgs::msg::GameStateRobot toMsg(const Robot & obj)
 {
-  ateam_msgs::msg::RobotState robot_state_msg;
+  ateam_msgs::msg::GameStateRobot robot_state_msg;
   robot_state_msg.pose.position.x = obj.pos.x();
   robot_state_msg.pose.position.y = obj.pos.y();
   robot_state_msg.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), obj.theta));
@@ -107,9 +109,9 @@ ateam_msgs::msg::RobotState toMsg(const Robot & obj)
   return robot_state_msg;
 }
 
-ateam_msgs::msg::World toMsg(const World & obj)
+ateam_msgs::msg::GameStateWorld toMsg(const World & obj)
 {
-  ateam_msgs::msg::World world_msg;
+  ateam_msgs::msg::GameStateWorld world_msg;
 
   world_msg.current_time =
     rclcpp::Time(
@@ -119,30 +121,28 @@ ateam_msgs::msg::World toMsg(const World & obj)
   world_msg.field = toMsg(obj.field);
   world_msg.referee_info = toMsg(obj.referee_info);
 
-  world_msg.balls.push_back(toMsg(obj.ball));
+  world_msg.ball = toMsg(obj.ball);
 
   for (const Robot & robot : obj.our_robots) {
     if (robot.visible || robot.radio_connected) {
       world_msg.our_robots.push_back(toMsg(robot));
     } else {
-      world_msg.our_robots.push_back(ateam_msgs::msg::RobotState());
+      world_msg.our_robots.push_back(ateam_msgs::msg::GameStateRobot());
     }
   }
 
   for (const Robot & robot : obj.their_robots) {
     if (robot.visible) {
       world_msg.their_robots.push_back(toMsg(robot));
     } else {
-      world_msg.their_robots.push_back(ateam_msgs::msg::RobotState());
+      world_msg.their_robots.push_back(ateam_msgs::msg::GameStateRobot());
     }
   }
 
   world_msg.ball_in_play = obj.in_play;
   world_msg.double_touch_enforced = obj.double_touch_forbidden_id_.has_value();
   world_msg.double_touch_id = obj.double_touch_forbidden_id_.value_or(-1);
 
-  world_msg.fps = obj.fps;
-
   return world_msg;
 }
 

ateam_kenobi/src/core/types/message_conversions.hpp

@@ -22,25 +22,20 @@
 #ifndef CORE__TYPES__MESSAGE_CONVERSIONS_HPP_
 #define CORE__TYPES__MESSAGE_CONVERSIONS_HPP_
 
-#include <ateam_msgs/msg/world.hpp>
-#include <ateam_msgs/msg/behavior_executor_state.hpp>
-#include <ateam_msgs/msg/trajectory.hpp>
-#include <ateam_msgs/msg/ball_state.hpp>
-#include <ateam_msgs/msg/robot_state.hpp>
-#include <ateam_msgs/msg/field_info.hpp>
-#include <ateam_msgs/msg/field_sided_info.hpp>
-#include <ateam_msgs/msg/referee_info.hpp>
+#include <ateam_msgs/msg/game_state_ball.hpp>
+#include <ateam_msgs/msg/game_state_robot.hpp>
+#include <ateam_msgs/msg/game_state_world.hpp>
 
+#include "core/types/ball.hpp"
+#include "core/types/robot.hpp"
 #include "core/types/world.hpp"
-#include "core/types/field.hpp"
-#include "core/types/referee_info.hpp"
+
 
 namespace ateam_kenobi::message_conversions
 {
-ateam_msgs::msg::BallState toMsg(const Ball & obj);
-ateam_msgs::msg::RobotState toMsg(const Robot & obj);
-
-ateam_msgs::msg::World toMsg(const World & obj);
+ateam_msgs::msg::GameStateBall toMsg(const Ball & obj);
+ateam_msgs::msg::GameStateRobot toMsg(const Robot & obj);
+ateam_msgs::msg::GameStateWorld toMsg(const World & obj);
 
 }  // namespace ateam_kenobi::message_conversions
 

ateam_kenobi/src/kenobi_node.cpp

@@ -25,18 +25,19 @@
 #include <functional>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
-#include <ateam_msgs/msg/ball_state.hpp>
 #include <ateam_radio_msgs/msg/basic_telemetry.hpp>
 #include <ateam_radio_msgs/msg/connection_status.hpp>
-#include <ateam_msgs/msg/robot_state.hpp>
+#include <ateam_msgs/msg/vision_state_ball.hpp>
+#include <ateam_msgs/msg/vision_state_robot.hpp>
 #include <ateam_msgs/msg/field_info.hpp>
 #include <ateam_msgs/msg/robot_motion_command.hpp>
 #include <ateam_msgs/msg/overlay.hpp>
 #include <ateam_msgs/msg/play_info.hpp>
-#include <ateam_msgs/msg/world.hpp>
+#include <ateam_msgs/msg/game_state_world.hpp>
 #include <ateam_msgs/srv/set_override_play.hpp>
 #include <ateam_msgs/srv/set_play_enabled.hpp>
 #include <ateam_msgs/msg/playbook_state.hpp>
+#include <ateam_msgs/msg/kenobi_status.hpp>
 #include <ateam_common/cache_directory.hpp>
 #include <ateam_common/game_controller_listener.hpp>
 #include <ateam_common/topic_names.hpp>
@@ -86,14 +87,14 @@ class KenobiNode : public rclcpp::Node
       "/play_info",
       rclcpp::SystemDefaultsQoS());
 
-    create_indexed_subscribers<ateam_msgs::msg::RobotState>(
+    create_indexed_subscribers<ateam_msgs::msg::VisionStateRobot>(
       blue_robots_subscriptions_,
       Topics::kBlueTeamRobotPrefix,
       10,
       &KenobiNode::blue_robot_state_callback,
       this);
 
-    create_indexed_subscribers<ateam_msgs::msg::RobotState>(
+    create_indexed_subscribers<ateam_msgs::msg::VisionStateRobot>(
       yellow_robots_subscriptions_,
       Topics::kYellowTeamRobotPrefix,
       10,
@@ -118,12 +119,12 @@ class KenobiNode : public rclcpp::Node
       robot_commands_publishers_, Topics::kRobotMotionCommandPrefix,
       rclcpp::SystemDefaultsQoS(), this);
 
-    ball_subscription_ = create_subscription<ateam_msgs::msg::BallState>(
+    ball_subscription_ = create_subscription<ateam_msgs::msg::VisionStateBall>(
       std::string(Topics::kBall),
       10,
       std::bind(&KenobiNode::ball_state_callback, this, std::placeholders::_1));
 
-    world_publisher_ = create_publisher<ateam_msgs::msg::World>(
+    world_publisher_ = create_publisher<ateam_msgs::msg::GameStateWorld>(
       "~/world",
       rclcpp::SystemDefaultsQoS());
 
@@ -146,6 +147,9 @@ class KenobiNode : public rclcpp::Node
       "~/playbook_state",
       rclcpp::SystemDefaultsQoS());
 
+    status_publisher_ = create_publisher<ateam_msgs::msg::KenobiStatus>("~/status",
+        rclcpp::SystemDefaultsQoS());
+
     timer_ = create_wall_timer(10ms, std::bind(&KenobiNode::timer_callback, this));
 
     const auto playbook_path = declare_parameter<std::string>("playbook", "");
@@ -182,10 +186,10 @@ class KenobiNode : public rclcpp::Node
   FpsTracker fps_tracker_;
   rclcpp::Publisher<ateam_msgs::msg::OverlayArray>::SharedPtr overlay_publisher_;
   rclcpp::Publisher<ateam_msgs::msg::PlayInfo>::SharedPtr play_info_publisher_;
-  rclcpp::Subscription<ateam_msgs::msg::BallState>::SharedPtr ball_subscription_;
-  std::array<rclcpp::Subscription<ateam_msgs::msg::RobotState>::SharedPtr,
+  rclcpp::Subscription<ateam_msgs::msg::VisionStateBall>::SharedPtr ball_subscription_;
+  std::array<rclcpp::Subscription<ateam_msgs::msg::VisionStateRobot>::SharedPtr,
     16> blue_robots_subscriptions_;
-  std::array<rclcpp::Subscription<ateam_msgs::msg::RobotState>::SharedPtr,
+  std::array<rclcpp::Subscription<ateam_msgs::msg::VisionStateRobot>::SharedPtr,
     16> yellow_robots_subscriptions_;
   std::array<rclcpp::Subscription<ateam_radio_msgs::msg::BasicTelemetry>::SharedPtr,
     16> robot_feedback_subscriptions_;
@@ -198,10 +202,11 @@ class KenobiNode : public rclcpp::Node
   rclcpp::Service<ateam_msgs::srv::SetOverridePlay>::SharedPtr override_service_;
   rclcpp::Service<ateam_msgs::srv::SetPlayEnabled>::SharedPtr play_enabled_service_;
   rclcpp::Publisher<ateam_msgs::msg::PlaybookState>::SharedPtr playbook_state_publisher_;
+  rclcpp::Publisher<ateam_msgs::msg::KenobiStatus>::SharedPtr status_publisher_;
 
   ateam_common::GameControllerListener game_controller_listener_;
 
-  rclcpp::Publisher<ateam_msgs::msg::World>::SharedPtr world_publisher_;
+  rclcpp::Publisher<ateam_msgs::msg::GameStateWorld>::SharedPtr world_publisher_;
 
   rclcpp::TimerBase::SharedPtr timer_;
 
@@ -216,7 +221,7 @@ class KenobiNode : public rclcpp::Node
   }
 
   void blue_robot_state_callback(
-    const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg,
+    const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg,
     int id)
   {
     const auto our_color = game_controller_listener_.GetTeamColor();
@@ -229,7 +234,7 @@ class KenobiNode : public rclcpp::Node
   }
 
   void yellow_robot_state_callback(
-    const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg,
+    const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg,
     int id)
   {
     const auto our_color = game_controller_listener_.GetTeamColor();
@@ -244,7 +249,7 @@ class KenobiNode : public rclcpp::Node
   void robot_state_callback(
     std::array<Robot, 16> & robot_states,
     std::size_t id,
-    const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg)
+    const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg)
   {
     robot_states.at(id).visible = robot_state_msg->visible;
     if (robot_state_msg->visible) {
@@ -278,7 +283,7 @@ class KenobiNode : public rclcpp::Node
     world_.our_robots.at(id).radio_connected = robot_connection_msg->radio_connected;
   }
 
-  void ball_state_callback(const ateam_msgs::msg::BallState::SharedPtr ball_state_msg)
+  void ball_state_callback(const ateam_msgs::msg::VisionStateBall::SharedPtr ball_state_msg)
   {
     world_.ball.visible = ball_state_msg->visible;
     const auto ball_visibility_timed_out = ateam_common::TimeDiffSeconds(world_.current_time,
@@ -427,10 +432,12 @@ class KenobiNode : public rclcpp::Node
         "DETECTED TEAM SIDE WAS UNKNOWN");
     }
 
-    fps_tracker_.update(world_);
-
     world_publisher_->publish(ateam_kenobi::message_conversions::toMsg(world_));
 
+    ateam_msgs::msg::KenobiStatus status_msg;
+    status_msg.fps = fps_tracker_.Update(world_);
+    status_publisher_->publish(status_msg);
+
     auto motion_commands = runPlayFrame(world_);
 
     defense_area_enforcement::EnforceDefenseAreaKeepout(world_, motion_commands);

ateam_msgs/CMakeLists.txt

@@ -10,23 +10,25 @@ find_package(sensor_msgs REQUIRED)
 find_package(ssl_league_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  msg/BallState.msg
-  msg/BehaviorExecutorState.msg
   msg/JoystickControlStatus.msg
+  msg/KenobiStatus.msg
   msg/Overlay.msg
   msg/OverlayArray.msg
   msg/RefereeInfo.msg
   msg/RobotMotionCommand.msg
-  msg/RobotState.msg
-  msg/Sample3d.msg
   msg/TeamClientConnectionStatus.msg
-  msg/Trajectory.msg
-  msg/VisionCameraState.msg
-  msg/VisionIMMState.msg
-  msg/VisionMHTState.msg
-  msg/VisionModelState.msg
-  msg/VisionWorldState.msg
-  msg/World.msg
+
+  msg/GameStateBall.msg
+  msg/GameStateRobot.msg
+  msg/GameStateWorld.msg
+
+  msg/VisionStateBall.msg
+  msg/VisionStateCamera.msg
+  msg/VisionStateCameraArray.msg
+  msg/VisionStateIMM.msg
+  msg/VisionStateMHT.msg
+  msg/VisionStateModel.msg
+  msg/VisionStateRobot.msg
 
   msg/FieldInfo.msg
   msg/FieldSidedInfo.msg

ateam_msgs/msg/GameStateBall.msg

@@ -0,0 +1,4 @@
+geometry_msgs/Pose pose
+geometry_msgs/Twist twist
+geometry_msgs/Accel accel
+bool visible

ateam_msgs/msg/GameStateRobot.msg

@@ -0,0 +1,6 @@
+geometry_msgs/Pose pose
+geometry_msgs/Twist twist
+geometry_msgs/Twist twist_body
+geometry_msgs/Accel accel
+bool visible
+bool connected

ateam_msgs/msg/GameStateWorld.msg

@@ -0,0 +1,12 @@
+builtin_interfaces/Time current_time
+
+FieldInfo field
+RefereeInfo referee_info
+
+GameStateBall ball
+GameStateRobot[] our_robots
+GameStateRobot[] their_robots
+
+bool ball_in_play
+bool double_touch_enforced
+int32 double_touch_id

ateam_msgs/msg/KenobiStatus.msg

@@ -0,0 +1 @@
+float64 fps