Class state::StateProjector

class StateProjector

Public Functions

void record_action(Action action, rclcpp::Time time)

Record an action into the history.

Parameters:

  • action – [in] The action to be recorded

  • time – [in] The time at which the action was taken by the actuators.

void record_speed(float speed, rclcpp::Time time)

Record a speed into the history.

Parameters:

  • speed – [in] The speed to be recorded

  • time – [in] The time at which the speed was measured. Should have no latency.

void record_pose(float x, float y, float yaw, rclcpp::Time time)

Record a pose into the history. Although pose data itself is not measured, this is called when the spline is updated, and a pose of (0,0,0) is inferred from the spline.

Parameters:

  • x – The x coordinate of the pose

  • y – The y coordinate of the pose

  • yaw – The yaw of the vehicle w.r.t. the inertial coordinate frame

  • time – The time at which the vehicle had the pose. Since the pose is inferred from the spline, this should be when the LIDAR points first came in.

State project(const rclcpp::Time &time, LoggerFunc logger) const

“main” projection function. Projects the state of the car at a given time, from the most recent pose record and the history of actions and speeds since that pose record.

Parameters:

  • time – The time at which the state is to be projected

  • logger – The logger function to be used

Returns:

The projected state of the car at the given time

bool is_ready() const

Whether the StateProjector is ready to project a state. This is true if there is a pose record, which is every time since the first spline is received.

Returns:

True if the StateProjector is ready to project a state, false otherwise.

Private Functions

void print_history() const

Prints the elements of m_history_since_pose, for debugging purposes.

Private Members

Record m_init_action   = { .action {}, .time = rclcpp::Time(0UL, default_clock_type), .type = Record::Type::Action}

“Default” action to be used until a recorded action is available

Note

m_init_action and m_init_speed should occur <= m_pose_record, if m_pose_record exists

Record m_init_speed   = { .speed = 0, .time = rclcpp::Time(0UL, default_clock_type), .type = Record::Type::Speed}

“Default” speed to be used until a recorded speed is available

Note

direction of velocity is inferred from swangle

std::optional<Record> m_pose_record = std::nullopt

most recent and only pose (new pose implies a new coord. frame, throw away data in old coord. frame) only nullopt before first pose received

std::multiset<Record, CompareRecordTimes> m_history_since_pose = {}

Contains all action and speed records since the last pose record. Multiset is used like a self-sorting array.

Invariants of m_history_since_pose: should only contain Action and Speed records time stamps of each record should be strictly after m_pose_record

struct CompareRecordTimes

Helper binary operator for sorting records by time, needed for the multiset.

struct Record

Historical record type.