diff --git a/docker/action/model_predictive_control/model_predictive_control.Dockerfile b/docker/action/model_predictive_control/model_predictive_control.Dockerfile index a10d3c59..c9b05a3c 100644 --- a/docker/action/model_predictive_control/model_predictive_control.Dockerfile +++ b/docker/action/model_predictive_control/model_predictive_control.Dockerfile @@ -6,8 +6,12 @@ FROM ${BASE_IMAGE} as source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/action/model_predictive_control model_predictive_control +COPY src/action/local_planning local_planning COPY src/wato_msgs/sample_msgs sample_msgs +COPY src/action/model_predictive_control model_predictive_control + +# Copy in CARLA messages +RUN git clone --depth 1 https://github.com/carla-simulator/ros-carla-msgs.git --branch 1.3.0 # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ diff --git a/docker/infrastructure/foxglove/foxglove.Dockerfile b/docker/infrastructure/foxglove/foxglove.Dockerfile index 2802819b..4ae96c17 100644 --- a/docker/infrastructure/foxglove/foxglove.Dockerfile +++ b/docker/infrastructure/foxglove/foxglove.Dockerfile @@ -8,6 +8,9 @@ WORKDIR ${AMENT_WS}/src # Copy in source code COPY src/wato_msgs wato_msgs +# Copy in CARLA messages +RUN git clone --depth 1 https://github.com/carla-simulator/ros-carla-msgs.git --branch 1.3.0 + # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ rosdep install --from-paths . --ignore-src -r -s \ diff --git a/docker/simulation/carla_viz/carlaviz_entrypoint.sh b/docker/simulation/carla_viz/carlaviz_entrypoint.sh index dbae6309..00f9ae73 100644 --- a/docker/simulation/carla_viz/carlaviz_entrypoint.sh +++ b/docker/simulation/carla_viz/carlaviz_entrypoint.sh @@ -1,4 +1,3 @@ - is_backend_up="" function wait_for_backend_up() { @@ -35,6 +34,7 @@ fi echo "Backend launched." echo "Launching frontend" + # enable nginx service nginx restart echo "Frontend launched. Please open your browser" @@ -50,4 +50,5 @@ do exit 1 fi sleep 5 -done \ No newline at end of file +done + diff --git a/modules/dev_overrides/docker-compose.action.yaml b/modules/dev_overrides/docker-compose.action.yaml index 1984e6db..55f645d8 100644 --- a/modules/dev_overrides/docker-compose.action.yaml +++ b/modules/dev_overrides/docker-compose.action.yaml @@ -13,7 +13,7 @@ services: image: "${ACTION_GLOBAL_PLANNING_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/action/global_planning:/home/ament_ws/src/global_planning + - ${MONO_DIR}/src/action/global_planning:/home/bolty/ament_ws/src/global_planning behaviour_planning: <<: *fixuid @@ -23,7 +23,7 @@ services: image: "${ACTION_BEHAVIOUR_PLANNING_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/action/behaviour_planning:/home/ament_ws/src/behaviour_planning + - ${MONO_DIR}/src/action/behaviour_planning:/home/bolty/ament_ws/src/behaviour_planning local_planning: <<: *fixuid @@ -33,14 +33,13 @@ services: image: "${ACTION_LOCAL_PLANNING_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/action/local_planning:/home/ament_ws/src/local_planning + - ${MONO_DIR}/src/action/local_planning:/home/bolty/ament_ws/src/local_planning model_predictive_control: - <<: *fixuid extends: file: ../docker-compose.action.yaml service: model_predictive_control image: "${ACTION_MPC_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/action/model_predictive_control:/home/ament_ws/src/model_predictive_control \ No newline at end of file + - ${MONO_DIR}/src/action/model_predictive_control:/home/bolty/ament_ws/src/model_predictive_control \ No newline at end of file diff --git a/modules/dev_overrides/docker-compose.simulation.yaml b/modules/dev_overrides/docker-compose.simulation.yaml index 2da94571..51b5c454 100644 --- a/modules/dev_overrides/docker-compose.simulation.yaml +++ b/modules/dev_overrides/docker-compose.simulation.yaml @@ -15,10 +15,10 @@ services: extends: file: ../docker-compose.simulation.yaml service: carla_ros_bridge - command: tail -F anything + # command: tail -F anything volumes: - ${MONO_DIR}/src/simulation/carla_config:/home/bolty/ament_ws/src/carla_config - # command: /bin/bash -c "ros2 launch carla_config carla.launch.py" + command: /bin/bash -c "ros2 launch carla_config carla.launch.py" carla_viz: extends: diff --git a/modules/docker-compose.action.yaml b/modules/docker-compose.action.yaml index ab73667e..12ea3c2a 100644 --- a/modules/docker-compose.action.yaml +++ b/modules/docker-compose.action.yaml @@ -41,6 +41,6 @@ services: cache_from: - "${ACTION_MPC_IMAGE}:build_${TAG}" - "${ACTION_MPC_IMAGE}:build_main" - target: deploy + target: deploy image: "${ACTION_MPC_IMAGE}:${TAG}" command: /bin/bash -c "ros2 launch model_predictive_control model_predictive_control.launch.py" diff --git a/src/action/model_predictive_control/config/params.yaml b/src/action/model_predictive_control/config/params.yaml new file mode 100644 index 00000000..e69de29b diff --git a/src/action/model_predictive_control/launch/mpc_launch.py b/src/action/model_predictive_control/launch/mpc_launch.py new file mode 100644 index 00000000..e69de29b diff --git a/src/action/model_predictive_control/model_predictive_control/boxconstraint.py b/src/action/model_predictive_control/model_predictive_control/boxconstraint.py new file mode 100644 index 00000000..3a09cb20 --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/boxconstraint.py @@ -0,0 +1,76 @@ +import numpy as np +import torch + + +class BoxConstraint: + """ + 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. + :param ub: dimwise list of lower bounds. + :param plot_idxs: When plotting, the box itself might be defined in some dimension greater than 2 but we might only want to + plot the workspace variables and so plot_idxs allows us to limit the consideration of plot_constraint_set to those variables. + """ + self.lb = np.array(lb, ndmin=2).reshape(-1, 1) + 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" + self.setup_constraint_matrix() + + def __str__(self): return "Lower bound: %s, Upper bound: %s" % ( + self.lb, self.ub) + + def get_random_vectors(self, num_samples): + rand_samples = np.random.rand(self.dim, num_samples) + for i in range(self.dim): + scale_factor, shift_factor = (self.ub[i] - self.lb[i]), self.lb[i] + rand_samples[i, :] = (rand_samples[i, :] * + scale_factor) + shift_factor + return rand_samples + + def setup_constraint_matrix(self): + dim = self.lb.shape[0] + # Casadi can't do matrix mult with Torch instances but only numpy instead. So have to use the np version of the H and b matrix/vector when + # defining constraints in the opti stack. + self.H_np = np.vstack((-np.eye(dim), np.eye(dim))) + self.H = torch.Tensor(self.H_np) + # self.b = torch.Tensor(np.hstack((-self.lb, self.ub))) + self.b_np = np.vstack((-self.lb, self.ub)) + self.b = torch.Tensor(self.b_np) + # print(self.b) + self.sym_func = lambda x: self.H @ np.array(x, ndmin=2).T - self.b + + def check_satisfaction(self, sample): + # If sample is within the polytope defined by the constraints return 1 else 0. + # print(sample, np.array(sample, ndmin=2).T, self.sym_func(sample), self.b) + return (self.sym_func(sample) <= 0).all() + + def generate_uniform_samples(self, num_samples): + n = int(np.round(num_samples**(1. / self.lb.shape[0]))) + + # Generate a 1D array of n equally spaced values between the lower and + # upper bounds for each dimension + coords = [] + for i in range(self.lb.shape[0]): + coords.append(np.linspace(self.lb[i, 0], self.ub[i, 0], n)) + + # Create a meshgrid of all possible combinations of the n-dimensions + meshes = np.meshgrid(*coords, indexing='ij') + + # Flatten the meshgrid and stack the coordinates to create an array of + # size (K, n-dimensions) + samples = np.vstack([m.flatten() for m in meshes]) + + # Truncate the array to K samples + samples = samples[:num_samples, :] + + # Print the resulting array + return samples + + def clip_to_bounds(self, samples): + return np.clip(samples, self.lb, self.ub) diff --git a/src/action/model_predictive_control/model_predictive_control/mpc.py b/src/action/model_predictive_control/model_predictive_control/mpc.py new file mode 100644 index 00000000..59e3833f --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/mpc.py @@ -0,0 +1,327 @@ +import carla +import casadi as ca +import numpy as np +import datetime +import os +import shutil + +from boxconstraint import BoxConstraint + +SIM_DURATION = 500 # Simulation duration in time steps + +## SETUP ## +# Connect to CARLA +client = carla.Client('localhost', 2000) +maps = [m.replace('/Game/Carla/Maps/', '') + for m in client.get_available_maps()] +print('Available maps: ', maps) +world = client.get_world() +mymap = world.get_map() +print('Using map: ', mymap.name) +spectator = world.get_spectator() + +# CARLA Settings +settings = world.get_settings() +# Timing settings +settings.synchronous_mode = True # Enables synchronous mode +TIME_STEP = 0.05 # Time step for synchronous mode +settings.fixed_delta_seconds = TIME_STEP +# Physics substep settings +settings.substepping = True +settings.max_substep_delta_time = 0.01 +settings.max_substeps = 10 + +world.apply_settings(settings) + +# Output client and world objects to console +print(client) +print(world) + + +# Function to move the spectator camera +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.set_transform(spectator_transform) + + +# Use recommended spawn points +spawn_points = mymap.get_spawn_points() +spawn_point = spawn_points[0] + +# Spawn vehicle +vehicles = world.get_actors().filter('vehicle.*') +blueprint_library = world.get_blueprint_library() +vehicle_bp = blueprint_library.filter('model3')[0] +print("Vehicle blueprint attributes:") +for attr in vehicle_bp: + print(' - {}'.format(attr)) + +if len(vehicles) == 0: + vehicle = world.spawn_actor(vehicle_bp, spawn_point) +else: + # Reset world + for vehicle in vehicles: + vehicle.destroy() + vehicle = world.spawn_actor(vehicle_bp, spawn_point) +print(vehicle) + + +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 + return ca.vertcat(waypoint_x, waypoint_y) + + +def generate_waypoint(x, y): + return ca.vertcat(x, y) + + +waypoints = [] + +for i in range(SIM_DURATION): + waypoints.append(generate_waypoint_relative_to_spawn(-10, 0)) + +# Parameters +params = { + 'L': 2.875 # Wheelbase of the vehicle. Source : https://www.tesla.com/ownersmanual/model3/en_us/GUID-56562137-FC31-4110-A13C-9A9FC6657BF0.html +} +T = 2.0 # Prediction horizon in seconds +N = int(T / TIME_STEP) # Prediction horizon in time steps +dt = TIME_STEP # Time step for discretization +state_dim = 4 # Dimension of the state [x, y, theta, v] +# Dimension of the control input [steering angle, acceleration] +control_dim = 2 + +# Initialize Opti object +opti = ca.Opti() + +# Declare variables +# state trajectory variables over prediction horizon +X = opti.variable(state_dim, N + 1) +# control trajectory variables over prediction horizon +U = opti.variable(control_dim, N) +P = opti.parameter(state_dim) # initial state parameter +# Base state penalty matrix (emphasizes position states) +Q_base = ca.MX.eye(state_dim) +# Increase factor for each step in the prediction horizon +weight_increase_factor = 1.00 +R = ca.MX.eye(control_dim) # control penalty matrix for objective function +W = opti.parameter(2, N) # Reference trajectory parameter + +# Objective +obj = 0 +for k in range(N): + # 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 + x_next = X[:, k + 1] # Next state + + # Reference state with waypoint and zero for other states + x_ref = ca.vertcat(W[:, k], ca.MX.zeros(state_dim - 2, 1)) + + dx = x_k - x_ref # Deviation of state from reference state + # Control input deviation (assuming a desired control input of zero) + du = u_k + + # Quadratic cost with reference state and control input + # Minimize quadratic cost and deviation from reference state + obj += ca.mtimes([ca.mtimes(dx.T, Q), dx]) + \ + ca.mtimes([ca.mtimes(du.T, R), du]) + +opti.minimize(obj) + +# Maximum steerin angle for dynamics +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 +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(N): + # 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]), + X[3, k] * ca.sin(X[2, k]), + (X[3, k] / params['L']) * ca.tan(steering_angle_rad), + U[1, k] + )) + +# Constraints +opti.subject_to(X[:, 0] == P) # Initial state constraint + +# Input constraints +steering_angle_bounds = [-1.0, 1.0] +acceleration_bounds = [-1.0, 1.0] +lb = np.array([steering_angle_bounds[0], + acceleration_bounds[0]]).reshape(-1, 1) +ub = np.array([steering_angle_bounds[1], + acceleration_bounds[1]]).reshape(-1, 1) +action_space = BoxConstraint(lb=lb, ub=ub) + +# State constraints +# x_bounds = [-10000, 1000] # x position bounds (effectively no bounds) +# y_bounds = [-1000, 1000] # y position bounds (effectively no bounds) +# theta_bounds = [0, 360] # theta bounds in degrees +# v_bounds = [-10, 10] # velocity bounds +# lb = np.array([x_bounds[0], y_bounds[0], theta_bounds[0], v_bounds[0]]).reshape(-1, 1) +# ub = np.array([x_bounds[1], y_bounds[1], theta_bounds[1], v_bounds[1]]).reshape(-1, 1) +# state_space = BoxConstraint(lb=lb, ub=ub) + +# Apply constraints to optimization problem +for i in range(N): + # Input constraints + opti.subject_to(action_space.H_np @ U[:, i] <= action_space.b_np) + + # State constraints + # opti.subject_to(state_space.H_np @ X[:, i] <= state_space.b_np) + +# Setup solver +acceptable_dual_inf_tol = 1e11 +acceptable_compl_inf_tol = 1e-3 +acceptable_iter = 15 +acceptable_constr_viol_tol = 1e-3 +acceptable_tol = 1e-6 + +opts = {"ipopt.acceptable_tol": acceptable_tol, + "ipopt.acceptable_constr_viol_tol": acceptable_constr_viol_tol, + "ipopt.acceptable_dual_inf_tol": acceptable_dual_inf_tol, + "ipopt.acceptable_iter": acceptable_iter, + "ipopt.acceptable_compl_inf_tol": acceptable_compl_inf_tol, + "ipopt.hessian_approximation": "limited-memory", + "ipopt.print_level": 0} +opti.solver('ipopt', opts) + +# Array to store closed-loop trajectory states (X and Y coordinates) +closed_loop_data = [] +open_loop_data = [] +residuals_data = [] + +# Initialize warm-start parameters +prev_sol_x = None +prev_sol_u = None + +# Main Loop +for i in range( + SIM_DURATION - + N): # Subtract N since we need to be able to predict N steps into the future + print("Iteration: ", i) + + move_spectator_to_vehicle(vehicle, spectator) + + # Draw current waypoints in CARLA + for waypoint in waypoints[i:i + N]: + waypoint_x = float(np.array(waypoint[0])) + waypoint_y = float(np.array(waypoint[1])) + + carla_waypoint = carla.Location(x=waypoint_x, y=waypoint_y, z=0.5) + + 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)) + + # Fetch initial state from CARLA + x0 = vehicle.get_transform().location.x + y0 = vehicle.get_transform().location.y + theta0 = vehicle.get_transform().rotation.yaw / 180 * ca.pi + velocity_vector = vehicle.get_velocity() + v0 = ca.sqrt(velocity_vector.x ** 2 + velocity_vector.y ** 2) + + print("Current x: ", x0) + print("Current y: ", y0) + print("Current yaw: ", vehicle.get_transform().rotation.yaw) + print("Current theta: ", theta0) + print("Current velocity: ", v0) + + # Store current state in the closed-loop trajectory data + if i > 0: + closed_loop_data.append([x0, y0, theta0, v0]) + + # Set initial state for optimization problem + initial_state = ca.vertcat(x0, y0, theta0, v0) + opti.set_value(P, initial_state) + + # Set the reference trajectory for the current iteration + opti.set_value(W, ca.horzcat(*waypoints[i:i + N])) # Concatenate waypoints + + if prev_sol_x is not None and prev_sol_u is not None: + # Warm-starting the solver with the previous solution + opti.set_initial(X, prev_sol_x) + opti.set_initial(U, prev_sol_u) + + # Solve the optimization problem + sol = opti.solve() + + # If the solver is successful, apply the first control input to the vehicle + if sol.stats()['success']: + u = sol.value(U[:, 0]) + + # Bound acceleration and steering angle to [-1, 1] + u[0] = np.clip(u[0], -1, 1) + u[1] = np.clip(u[1], -1, 1) + + print("Steering angle: ", u[0]) + print("Acceleration: ", u[1]) + + if u[1] < 0: + vehicle.apply_control( + carla.VehicleControl( + throttle=-u[1], + steer=u[0], + reverse=True)) + else: + vehicle.apply_control( + carla.VehicleControl( + throttle=u[1], steer=u[0])) + + # Store open-loop trajectory data with control input applied to vehicle + open_loop_trajectory = sol.value(X) + open_loop_trajectory = open_loop_trajectory.T.reshape(-1, state_dim) + open_loop_trajectory = np.hstack( + (open_loop_trajectory, np.tile(u, (N + 1, 1)))) + open_loop_data.append(open_loop_trajectory) + + # Compute and store residuals if i > 0 since we need a previous state + # to compare + if i > 0: + # Predicted next state from the previous solution + predicted_state = prev_sol_x[:, 1] + # Current actual state from CARLA + actual_state = np.array([x0, y0, theta0, v0]) + residual = actual_state - predicted_state + residuals_data.append(residual) + + # Update previous solution variables for warm-starting next iteration + prev_sol_x = sol.value(X) + prev_sol_u = sol.value(U) + + else: + print("Error in optimization problem.") + + print("") + world.tick() # Tick the CARLA world diff --git a/src/action/model_predictive_control/model_predictive_control/mpc_core.py b/src/action/model_predictive_control/model_predictive_control/mpc_core.py new file mode 100644 index 00000000..d6d5740e --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/mpc_core.py @@ -0,0 +1,259 @@ +import casadi as ca +import numpy as np +import datetime +import os +import shutil + +from boxconstraint import BoxConstraint + +TIME_STEP = 0.05 +PREDICTION_HORIZON = 2.0 +SIM_DURATION = 500 + + +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.T = PREDICTION_HORIZON # Prediction horizon in seconds + self.N = int(T / TIME_STEP) # Prediction horizon in time steps + self.dt = TIME_STEP # Time step for discretization + self.state_dim = 4 # Dimension of the state [x, y, theta, v] + # Dimension of the control input [steering angle, acceleration] + self.control_dim = 2 + + # Initialize Opti object + self.opti = ca.Opti() + + # Declare variables + # 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 + # Base state penalty matrix (emphasizes position states) + self.Q_base = ca.MX.eye(self.state_dim) + # Increase factor for each step in the prediction horizon + self.weight_increase_factor = 1.00 + # control penalty matrix for objective function + self.R = ca.MX.eye(self.control_dim) + # Reference trajectory parameter + self.W = self.opti.parameter(2, self.N) + + # Objective + self.obj = 0 + + self.raw_waypoints = [] + self.waypoints = [] + + self.closed_loop_data = [] + self.open_loop_data = [] + self.residuals_data = [] + + self.prev_sol_x = None + self.prev_sol_u = None + + # MPC Initial Setup + self.get_waypoints() + self.setup_mpc() + self.setup_constraints() + self.setup_solver() + + # Vehicle States + self.x0 = 0 + self.y0 = 0 + self.theta0 = 0 + self.v0 = 0 + + def convert_waypoints(self): + """ + Convert raw waypoints (alternating x, y in a flat list) to CasADi-compatible waypoints. + """ + self.waypoints = [] # Clear old waypoints + + if len(self.raw_waypoints) % 2 != 0: # Check if raw_waypoints is even + print( + "Error: raw_waypoints length is not even. Ignoring the last unpaired value.") + self.raw_waypoints = self.raw_waypoints[:-1] + + for i in range(0, len(self.raw_waypoints), 2): + x, y = self.raw_waypoints[i], self.raw_waypoints[i + 1] + waypoint = self.generate_waypoint(x, y) + self.waypoints.append(waypoint) + + def generate_waypoint(x, y): # Convert to CasADi format and add to the waypoints list + return ca.vertcat(x, y) + + def setup_mpc(self): + """ + Setup the MPC problem with CasADi + """ + + for k in range(self.N): + # 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 + + # Reference state with waypoint and zero for other states + x_ref = ca.vertcat( + self.W[:, k], ca.MX.zeros(self.state_dim - 2, 1)) + + dx = x_k - x_ref # Deviation of state from reference state + # Control input deviation (assuming a desired control input of + # zero) + du = u_k + + # Quadratic cost with reference state and control input + # Minimize quadratic cost and deviation from reference state + self.obj += ca.mtimes([ca.mtimes(dx.T, Q), dx]) + \ + ca.mtimes([ca.mtimes(du.T, self.R), du]) + + self.opti.minimize(self.obj) + + # 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 + + # Dynamics (Euler discretization using bicycle model) + for k in range(self.N): + # 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.X[3, k] * + ca.sin(self.X[2, k]), (self.X[3, k] / + self.params['L']) * + ca.tan(steering_angle_rad), self.U[1, k])) + + def setup_constraints(self): + + # Initial state constraint + self.opti.subject_to(self.X[:, 0] == self.P) + + # Input constraints + steering_angle_bounds = [-1.0, 1.0] + acceleration_bounds = [-1.0, 1.0] + lb = np.array([steering_angle_bounds[0], + acceleration_bounds[0]]).reshape(-1, 1) + ub = np.array([steering_angle_bounds[1], + acceleration_bounds[1]]).reshape(-1, 1) + action_space = BoxConstraint(lb=lb, ub=ub) + + # State constraints + # x_bounds = [-10000, 1000] # x position bounds (effectively no bounds) + # y_bounds = [-1000, 1000] # y position bounds (effectively no bounds) + # theta_bounds = [0, 360] # theta bounds in degrees + # v_bounds = [-10, 10] # velocity bounds + # lb = np.array([x_bounds[0], y_bounds[0], theta_bounds[0], v_bounds[0]]).reshape(-1, 1) + # ub = np.array([x_bounds[1], y_bounds[1], theta_bounds[1], v_bounds[1]]).reshape(-1, 1) + # state_space = BoxConstraint(lb=lb, ub=ub) + + # Apply constraints to optimization problem + for i in range(self.N): + # Input constraints + self.opti.subject_to(action_space.H_np @ + self.U[:, i] <= action_space.b_np) + + # 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 + acceptable_iter = 15 + acceptable_constr_viol_tol = 1e-3 + acceptable_tol = 1e-6 + + opts = {"ipopt.acceptable_tol": acceptable_tol, + "ipopt.acceptable_constr_viol_tol": acceptable_constr_viol_tol, + "ipopt.acceptable_dual_inf_tol": acceptable_dual_inf_tol, + "ipopt.acceptable_iter": acceptable_iter, + "ipopt.acceptable_compl_inf_tol": acceptable_compl_inf_tol, + "ipopt.hessian_approximation": "limited-memory", + "ipopt.print_level": 0} + self.opti.solver('ipopt', opts) + + def compute_control(self, i): + """ + Update the vehicle state based on the incoming ROS message. + :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(x, y, theta0, v0)) + + print("Current x: ", x0) + print("Current y: ", y0) + print("Current theta: ", theta0) + print("Current velocity: ", v0) + + if i > 0: + # Original Code need i > 0 + self.closed_loop_data.append([x0, y0, theta0, v0]) + + 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 + + if self.prev_sol_x is not None and self.prev_sol_u is not None: + # Warm-starting the solver with the previous solution + self.opti.set_initial(self.X, self.prev_sol_x) + self.opti.set_initial(self.U, self.prev_sol_u) + + """ + Solve the MPC optimization problem and return the control commands. + :return: Tuple of (steering angle, throttle). + """ + 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]) + steering_angle = np.clip(u[0], -1.0, 1.0) + throttle = np.clip(u[1], -1.0, 1.0) + + print("Steering angle: ", u[0]) + print("Acceleration: ", u[1]) + + # Store open-loop trajectory data with control input applied to + # vehicle + open_loop_trajectory = sol.value(self.X) + open_loop_trajectory = open_loop_trajectory.T.reshape( + -1, self.state_dim) + open_loop_trajectory = np.hstack( + (open_loop_trajectory, np.tile(u, (N + 1, 1)))) + self.open_loop_data.append(open_loop_trajectory) + + if i > 0: + # Predicted next state from the previous solution + predicted_state = self.prev_sol_x[:, 1] + # Current actual state from CARLA + actual_state = np.array([x0, y0, theta0, v0]) + 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 diff --git a/src/action/model_predictive_control/model_predictive_control/mpc_node.py b/src/action/model_predictive_control/model_predictive_control/mpc_node.py new file mode 100644 index 00000000..2c7954d3 --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/mpc_node.py @@ -0,0 +1,119 @@ +# Copyright 2023 WATonomous +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node + +from wato_msgs / simulation / path_planning_msgs import CarlaEgoVehicleControl, CarlaEgoVehicleStatus +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import Pose, Quaternion + +from model_predictive_control.mpc_core import MPCCore + +# For extracting theta from w in quaternion +from tf_transformations import euler_from_quaternion + + +class MPCNode(Node): + def __init__(self): + super().__init__('MPCNode') + + self.mpc_core = MPCCore() + + # Subscribe to vehicle state (only retrieving velocity) + self.state_subscription = self.create_subscription( + CarlaEgoVehicleStatus, + '/carla/ego/vehicle_status', + self.vehicle_state_callback, + 10) + + # Subscribe to get vehicle position/orientation (x, y, w) + self.state_odom_subscription = self.create_subscription( + Odometry, 'T/carla/ego/odometry', self.state_odom_callback, 10) + + self.control_publisher = self.create_publisher( + CarlaEgoVehicleControl, '/carla/ego/vehicle_control_cmd', 10) + + self.goal_publisher = self.create_publisher( + Pose, '/carla/ego/goal', 10) + + # Subscribe to waypoints from CARLA + self.waypoints_subscription = self.create_subscription( + Path, '/carla/ego/waypoints', self.waypoints_callback, 10) + + self.goal_point_x = 10 + self.goal_point_y = 10 + publish_goal(self.goal_point_x, self.goal_point_y) + + def vehicle_state_callback(self, msg): + mpc_core.v0 = msg.velocity + + # Extract theta/yaw/orientation of the car in the x-y plane from + # quaternion + quaternion = [ + msg.orientation.x, + msg.orientation.y, + msg.orientation.z, + msg.orientation.w] + _, _, mpc_core.theta0 = euler_from_quaternion(quaternion) + + def waypoints_callback(self, msg): + for pose_stamped in msg.poses: + x = pose_stamped.pose.position.x + y = pose_stamped.pose.position.y + mpc_core.raw_waypoints.append(x) + mpc_core.raw_waypoints.append(y) + + mpc_core.convert_waypoints() + start_main_loop() + + def state_odom_callback(self, msg): + mpc_core.x = msg.pose.pose.position.x + mpc_core.y = msg.pose.pose.position.y + + def publish_goal(self, x, y): + goal_msg = Pose() + + goal_msg.position.x = x + goal_msg.position.y = y + goal_msg.position.z = 0.0 + goal_msg.orientation.x = 0.0 + goal_msg.orientation.y = 0.0 + goal_msg.orientation.z = 0.0 + goal_msg.orientation.w = 1.0 + + self.goal_publisher.publish(goal_msg) + + def start_main_loop(self): + # Subtract N since we need to be able to predict N steps into the + # future + for i in range(self.mpc_core.SIM_DURATION - self.mpc_core.N): + steering_angle, throttle = self.mpc_core.compute_control(i) + + control_msg = CarlaEgoVehicleControl() + control_msg.steer = steering_angle + control_msg.throttle = throttle + self.control_publisher.publish(control_msg) + + +def main(args=None): + rclpy.init(args=args) + mpc_node = MPCNode() + rclpy.spin(mpc_node) + mpc_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/action/model_predictive_control/package.xml b/src/action/model_predictive_control/package.xml index a855111e..90884e4c 100644 --- a/src/action/model_predictive_control/package.xml +++ b/src/action/model_predictive_control/package.xml @@ -8,7 +8,7 @@ TODO: License declaration ament_cmake - + geometry_msgs ament_cmake diff --git a/src/action/model_predictive_control/test/mpc_test.py b/src/action/model_predictive_control/test/mpc_test.py new file mode 100644 index 00000000..589a05c3 --- /dev/null +++ b/src/action/model_predictive_control/test/mpc_test.py @@ -0,0 +1,2 @@ +# create some trajectory and init mpc and carla nodes + diff --git a/src/simulation/carla_config/launch/carla.launch.py b/src/simulation/carla_config/launch/carla.launch.py index d3af646c..c39e8e17 100644 --- a/src/simulation/carla_config/launch/carla.launch.py +++ b/src/simulation/carla_config/launch/carla.launch.py @@ -150,10 +150,18 @@ def generate_launch_description(): output='screen' ) - waypoint_topic = DeclareLaunchArgument('waypoint_topic', default_value=[ - '/carla/', LaunchConfiguration('role_name'), '/waypoints']) - waypoint_topic_old = DeclareLaunchArgument('waypoint_topic_old', default_value=[ - '/carla/', LaunchConfiguration('role_name'), '/waypointsOld']) + waypoint_topic = DeclareLaunchArgument( + 'waypoint_topic', + default_value=[ + '/carla/', + LaunchConfiguration('role_name'), + '/waypoints']) + waypoint_topic_old = DeclareLaunchArgument( + 'waypoint_topic_old', + default_value=[ + '/carla/', + LaunchConfiguration('role_name'), + '/waypointsOld']) """ Launch CARLA Waypoint Publisher """ carla_waypoint_publisher = Node( diff --git a/src/simulation/carla_notebooks/boxconstraint.py b/src/simulation/carla_notebooks/boxconstraint.py new file mode 100644 index 00000000..3a09cb20 --- /dev/null +++ b/src/simulation/carla_notebooks/boxconstraint.py @@ -0,0 +1,76 @@ +import numpy as np +import torch + + +class BoxConstraint: + """ + 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. + :param ub: dimwise list of lower bounds. + :param plot_idxs: When plotting, the box itself might be defined in some dimension greater than 2 but we might only want to + plot the workspace variables and so plot_idxs allows us to limit the consideration of plot_constraint_set to those variables. + """ + self.lb = np.array(lb, ndmin=2).reshape(-1, 1) + 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" + self.setup_constraint_matrix() + + def __str__(self): return "Lower bound: %s, Upper bound: %s" % ( + self.lb, self.ub) + + def get_random_vectors(self, num_samples): + rand_samples = np.random.rand(self.dim, num_samples) + for i in range(self.dim): + scale_factor, shift_factor = (self.ub[i] - self.lb[i]), self.lb[i] + rand_samples[i, :] = (rand_samples[i, :] * + scale_factor) + shift_factor + return rand_samples + + def setup_constraint_matrix(self): + dim = self.lb.shape[0] + # Casadi can't do matrix mult with Torch instances but only numpy instead. So have to use the np version of the H and b matrix/vector when + # defining constraints in the opti stack. + self.H_np = np.vstack((-np.eye(dim), np.eye(dim))) + self.H = torch.Tensor(self.H_np) + # self.b = torch.Tensor(np.hstack((-self.lb, self.ub))) + self.b_np = np.vstack((-self.lb, self.ub)) + self.b = torch.Tensor(self.b_np) + # print(self.b) + self.sym_func = lambda x: self.H @ np.array(x, ndmin=2).T - self.b + + def check_satisfaction(self, sample): + # If sample is within the polytope defined by the constraints return 1 else 0. + # print(sample, np.array(sample, ndmin=2).T, self.sym_func(sample), self.b) + return (self.sym_func(sample) <= 0).all() + + def generate_uniform_samples(self, num_samples): + n = int(np.round(num_samples**(1. / self.lb.shape[0]))) + + # Generate a 1D array of n equally spaced values between the lower and + # upper bounds for each dimension + coords = [] + for i in range(self.lb.shape[0]): + coords.append(np.linspace(self.lb[i, 0], self.ub[i, 0], n)) + + # Create a meshgrid of all possible combinations of the n-dimensions + meshes = np.meshgrid(*coords, indexing='ij') + + # Flatten the meshgrid and stack the coordinates to create an array of + # size (K, n-dimensions) + samples = np.vstack([m.flatten() for m in meshes]) + + # Truncate the array to K samples + samples = samples[:num_samples, :] + + # Print the resulting array + return samples + + def clip_to_bounds(self, samples): + return np.clip(samples, self.lb, self.ub) diff --git a/src/simulation/carla_notebooks/mpc_script.py b/src/simulation/carla_notebooks/mpc_script.py new file mode 100644 index 00000000..59e3833f --- /dev/null +++ b/src/simulation/carla_notebooks/mpc_script.py @@ -0,0 +1,327 @@ +import carla +import casadi as ca +import numpy as np +import datetime +import os +import shutil + +from boxconstraint import BoxConstraint + +SIM_DURATION = 500 # Simulation duration in time steps + +## SETUP ## +# Connect to CARLA +client = carla.Client('localhost', 2000) +maps = [m.replace('/Game/Carla/Maps/', '') + for m in client.get_available_maps()] +print('Available maps: ', maps) +world = client.get_world() +mymap = world.get_map() +print('Using map: ', mymap.name) +spectator = world.get_spectator() + +# CARLA Settings +settings = world.get_settings() +# Timing settings +settings.synchronous_mode = True # Enables synchronous mode +TIME_STEP = 0.05 # Time step for synchronous mode +settings.fixed_delta_seconds = TIME_STEP +# Physics substep settings +settings.substepping = True +settings.max_substep_delta_time = 0.01 +settings.max_substeps = 10 + +world.apply_settings(settings) + +# Output client and world objects to console +print(client) +print(world) + + +# Function to move the spectator camera +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.set_transform(spectator_transform) + + +# Use recommended spawn points +spawn_points = mymap.get_spawn_points() +spawn_point = spawn_points[0] + +# Spawn vehicle +vehicles = world.get_actors().filter('vehicle.*') +blueprint_library = world.get_blueprint_library() +vehicle_bp = blueprint_library.filter('model3')[0] +print("Vehicle blueprint attributes:") +for attr in vehicle_bp: + print(' - {}'.format(attr)) + +if len(vehicles) == 0: + vehicle = world.spawn_actor(vehicle_bp, spawn_point) +else: + # Reset world + for vehicle in vehicles: + vehicle.destroy() + vehicle = world.spawn_actor(vehicle_bp, spawn_point) +print(vehicle) + + +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 + return ca.vertcat(waypoint_x, waypoint_y) + + +def generate_waypoint(x, y): + return ca.vertcat(x, y) + + +waypoints = [] + +for i in range(SIM_DURATION): + waypoints.append(generate_waypoint_relative_to_spawn(-10, 0)) + +# Parameters +params = { + 'L': 2.875 # Wheelbase of the vehicle. Source : https://www.tesla.com/ownersmanual/model3/en_us/GUID-56562137-FC31-4110-A13C-9A9FC6657BF0.html +} +T = 2.0 # Prediction horizon in seconds +N = int(T / TIME_STEP) # Prediction horizon in time steps +dt = TIME_STEP # Time step for discretization +state_dim = 4 # Dimension of the state [x, y, theta, v] +# Dimension of the control input [steering angle, acceleration] +control_dim = 2 + +# Initialize Opti object +opti = ca.Opti() + +# Declare variables +# state trajectory variables over prediction horizon +X = opti.variable(state_dim, N + 1) +# control trajectory variables over prediction horizon +U = opti.variable(control_dim, N) +P = opti.parameter(state_dim) # initial state parameter +# Base state penalty matrix (emphasizes position states) +Q_base = ca.MX.eye(state_dim) +# Increase factor for each step in the prediction horizon +weight_increase_factor = 1.00 +R = ca.MX.eye(control_dim) # control penalty matrix for objective function +W = opti.parameter(2, N) # Reference trajectory parameter + +# Objective +obj = 0 +for k in range(N): + # 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 + x_next = X[:, k + 1] # Next state + + # Reference state with waypoint and zero for other states + x_ref = ca.vertcat(W[:, k], ca.MX.zeros(state_dim - 2, 1)) + + dx = x_k - x_ref # Deviation of state from reference state + # Control input deviation (assuming a desired control input of zero) + du = u_k + + # Quadratic cost with reference state and control input + # Minimize quadratic cost and deviation from reference state + obj += ca.mtimes([ca.mtimes(dx.T, Q), dx]) + \ + ca.mtimes([ca.mtimes(du.T, R), du]) + +opti.minimize(obj) + +# Maximum steerin angle for dynamics +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 +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(N): + # 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]), + X[3, k] * ca.sin(X[2, k]), + (X[3, k] / params['L']) * ca.tan(steering_angle_rad), + U[1, k] + )) + +# Constraints +opti.subject_to(X[:, 0] == P) # Initial state constraint + +# Input constraints +steering_angle_bounds = [-1.0, 1.0] +acceleration_bounds = [-1.0, 1.0] +lb = np.array([steering_angle_bounds[0], + acceleration_bounds[0]]).reshape(-1, 1) +ub = np.array([steering_angle_bounds[1], + acceleration_bounds[1]]).reshape(-1, 1) +action_space = BoxConstraint(lb=lb, ub=ub) + +# State constraints +# x_bounds = [-10000, 1000] # x position bounds (effectively no bounds) +# y_bounds = [-1000, 1000] # y position bounds (effectively no bounds) +# theta_bounds = [0, 360] # theta bounds in degrees +# v_bounds = [-10, 10] # velocity bounds +# lb = np.array([x_bounds[0], y_bounds[0], theta_bounds[0], v_bounds[0]]).reshape(-1, 1) +# ub = np.array([x_bounds[1], y_bounds[1], theta_bounds[1], v_bounds[1]]).reshape(-1, 1) +# state_space = BoxConstraint(lb=lb, ub=ub) + +# Apply constraints to optimization problem +for i in range(N): + # Input constraints + opti.subject_to(action_space.H_np @ U[:, i] <= action_space.b_np) + + # State constraints + # opti.subject_to(state_space.H_np @ X[:, i] <= state_space.b_np) + +# Setup solver +acceptable_dual_inf_tol = 1e11 +acceptable_compl_inf_tol = 1e-3 +acceptable_iter = 15 +acceptable_constr_viol_tol = 1e-3 +acceptable_tol = 1e-6 + +opts = {"ipopt.acceptable_tol": acceptable_tol, + "ipopt.acceptable_constr_viol_tol": acceptable_constr_viol_tol, + "ipopt.acceptable_dual_inf_tol": acceptable_dual_inf_tol, + "ipopt.acceptable_iter": acceptable_iter, + "ipopt.acceptable_compl_inf_tol": acceptable_compl_inf_tol, + "ipopt.hessian_approximation": "limited-memory", + "ipopt.print_level": 0} +opti.solver('ipopt', opts) + +# Array to store closed-loop trajectory states (X and Y coordinates) +closed_loop_data = [] +open_loop_data = [] +residuals_data = [] + +# Initialize warm-start parameters +prev_sol_x = None +prev_sol_u = None + +# Main Loop +for i in range( + SIM_DURATION - + N): # Subtract N since we need to be able to predict N steps into the future + print("Iteration: ", i) + + move_spectator_to_vehicle(vehicle, spectator) + + # Draw current waypoints in CARLA + for waypoint in waypoints[i:i + N]: + waypoint_x = float(np.array(waypoint[0])) + waypoint_y = float(np.array(waypoint[1])) + + carla_waypoint = carla.Location(x=waypoint_x, y=waypoint_y, z=0.5) + + 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)) + + # Fetch initial state from CARLA + x0 = vehicle.get_transform().location.x + y0 = vehicle.get_transform().location.y + theta0 = vehicle.get_transform().rotation.yaw / 180 * ca.pi + velocity_vector = vehicle.get_velocity() + v0 = ca.sqrt(velocity_vector.x ** 2 + velocity_vector.y ** 2) + + print("Current x: ", x0) + print("Current y: ", y0) + print("Current yaw: ", vehicle.get_transform().rotation.yaw) + print("Current theta: ", theta0) + print("Current velocity: ", v0) + + # Store current state in the closed-loop trajectory data + if i > 0: + closed_loop_data.append([x0, y0, theta0, v0]) + + # Set initial state for optimization problem + initial_state = ca.vertcat(x0, y0, theta0, v0) + opti.set_value(P, initial_state) + + # Set the reference trajectory for the current iteration + opti.set_value(W, ca.horzcat(*waypoints[i:i + N])) # Concatenate waypoints + + if prev_sol_x is not None and prev_sol_u is not None: + # Warm-starting the solver with the previous solution + opti.set_initial(X, prev_sol_x) + opti.set_initial(U, prev_sol_u) + + # Solve the optimization problem + sol = opti.solve() + + # If the solver is successful, apply the first control input to the vehicle + if sol.stats()['success']: + u = sol.value(U[:, 0]) + + # Bound acceleration and steering angle to [-1, 1] + u[0] = np.clip(u[0], -1, 1) + u[1] = np.clip(u[1], -1, 1) + + print("Steering angle: ", u[0]) + print("Acceleration: ", u[1]) + + if u[1] < 0: + vehicle.apply_control( + carla.VehicleControl( + throttle=-u[1], + steer=u[0], + reverse=True)) + else: + vehicle.apply_control( + carla.VehicleControl( + throttle=u[1], steer=u[0])) + + # Store open-loop trajectory data with control input applied to vehicle + open_loop_trajectory = sol.value(X) + open_loop_trajectory = open_loop_trajectory.T.reshape(-1, state_dim) + open_loop_trajectory = np.hstack( + (open_loop_trajectory, np.tile(u, (N + 1, 1)))) + open_loop_data.append(open_loop_trajectory) + + # Compute and store residuals if i > 0 since we need a previous state + # to compare + if i > 0: + # Predicted next state from the previous solution + predicted_state = prev_sol_x[:, 1] + # Current actual state from CARLA + actual_state = np.array([x0, y0, theta0, v0]) + residual = actual_state - predicted_state + residuals_data.append(residual) + + # Update previous solution variables for warm-starting next iteration + prev_sol_x = sol.value(X) + prev_sol_u = sol.value(U) + + else: + print("Error in optimization problem.") + + print("") + world.tick() # Tick the CARLA world diff --git a/src/simulation/carla_notebooks/mpc_test.py b/src/simulation/carla_notebooks/mpc_test.py new file mode 100644 index 00000000..80e85421 --- /dev/null +++ b/src/simulation/carla_notebooks/mpc_test.py @@ -0,0 +1,14 @@ +import carla +import os +import random + +## 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") + +# Connect to the client and retrieve the world object +client = carla.Client(client_name, 2000) +world = client.get_world() +spectator = world.get_spectator() diff --git a/src/simulation/carla_sim/launch/carla_sim.launch.py b/src/simulation/carla_sim/launch/carla_sim.launch.py new file mode 100644 index 00000000..45a077bb --- /dev/null +++ b/src/simulation/carla_sim/launch/carla_sim.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +import launch_ros.actions + +# For creating launch arguments +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + publish_autopilot_arg = DeclareLaunchArgument( + 'publish_autopilot', + default_value='False' + ) + + return LaunchDescription( + [ + launch_ros.actions.Node( + namespace="carla_sample_node", + package='carla_sample_node', + executable='carla_sample_node', + parameters=[{ + 'publish_autopilot': LaunchConfiguration('publish_autopilot') + }], + output='screen') + ]) diff --git a/src/simulation/carla_sim/package.xml b/src/simulation/carla_sim/package.xml new file mode 100644 index 00000000..7e3d0b25 --- /dev/null +++ b/src/simulation/carla_sim/package.xml @@ -0,0 +1,20 @@ + + + + carla_sample_node + 0.0.0 + Sample python node to read data from CARLA + Vishal Jayakumar + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + rclpy + + + ament_python + + \ No newline at end of file diff --git a/src/simulation/carla_sim/setup.cfg b/src/simulation/carla_sim/setup.cfg new file mode 100644 index 00000000..e9a26297 --- /dev/null +++ b/src/simulation/carla_sim/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/carla_sample_node +[install] +install_scripts=$base/lib/carla_sample_node \ No newline at end of file diff --git a/src/simulation/carla_sim/setup.py b/src/simulation/carla_sim/setup.py new file mode 100644 index 00000000..18127651 --- /dev/null +++ b/src/simulation/carla_sim/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup +from glob import glob +import os + +package_name = 'carla_sample_node' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, glob(os.path.join('launch', '*.launch.py'))) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Vishal Jayakumar', + maintainer_email='v3jayaku@watonomous.ca', + description='Sample python node to read data from CARLA', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'carla_sample_node = carla_sample_node.carla_sample_node:main' + ], + }, +) diff --git a/src/simulation/carla_sim/temp.txt b/src/simulation/carla_sim/temp.txt deleted file mode 100644 index f41acb40..00000000 --- a/src/simulation/carla_sim/temp.txt +++ /dev/null @@ -1 +0,0 @@ -place holder for actual sim code \ No newline at end of file diff --git a/watod-config.sh b/watod-config.sh index 3255dd40..eafc5ed9 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -15,7 +15,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -# ACTIVE_MODULES="" +ACTIVE_MODULES="simulation action" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. @@ -23,11 +23,11 @@ ## - deploy (default) : runs production-grade containers (non-editable) ## - develop : runs developer containers (editable) -# MODE_OF_OPERATION="" +MODE_OF_OPERATION="develop" ############################## ADVANCED CONFIGURATIONS ############################## ## Name to append to docker containers. DEFAULT = "" -# COMPOSE_PROJECT_NAME="" +COMPOSE_PROJECT_NAME="hasan3773" ## Tag to use. Images are formatted as : with forward slashes replaced with dashes. ## DEFAULT = ""