2.2.3.3. control_node module¶

ROS Node for control module.

Planned trajectory by Motion Planning, estimated vehicle pose by SLAM and current steering wheel angle are input for a model predictive controller which uses a nonlinear bicycle model to predict which are the optimal control inputs (ackermann angle rate and acceleration) to follow this trajectory. The model predictive controller therefore looks one second into the future. The resulting predicted states and control inputs are used to calculate reference values for steering actuator, motor and service brake.

class control_node.AutonomousMissionEnum(value)[source]¶

Bases: Enum

Enum to describe which Autonomous Mission is currently select.

ACCELERATION = 1¶

AUTOCROSS = 6¶

BRAKETEST = 4¶

INSPECTION = 5¶

MANUAL_DRIVING = 0¶

SKIDPAD = 2¶

TRACKDRIVE = 3¶

class control_node.ControlNode[source]¶

Bases: object

Implement functionality for calculating reference values for actuators based on trajectory and vehicle pose.

Control node class to implement functionality for calculating reference values for steering actuator, motor and service brake with MPC based on planned trajectory, estimated vehicle pose and actual ackermann angle. Uses a nonlinear bicycle model within the MPC.

mission¶

Current selected Autonomous Mission by user on dashboard display.

Type:: AutonomousMissionEnum

steering_wheel_angle_control_value_time¶

Assumed dead time [s] of steering actuator and thus time to select from prediction horizon for steering reference value.

Type:: float

torque_control_value_time¶

Assumed dead time [s] of inverter and motor and thus time to select from prediction horizon for motor reference value.

Type:: float

service_brake_pressure_control_value_time¶

Assumed dead time [s] of service brake and thus time to select from prediction horizon for brake reference value.

Type:: float

steering_wheel_angle_range¶

Range of steering [degrees] from most left to most right steering wheel angle.

Type:: float

steering_wheel_angle_offset¶

Offset of steering wheel angle, where the vehicle really drives straight.

Type:: float

steering_wheel_angle_upper_boundary¶

Allowed upper boundary for steering wheel angle for the current selected mission.

Type:: float

steering_wheel_angle_lower_boundary¶

Allowed lower boundary for steering wheel angle for the current selected mission.

Type:: float

vehicle_model¶

Vehicle Model used for translating steering wheel angle to ackermann angle and the otherway around as well as acceleration to motor torque and service brake pressure.

Type:: VehicleModel

mpc¶

Model predictive controller instance to predict optimal control inputs for the prediction horizon.

Type:: MPC

steering_wheel_angle_pub¶

ROS publisher to publish steering wheel angle reference values as requests.

Type:: rospy.Publisher

torque_pub¶

ROS publisher to publish motor torque reference values as requests.

Type:: rospy.Publisher

service_brake_pressure_pub¶

ROS publisher to publish service brake pressure reference values as requests.

Type:: rospy.Publisher

predicted_path_pub¶

ROS publisher to publish visualization of predicted path.

Type:: rospy.Publisher

predicted_states_pub¶

ROS publisher to publish predicted states and predicted control inputs.

Type:: rospy.Publisher

mpc_statistics_pub¶

ROS publisher to publish statistics of the model prediction controller for the current prediction.

Type:: rospy.Publisher

tf_buffer¶

Transformation tree buffer for buffering transforms of the transformation tree. Used to get offset between rear axle and center of gravity.

Type:: tf2_ros.Buffer

transform_listener¶

Transformation tree listener to listen to transform of the transformation tree. Used to get offset between rear axle and center of gravity.

Type:: tf2_ros.TransformListener

rear_axle_to_cog_offset¶

Projected offset between rear axle and center of gravity in x direction.

Type:: float

state_vector¶

Current state vector of the the bicycle model of the MPC, shape: (5, ).

Type:: npt.NDArray

pred_x¶

Current predicted states of the bicycle model of the MPC for the prediction horizon, shape (n, 5) with n equals number of prediction steps. (x, y, psi, v_x, delta)

Type:: Optional[npt.NDArray]

pred_u¶

Current predicted control inputs of the bicycle model of the MPC for the prediction horizon, shape (n, 2) with n equals number of prediction steps. (a_x, delta_rate)

Type:: Optional[npt.NDArray]

pose_lock¶

Mutual exclusion lock for current vehicle pose estimated by SLAM.

Type:: threading.Lock

trajectory_lock¶

Mutual exclusion lock for current trajectory planned by Motion Planning.

Type:: threading.Lock

steering_lock¶

Mutual exclusion lock for current measured actual steering wheel angle.

Type:: threading.Lock

trajectory¶

Current planned trajectory calculated by Motion Planning.

Type:: Optional[TrajectoryPoints]

vehicle_pose¶

Current vehicle pose estimated by SLAM.

Type:: Optional[VehiclePose]

ackermann_angle_actual¶

Current actual ackermann angle converted with vehicle model from measured actual steering wheel angle.

Type:: Optional[float]

mission_machine_state¶

Current state of Autonomous System intern Mission Machine.

Type:: MissionMachineEnum

Initialize Control Node.

get_rear_axle_to_cog_offset()[source]¶: Get offset between rear axle and cog from tf tree and saves the offset as attribute.

get_state_vector_for_vehicle_pose_and_ackermann_angle(vehicle_pose: VehiclePose, ackermann_angle: float) → ndarray[Any, dtype[ScalarType]][source]¶

Get state vector (x, y, psi, v, delta) from ROS vehicle pose message and ackermann angle.

Translates rear axle position from vehicle pose message to center of gravity position.

Parameters:

vehicle_pose (VehiclePose) – Vehicle pose message originated from SLAM estimation.
ackermann_angle (float) – Ackermann angle.

Returns:

State vector (x, y, psi, v, delta), shape: (5, ).

Return type:

npt.NDArray

mission_completed_callback(msg: MissionFinished)[source]¶

Buffer whether the mission is completed calculated by finish line detection and received via ROS.

A completed mission signals that we should brake as fast as possible and allowed.

Parameters:: msg (MissionFinished) – Message indicating whether mission is completed.

mission_machine_state_callback(mission_machine_state: MissionMachineState)[source]¶

Buffer mission machine state from received ros message.

Parameters:: mission_machine_state (MissionMachineState) – Current State of the Autonomous System intern Mission Machine.

motion_planning_callback(trajectory: TrajectoryPoints)[source]¶

Buffer planned trajectory calculated by motion planning from received ros message..

Parameters:: trajectory (TrajectoryPoints) – Current by motion planning planned trajectory.

publish_control_requests(stamp: Time, time_offset: float = 0.0)[source]¶

Publish refrence values for steering, motor and brake based on the latest prediction of the mpc.

Interpolates the predicted states and control inputs based on the respective assumed deadtime of the actor and the time since the last succesful prediction (time_offset). This can be used to still send a new reference values after a failed prediction.

Also prevents requesting engine torque and service brake pressure at the same time. Also prevents sending service brake pressure during EBS test.

Parameters:

stamp (rospy.Time) – Timestamp for the header of the request messages.
time_offset (float, optional) – Time since the last succesful prediction, by default 0.

publish_mpc_statistics(stamp: Time)[source]¶

Publish statistics of the prediction of the model predictive controller.

Parameters:: stamp (rospy.Time) – Timestamp when the prediction has been executed for the header of the ROS message.

publish_old_control_requests(failed_steps_n: int, stamp: Time)[source]¶

Publish new control requests based on an old prediction.

Used when an execution of the model predictive controller and it’s respective predition was not succesful. An old prediction is used but a point more in the future is used.

Parameters:

failed_steps_n (int) – Number of predictions that failed since the last succesful prediction.
stamp (rospy.Time) – Timestamp for the header of the request messages.

publish_output(trajectory: TrajectoryPoints, vehicle_pose: VehiclePose, stamp: Time)[source]¶

Publish all output for the current prediction of the model predictive controller.

Parameters:

trajectory (TrajectoryPoints) – Trajectory for which the prediction has been executed. Used for its image header.
vehicle_pose (VehiclePose) – Vehicle pose for which the prediction has been executed. Used for its header.
stamp (rospy.Time) – Timestamp when the prediction has been executed for the header of the ROS messages.

publish_predicted_path_visualization(stamp: Time)[source]¶

Publish predicted states as path for visualization.

Parameters:: stamp (rospy.Time) – Timestamp for the header of the ROS message.

publish_predicted_states_and_inputs(trajectory: TrajectoryPoints, vehicle_pose: VehiclePose, stamp: Time)[source]¶

Publish predicted states and control inputs to ROS topic.

Publishes all states, all control inputs and all converted control inputs.

Parameters:

trajectory (TrajectoryPoints) – Trajectory for which the prediction has been executed. Used for its image header.
vehicle_pose (VehiclePose) – Vehicle pose for which the prediction has been executed. Used for its header.
stamp (rospy.Time) – Timestamp for the header of the ROS message.

publish_service_brake_pressure(service_brake_pressure: float, stamp: Time)[source]¶

Publish reference service brake pressure on ROS topic.

Parameters:

service_brake_pressure (float) – Service Brake pressure to request.
stamp (rospy.Time) – Timestamp for header of request message.

publish_steering_wheel_angle_request(steering_wheel_angle: float, stamp: Time)[source]¶

Publish reference steering wheel angle on ROS topic.

Parameters:

steering_wheel_angle (float) – Steering wheel angle to request.
stamp (rospy.Time) – Timestamp for header of request message.

publish_torque_request(torque: float, stamp: Time)[source]¶

Publish reference motor torque on ROS topic.

Parameters:

torque (float) – Motor torque to request.
stamp (rospy.Time) – Timestamp for header of request message.

publish_velocity_request(velocity: float, stamp: Time)[source]¶

Publish reference velocity on ROS topic.

Parameters:

velocity (float) – Vehicle velocity to request.
stamp (rospy.Time) – Timestamp for header of request message.

run()[source]¶

Run the control module.

Only executes the model predictive controller when MissionMachine State is Driving, PreFinished (mission completed but not finished) or emergency. Also Trajectory, vehicle pose and actual ackermann angle has to be received atleast once and the offset between rear axle and center of gravity has to be known.

Newest trajectory, actual ackermann angle and vehicle pose are read by using mutual exclusion.

If the prediction of the model predictive controller is sucecesful all output is published. If the prediction failed, but atleast on prediction had been succesful, new reference values based on old predictions are published. Statistics of the prediction are always publishing including the exit flag and time to solve.

steering_wheel_angle_callback(steering_wheel_angle: SteeringWheelAngle)[source]¶

Buffer current actual steering wheel angle measured by ACTC and received via ros message.

Converts and buffers current steering wheel angle [degrees] to ackermann angle [rad].

Parameters:: steering_wheel_angle (SteeringWheelAngle) – Current steering wheel angle in degrees.

vehicle_pose_callback(vehicle_pose: VehiclePose)[source]¶

Buffer current vehicle pose estaimted by SLAM received from ros message.

Parameters:: vehicle_pose (VehiclePose) – Current vehicle pose estimated by SLAM.

class control_node.MissionMachineEnum(value)[source]¶

Bases: Enum

Enum to describe the current state of the Autonomous System intern Mission Machine.

IDLE means AS OFF. READY equals AS READY but with any mission but Inspection Mission. DRIVING equals AS Driving but with any mission but Inspection Mission. PRE_FINISHED means that the mission is completed but the vehicle has to come to a standstill before it is finished. FINISHED equals AS Finish. EMERGENCY equals AS Emergency. PRE_INSPECTION equals AS READY but with Inspection Mission. INSPECTION equals AS Driving but with Inspection Mission.

DRIVING = 2¶

EMERGENCY = 5¶

FINISHED = 4¶

IDLE = 0¶

INSPECTION = 7¶

PRE_FINISHED = 3¶

PRE_INSPECTION = 6¶

READY = 1¶