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 republish_action(const Action &action, rclcpp::Time current_time)

Publishes a control action to the control_action topic.

Parameters:: action – Action to publish

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.

void publish_and_print_info(InfoMsg info, const std::string &additional_info = "")

Prints the information in a InfoMsg to the given stream (usually the console).

Parameters:

stream – [in] Stream to print to
info – [in] Info message to print

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_rosbag_action_publisher: Publishes control action for actuators.

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

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

rclcpp::Subscription<ActionMsg>::SharedPtr m_rosbag_action_subscription: Exclusively for when replaying rosbags, and using it for state projection.

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<PoseMsg>::SharedPtr m_world_pose_subscription: Subscribes to inertial pose.

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

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.