Navigation Messages (C++)

horus::msg::NavGoal goal{};
goal.target_x = 5.0;       // meters
goal.target_y = 3.0;       // meters
goal.target_theta = 1.57;  // radians (face east)
goal.tolerance = 0.1f;     // accept within 10cm
goal.timestamp_ns = 0;

horus::msg::PathPlan plan{};
plan.waypoint_count = 3;
plan.goal_pose[0] = 5.0f;  // goal x
plan.goal_pose[1] = 3.0f;  // goal y
plan.goal_pose[2] = 0.0f;  // goal heading

// Waypoint 0: (0,0,0)
plan.waypoint_data[0] = 0.0f;
plan.waypoint_data[1] = 0.0f;
plan.waypoint_data[2] = 0.0f;

// Waypoint 1: (2.5, 1.5, 0.5)
plan.waypoint_data[3] = 2.5f;
plan.waypoint_data[4] = 1.5f;
plan.waypoint_data[5] = 0.5f;

// Waypoint 2: (5.0, 3.0, 0.0) = goal
plan.waypoint_data[6] = 5.0f;
plan.waypoint_data[7] = 3.0f;
plan.waypoint_data[8] = 0.0f;