-# create some trajectory and init mpc and carla nodes \ No newline at end of file +# create some trajectory and init mpc and carla nodes

from boxconstraint import BoxConstraint TIME_STEP = 0.05 -PREDICTION_HORIZON = 2.0 +PREDICTION_HORIZON = 2.0 class MPCCore: def __init__(self): - + self.params = { 'L': 2.875 # Wheelbase of the vehicle. Source : https://www.tesla.com/ownersmanual/model3/en_us/GUID-56562137-FC31-4110-A13C-9A9FC6657BF0.html }

self.dt = TIME_STEP # Time step for discretization self.state_dim = 4 # Dimension of the state [x, y, theta, v] self.control_dim = 2 # Dimension of the control input [steering angle, acceleration] - + # Initialize Opti object self.opti = ca.Opti() # Declare variables - self.X = self.opti.variable(self.state_dim, self.N + 1) # state trajectory variables over prediction horizon - self.U = self.opti.variable(self.control_dim, self.N) # control trajectory variables over prediction horizon + # state trajectory variables over prediction horizon + self.X = self.opti.variable(self.state_dim, self.N + 1) + # control trajectory variables over prediction horizon + self.U = self.opti.variable(self.control_dim, self.N) self.P = self.opti.parameter(self.state_dim) # initial state parameter - self.Q_base = ca.MX.eye(self.state_dim) # Base state penalty matrix (emphasizes position states) + # Base state penalty matrix (emphasizes position states) + self.Q_base = ca.MX.eye(self.state_dim) self.weight_increase_factor = 1.00 # Increase factor for each step in the prediction horizon self.R = ca.MX.eye(self.control_dim) # control penalty matrix for objective function self.W = self.opti.parameter(2, self.N) # Reference trajectory parameter - + # Objective self.obj = 0

# # Handle missing or invalid waypoint coordinates # print(f"Invalid waypoint: {wp}") - - def generate_waypoint(x, y): # Convert to CasADi format and add to the waypoints list + def generate_waypoint(x, y): # Convert to CasADi format and add to the waypoints list return ca.vertcat(x, y) def setup_mpc(self):

""" for k in range(self.N): - Q = self.Q_base * (self.weight_increase_factor ** k) # Increase weight for each step in the prediction horizon + # Increase weight for each step in the prediction horizon + Q = self.Q_base * (self.weight_increase_factor ** k) x_k = self.X[:, k] # Current state u_k = self.U[:, k] # Current control input x_next = self.X[:, k + 1] # Next state x_ref = ca.vertcat(self.W[:, k], - ca.MX.zeros(self.state_dim - 2, 1)) # Reference state with waypoint and zero for other states + ca.MX.zeros(self.state_dim - 2, 1)) # Reference state with waypoint and zero for other states dx = x_k - x_ref # Deviation of state from reference state du = u_k # Control input deviation (assuming a desired control input of zero)

# Maximum steerin angle for dynamics self.max_steering_angle_deg = max(wheel.max_steer_angle for wheel in - vehicle.get_physics_control().wheels) # Maximum steering angle in degrees (from vehicle physics control - self.max_steering_angle_rad = max_steering_angle_deg * (ca.pi / 180) # Maximum steering angle in radians + vehicle.get_physics_control().wheels) # Maximum steering angle in degrees (from vehicle physics control + self.max_steering_angle_rad = max_steering_angle_deg * \ + (ca.pi / 180) # Maximum steering angle in radians # Dynamics (Euler discretization using bicycle model) for k in range(self.N): - steering_angle_rad = self.U[0, k] * self.max_steering_angle_rad # Convert normalized steering angle to radians + # Convert normalized steering angle to radians + steering_angle_rad = self.U[0, k] * self.max_steering_angle_rad self.opti.subject_to(self.X[:, k + 1] == self.X[:, k] + self.dt * ca.vertcat( self.X[3, k] * ca.cos(self.X[2, k]),

self.U[1, k] )) - def setup_constraints(self): - + self.opti.subject_to(self.X[:, 0] == self.P) # Initial state constraint # Input constraints

# State constraints # opti.subject_to(state_space.H_np @ X[:, i] <= state_space.b_np) - + def setup_solver(self): acceptable_dual_inf_tol = 1e11 acceptable_compl_inf_tol = 1e-3

:param vehicle_state: VehicleState message with current position, velocity, and angle. """ # Update P (initial state) with the new vehicle state - self.opti.set_value(self.P, ca.vertcat(vehicle_state.pos_x, vehicle_state.pos_y, vehicle_state.angle, vehicle_state.velocity)) - + self.opti.set_value(self.P, ca.vertcat(vehicle_state.pos_x, + vehicle_state.pos_y, vehicle_state.angle, vehicle_state.velocity)) + x0 = vehicle_state.pos_x y0 = vehicle_state.pos_y theta0 = vehicle_state.angle - v0 = vehicle_state.velocity # Assumes CARLA sends a velocity scalar not a velocity vector + v0 = vehicle_state.velocity # Assumes CARLA sends a velocity scalar not a velocity vector i = vehicle_state.interation print("Current x: ", x0)

print("Current velocity: ", v0) if i > 0: - self.closed_loop_data.append([x0, y0, theta0, v0]) # Original Code need i > 0 + self.closed_loop_data.append([x0, y0, theta0, v0]) # Original Code need i > 0 initial_state = ca.vertcat(x0, y0, theta0, v0) self.opti.set_value(self.P, initial_state) # Set the reference trajectory for the current iteration - self.opti.set_value(self.W, ca.horzcat(*self.waypoints[i:i + self.N])) # Concatenate waypoints + self.opti.set_value(self.W, ca.horzcat( + *self.waypoints[i:i + self.N])) # Concatenate waypoints if self.prev_sol_x is not None and self.prev_sol_u is not None: # Warm-starting the solver with the previous solution

""" steering_angle = 0 throttle = 0 - + # Solve the optimization problem sol = self.opti.solve() - + if sol.stats()['success']: # Extract control inputs (steering angle, throttle) u = sol.value(self.U[:, 0])

self.open_loop_data.append(open_loop_trajectory) if i > 0: - predicted_state = self.prev_sol_x[:, 1] # Predicted next state from the previous solution + # Predicted next state from the previous solution + predicted_state = self.prev_sol_x[:, 1] actual_state = np.array([x0, y0, theta0, v0]) # Current actual state from CARLA residual = actual_state - predicted_state self.residuals_data.append(residual) - + # Update previous solution variables for warm-starting next iteration self.prev_sol_x = sol.value(self.X) self.prev_sol_u = sol.value(self.U)

else: print("Error in optimization problem.") - return steering_angle, throttle \ No newline at end of file + return steering_angle, throttle

import rclpy from rclpy.node import Node -from action_msgs import ControlCommands, VehicleState, WaypointArray +from action_msgs import ControlCommands, VehicleState, WaypointArray from carla_core import CarlaCore # Import the CARLA core logic

self.carla_core = CarlaCore(self.publish_state) # Publishers - self.vehicle_state_publisher = self.create_publisher(VehicleState, '/carla/vehicle_state', 10) + self.vehicle_state_publisher = self.create_publisher( + VehicleState, '/carla/vehicle_state', 10) self.waypoints_publisher = self.create_publisher(WaypointArray, '/carla/waypoints', 10) # Subscribers self.control_subscription = self.create_subscription( ControlCommands, '/mpc/control_commands', self.control_callback, 10) - self.carla_core.start_main_loop() - def publish_state(self): # Get the current vehicle state from carla_core

if __name__ == '__main__': - main() \ No newline at end of file + main()

""" Bounded constraints lb <= x <= ub as polytopic constraints -Ix <= -b and Ix <= b. np.vstack(-I, I) forms the H matrix from III-D-b of the paper """ + def __init__(self, lb=None, ub=None, plot_idxs=None): """ :param lb: dimwise list of lower bounds.

self.ub = np.array(ub, ndmin=2).reshape(-1, 1) self.plot_idxs = plot_idxs self.dim = self.lb.shape[0] - assert (self.lb < self.ub).all(), "Lower bounds must be greater than corresponding upper bound for any given dimension" + assert (self.lb < self.ub).all( + ), "Lower bounds must be greater than corresponding upper bound for any given dimension" self.setup_constraint_matrix() def __str__(self): return "Lower bound: %s, Upper bound: %s" % (self.lb, self.ub)

return samples def clip_to_bounds(self, samples): - return np.clip(samples, self.lb, self.ub) \ No newline at end of file + return np.clip(samples, self.lb, self.ub)

# See the License for the specific language governing permissions and # limitations under the License. -import rclpy +import rclpy from rclpy.node import Node -from action_msgs import ControlCommands, VehicleState, WaypointArray +from action_msgs import ControlCommands, VehicleState, WaypointArray from model_predictive_control.mpc_core import MPCCore

# Subscribe to waypoints from CARLA self.waypoints_subscription = self.create_subscription( WaypointArray, '/carla/waypoints', self.waypoints_callback, 10) - + self.control_publisher = self.create_publisher(ControlCommands, '/mpc/control_commands', 10) def vehicle_state_callback(self, msg):

if __name__ == '__main__': - main() \ No newline at end of file + main()

SIM_DURATION = 500 # Simulation duration in time steps in Carla TIME_STEP = 0.05 -PREDICTION_HORIZON = 2.0 +PREDICTION_HORIZON = 2.0 + class CarlaCore: def __init__(self, publish_state):

vehicle = world.spawn_actor(vehicle_bp, spawn_point) print(vehicle) - def get_waypoints(self): """Generate and return the list of waypoints relative to the vehicle's spawn point.""" - for i in range(SIM_DURATION): + for i in range(SIM_DURATION): self.waypoints.append(self.generate_waypoint_relative_to_spawn(-10, 0)) return self.waypoints def generate_waypoint_relative_to_spawn(self, forward_offset=0, sideways_offset=0): - waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + spawn_point.get_right_vector().x * sideways_offset - waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + spawn_point.get_right_vector().y * sideways_offset + waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + \ + spawn_point.get_right_vector().x * sideways_offset + waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + \ + spawn_point.get_right_vector().y * sideways_offset return [waypoint_x, waypoint_y] def get_vehicle_state(self):

Applies the received control commands to the vehicle. """ if throttle < 0: - self.vehicle.apply_control(carla.VehicleControl(throttle=-throttle, steer=steering_angle, reverse=True)) + self.vehicle.apply_control(carla.VehicleControl( + throttle=-throttle, steer=steering_angle, reverse=True)) else: - self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steering_angle)) + self.vehicle.apply_control(carla.VehicleControl( + throttle=throttle, steer=steering_angle)) def start_main_loop(self): """

extent = carla.Location(x=0.5, y=0.5, z=0.5) world.debug.draw_box(box=carla.BoundingBox(carla_waypoint, extent * 1e-2), - rotation=carla.Rotation(pitch=0, yaw=0, roll=0), life_time=TIME_STEP * 10, thickness=0.5, - color=carla.Color(255, 0, 0)) - - self.publish_state() # MPC Should Start after this !!!! - + rotation=carla.Rotation(pitch=0, yaw=0, roll=0), life_time=TIME_STEP * 10, thickness=0.5, + color=carla.Color(255, 0, 0)) + + self.publish_state() # MPC Should Start after this !!!! + # Should automatically apply control after publishing state to mpc print("") world.tick() # Tick the CARLA world - - + def move_spectator_to_vehicle(distance=10): vehicle_location = vehicle.get_location() # Set viewing angle to slightly above the vehicle - spectator_transform = carla.Transform(vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) + spectator_transform = carla.Transform( + vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) spectator.set_transform(spectator_transform) def publish_state(self): """To be overridden by CarlaNode for publishing the vehicle state.""" - pass \ No newline at end of file + pass

def move_spectator_to_vehicle(vehicle, spectator, distance=10): vehicle_location = vehicle.get_location() # Set viewing angle to slightly above the vehicle - spectator_transform = carla.Transform(vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) + spectator_transform = carla.Transform( + vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) spectator.set_transform(spectator_transform)

def generate_waypoint_relative_to_spawn(forward_offset=0, sideways_offset=0): - waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + spawn_point.get_right_vector().x * sideways_offset - waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + spawn_point.get_right_vector().y * sideways_offset + waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + \ + spawn_point.get_right_vector().x * sideways_offset + waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + \ + spawn_point.get_right_vector().y * sideways_offset return ca.vertcat(waypoint_x, waypoint_y)

# Objective obj = 0 for k in range(N): - Q = Q_base * (weight_increase_factor ** k) # Increase weight for each step in the prediction horizon + # Increase weight for each step in the prediction horizon + Q = Q_base * (weight_increase_factor ** k) x_k = X[:, k] # Current state u_k = U[:, k] # Current control input

# Dynamics (Euler discretization using bicycle model) for k in range(N): - steering_angle_rad = U[0, k] * max_steering_angle_rad # Convert normalized steering angle to radians + # Convert normalized steering angle to radians + steering_angle_rad = U[0, k] * max_steering_angle_rad opti.subject_to(X[:, k + 1] == X[:, k] + dt * ca.vertcat( X[3, k] * ca.cos(X[2, k]),

import rclpy from rclpy.node import Node -from action_msgs import ControlCommands, VehicleState, WaypointArray +from action_msgs import ControlCommands, VehicleState, WaypointArray from carla_core import CarlaCore # Import the CARLA core logic

self.carla_core = CarlaCore(self.publish_state) # Publishers - self.vehicle_state_publisher = self.create_publisher(VehicleState, '/carla/vehicle_state', 10) + self.vehicle_state_publisher = self.create_publisher( + VehicleState, '/carla/vehicle_state', 10) self.waypoints_publisher = self.create_publisher(WaypointArray, '/carla/waypoints', 10) # Subscribers self.control_subscription = self.create_subscription( ControlCommands, '/mpc/control_commands', self.control_callback, 10) - self.carla_core.start_main_loop() - def publish_state(self): # Get the current vehicle state from carla_core

if __name__ == '__main__': - main() \ No newline at end of file + main()

SIM_DURATION = 500 # Simulation duration in time steps in Carla TIME_STEP = 0.05 -PREDICTION_HORIZON = 2.0 +PREDICTION_HORIZON = 2.0 + class CarlaCore: def __init__(self, publish_state):

vehicle = world.spawn_actor(vehicle_bp, spawn_point) print(vehicle) - def get_waypoints(self): """Generate and return the list of waypoints relative to the vehicle's spawn point.""" - for i in range(SIM_DURATION): + for i in range(SIM_DURATION): self.waypoints.append(self.generate_waypoint_relative_to_spawn(-10, 0)) return self.waypoints def generate_waypoint_relative_to_spawn(self, forward_offset=0, sideways_offset=0): - waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + spawn_point.get_right_vector().x * sideways_offset - waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + spawn_point.get_right_vector().y * sideways_offset + waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + \ + spawn_point.get_right_vector().x * sideways_offset + waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + \ + spawn_point.get_right_vector().y * sideways_offset return [waypoint_x, waypoint_y] def get_vehicle_state(self):

Applies the received control commands to the vehicle. """ if throttle < 0: - self.vehicle.apply_control(carla.VehicleControl(throttle=-throttle, steer=steering_angle, reverse=True)) + self.vehicle.apply_control(carla.VehicleControl( + throttle=-throttle, steer=steering_angle, reverse=True)) else: - self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steering_angle)) + self.vehicle.apply_control(carla.VehicleControl( + throttle=throttle, steer=steering_angle)) def start_main_loop(self): """

extent = carla.Location(x=0.5, y=0.5, z=0.5) world.debug.draw_box(box=carla.BoundingBox(carla_waypoint, extent * 1e-2), - rotation=carla.Rotation(pitch=0, yaw=0, roll=0), life_time=TIME_STEP * 10, thickness=0.5, - color=carla.Color(255, 0, 0)) - - self.publish_state() # MPC Should Start after this !!!! - + rotation=carla.Rotation(pitch=0, yaw=0, roll=0), life_time=TIME_STEP * 10, thickness=0.5, + color=carla.Color(255, 0, 0)) + + self.publish_state() # MPC Should Start after this !!!! + # Should automatically apply control after publishing state to mpc print("") world.tick() # Tick the CARLA world - - + def move_spectator_to_vehicle(distance=10): vehicle_location = vehicle.get_location() # Set viewing angle to slightly above the vehicle - spectator_transform = carla.Transform(vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) + spectator_transform = carla.Transform( + vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) spectator.set_transform(spectator_transform) def publish_state(self): """To be overridden by CarlaNode for publishing the vehicle state.""" - pass \ No newline at end of file + pass

def move_spectator_to_vehicle(vehicle, spectator, distance=10): vehicle_location = vehicle.get_location() # Set viewing angle to slightly above the vehicle - spectator_transform = carla.Transform(vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) + spectator_transform = carla.Transform( + vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) spectator.set_transform(spectator_transform)

def generate_waypoint_relative_to_spawn(forward_offset=0, sideways_offset=0): - waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + spawn_point.get_right_vector().x * sideways_offset - waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + spawn_point.get_right_vector().y * sideways_offset + waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + \ + spawn_point.get_right_vector().x * sideways_offset + waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + \ + spawn_point.get_right_vector().y * sideways_offset return ca.vertcat(waypoint_x, waypoint_y)

# Objective obj = 0 for k in range(N): - Q = Q_base * (weight_increase_factor ** k) # Increase weight for each step in the prediction horizon + # Increase weight for each step in the prediction horizon + Q = Q_base * (weight_increase_factor ** k) x_k = X[:, k] # Current state u_k = U[:, k] # Current control input

# Dynamics (Euler discretization using bicycle model) for k in range(N): - steering_angle_rad = U[0, k] * max_steering_angle_rad # Convert normalized steering angle to radians + # Convert normalized steering angle to radians + steering_angle_rad = U[0, k] * max_steering_angle_rad opti.subject_to(X[:, k + 1] == X[:, k] + dt * ca.vertcat( X[3, k] * ca.cos(X[2, k]),

""" Bounded constraints lb <= x <= ub as polytopic constraints -Ix <= -b and Ix <= b. np.vstack(-I, I) forms the H matrix from III-D-b of the paper """ + def __init__(self, lb=None, ub=None, plot_idxs=None): """ :param lb: dimwise list of lower bounds.

self.ub = np.array(ub, ndmin=2).reshape(-1, 1) self.plot_idxs = plot_idxs self.dim = self.lb.shape[0] - assert (self.lb < self.ub).all(), "Lower bounds must be greater than corresponding upper bound for any given dimension" + assert (self.lb < self.ub).all( + ), "Lower bounds must be greater than corresponding upper bound for any given dimension" self.setup_constraint_matrix() def __str__(self): return "Lower bound: %s, Upper bound: %s" % (self.lb, self.ub)

return samples def clip_to_bounds(self, samples): - return np.clip(samples, self.lb, self.ub) \ No newline at end of file + return np.clip(samples, self.lb, self.ub)

## SETUP ## client_name = os.environ.get("CLIENT_NAME", "DOES NOT EXIST") if client_name == "DOES NOT EXIST": - raise Exception("The environment variable for the container name of the carla server has not been set") + raise Exception( + "The environment variable for the container name of the carla server has not been set") # Connect to the client and retrieve the world object client = carla.Client(client_name, 2000) world = client.get_world() spectator = world.get_spectator() -

# Include our package.xml file (os.path.join('share', package_name), ['package.xml']), # Include all launch files. - (os.path.join('share', package_name, 'launch'), \ + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))), ], install_requires=['setuptools'],

The following actions uses node12 which is deprecated and will be forced to run on node16: actions/setup-python@v1. For more info: https://github.blog/changelog/2023-06-13-github-actions-all-actions-will-run-on-node16-instead-of-node12-by-default/