FEB Messages
============

Overview
--------

The ``feb_msgs`` package contains custom ROS 2 message definitions used throughout the FEB autonomous racing system. These messages enable communication between perception, SLAM, planning, and control modules.

Message Definitions
-------------------

ConesCartesian
~~~~~~~~~~~~~~

Represents detected cones in Cartesian coordinates (x, y) relative to the vehicle.

**Fields:**

.. list-table::
   :widths: 20 20 60
   :header-rows: 1

   * - Field
     - Type
     - Description
   * - header
     - std_msgs/Header
     - Timestamp and frame information
   * - x
     - float64[]
     - X coordinates of cone positions (meters)
   * - y
     - float64[]
     - Y coordinates of cone positions (meters)
   * - color
     - int8[]
     - Cone color codes: 1 = yellow, 2 = blue, -1 = unknown
   * - eval_time
     - float64
     - Processing/evaluation time (seconds)

**Usage:**

Published by perception modules (camera, LiDAR) to provide detected cone positions to SLAM and planning.

**Topics:**

- ``/camera/cones`` - Camera-based cone detections
- ``/lidar/cones`` - LiDAR-based cone detections
- ``/perception/cones`` - Fused perception output

---

State
~~~~~

Represents the current vehicle state estimate.

**Fields:**

.. list-table::
   :widths: 20 20 60
   :header-rows: 1

   * - Field
     - Type
     - Description
   * - header
     - std_msgs/Header
     - Timestamp and frame information
   * - x
     - float64
     - X position in global frame (meters)
   * - y
     - float64
     - Y position in global frame (meters)
   * - velocity
     - float64
     - Forward velocity (m/s)
   * - heading
     - float64
     - Vehicle heading angle (radians)
   * - theta
     - float64
     - Steering angle (radians)
   * - lap_count
     - uint8
     - Current lap number

**Usage:**

Published by SLAM module to provide vehicle state estimate to planning and control modules.

**Topics:**

- ``/slam/state`` - Vehicle state estimate from SLAM

---

Map
~~~

Represents the track map with separated left and right cone boundaries.

**Fields:**

.. list-table::
   :widths: 20 20 60
   :header-rows: 1

   * - Field
     - Type
     - Description
   * - header
     - std_msgs/Header
     - Timestamp and frame information
   * - left_cones_x
     - float64[]
     - X coordinates of left track boundary cones (meters)
   * - left_cones_y
     - float64[]
     - Y coordinates of left track boundary cones (meters)
   * - right_cones_x
     - float64[]
     - X coordinates of right track boundary cones (meters)
   * - right_cones_y
     - float64[]
     - Y coordinates of right track boundary cones (meters)

**Usage:**

Published by SLAM module after track mapping and cone association. Used by global path planner to generate racing line.

**Topics:**

- ``/slam/map`` - Complete track map with cone boundaries

---

Cones
~~~~~

Represents detected cones in polar coordinates (range, bearing) relative to the vehicle.

**Fields:**

.. list-table::
   :widths: 20 20 60
   :header-rows: 1

   * - Field
     - Type
     - Description
   * - header
     - std_msgs/Header
     - Timestamp and frame information
   * - r
     - float64[]
     - Range to each cone (meters)
   * - theta
     - float64[]
     - Bearing angle to each cone (radians)
   * - color
     - int8[]
     - Cone color codes: 1 = yellow, 2 = blue, -1 = unknown

**Usage:**

Alternative representation for cone detections when polar coordinates are more natural (e.g., for certain sensor outputs or algorithms).

---

FebPath
~~~~~~~

Represents a planned trajectory with vehicle state variables along the path.

**Fields:**

.. list-table::
   :widths: 20 20 60
   :header-rows: 1

   * - Field
     - Type
     - Description
   * - header
     - std_msgs/Header
     - Timestamp and frame information
   * - x
     - float64[]
     - X positions along path (meters)
   * - y
     - float64[]
     - Y positions along path (meters)
   * - v
     - float64[]
     - Velocity profile along path (m/s)
   * - psi
     - float64[]
     - Heading angles along path (radians)
   * - th
     - float64[]
     - Steering angles along path (radians)
   * - a
     - float64[]
     - Acceleration profile along path (m/s²)
   * - thdot
     - float64[]
     - Steering rate along path (rad/s)

**Usage:**

Published by path planning modules (global/local path planners) and consumed by MPC controller for trajectory tracking.

**Topics:**

- ``/global_path/path`` - Global racing line
- ``/local_path/path`` - Local dynamic path
- ``/mpc/reference_path`` - Reference trajectory for MPC

---

Message Conventions
-------------------

**Color Codes:**

The FEB system uses standardized color codes for cone classification:

- ``1`` - Yellow cones (typically right side of track)
- ``2`` - Blue cones (typically left side of track)
- ``-1`` - Unknown/unclassified cones (e.g., LiDAR detections without color)

**Coordinate Frames:**

- **Vehicle Frame (base_footprint)**: Origin at vehicle center, X forward, Y left
- **Global Frame (map)**: Fixed world frame established by SLAM
- Most messages use the vehicle frame unless specified in the header

**Array Correspondence:**

For messages with multiple arrays (e.g., ``ConesCartesian``), arrays of the same index correspond to the same object:

.. code-block:: python

   # Example: Third cone
   cone_x = msg.x[2]
   cone_y = msg.y[2]
   cone_color = msg.color[2]