Class ControllerNode

class ControllerNode : public rclcpp::Node

Controller node! ROS node that subscribes to spline and twist, and publishes control actions and debugging information.

Refer to the explainer for a more detailed overview.

Public Functions

ControllerNode(std::shared_ptr<state::StateEstimator> state_estimator, std::shared_ptr<mppi::MppiController> mppi_controller)

Construct the controller node. Launches MPPI in a new (detached) thread.

Parameters:

state_estimator – Shared pointer to a state estimator
mppi_controller – Shared pointer to an mppi controller

Private Functions

void spline_callback(const SplineMsg &spline_msg)

Callback for spline subscription. Forwards message to StateEstimator::on_spline, and notifies MPPI thread of the dirty state.

Parameters:: spline_msg – Received spline message

void world_twist_callback(const TwistMsg &twist_msg)

Callback for world twist subscription. Forwards message to StateEstimator::on_world_twist, and notifies MPPI thread of the dirty state. Likely from GPS.

Parameters:: twist_msg – Received twist message

void world_pose_callback(const PoseMsg &pose_msg)

Callback for world pose subscription. Forwards message to StateEstimator::on_world_pose, and notifies MPPI thread of the dirty state. Likely from GPS. Currently unused.

Parameters:: pose_msg – Received pose message

void publish_action(const Action &action)

Publishes a control action to the control_action topic.

Parameters:: action – Action to publish

std::thread launch_mppi()

Launch MPPI thread, which loops the following routine persistently:

Wait to be notified that the state is dirty.
Run MPPI and write an action to the write buffer.
Swap the read and write buffers.

Returns:: the launched thread

void notify_state_dirty(): Notify MPPI thread that the state is dirty, and to refire if waiting.

ActionMsg action_to_msg(const Action &action)

Converts MPPI control action output to a ROS2 message. Affected by drive mode (FWD, RWD, AWD).

Parameters:: action – [in] Control action - output of MPPI.

Private Members

std::shared_ptr<state::StateEstimator> m_state_estimator: State estimator instance

std::shared_ptr<mppi::MppiController> m_mppi_controller: MPPI Controller instance

rclcpp::Publisher<ActionMsg>::SharedPtr m_action_publisher: Publishes control action for actuators.

rclcpp::Publisher<InfoMsg>::SharedPtr m_info_publisher: Publishes controller info for debugging.

rclcpp::Subscription<SplineMsg>::SharedPtr m_spline_subscription: Subscribes to path planning spline.

rclcpp::Subscription<TwistMsg>::SharedPtr m_world_twist_subscription: Subscribes to intertial twist.

rclcpp::Subscription<QuatMsg>::SharedPtr m_world_quat_subscription: Subscribes to intertial quaternion.

rclcpp::Subscription<PoseMsg>::SharedPtr m_world_pose_subscription: Subscribes to inertial pose.

std::mutex m_state_mut: Mutex protecting m_state_estimator. This needs to be acquired when forwarding callbacks to the state estimator or waiting on m_state_cond_var

std::condition_variable m_state_cond_var

Condition variable for notifying state dirty-ing. MPPI waits on this variable while state and spline callbacks notify it. m_state_mut must be acquired before waiting on this.