Setting up MPC architecture

docker/action/model_predictive_control/model_predictive_control.Dockerfile

@@ -6,7 +6,7 @@ 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
 
 # Scan for rosdeps

modules/dev_overrides/docker-compose.action.yaml

@@ -36,11 +36,10 @@ services:
       - ${MONO_DIR}/src/action/local_planning:/home/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
+      - ${MONO_DIR}/src/action/model_predictive_control:/home/bolty/model_predictive_control

modules/docker-compose.action.yaml

@@ -41,6 +41,5 @@ services:
       cache_from:
         - "${ACTION_MPC_IMAGE}:build_${TAG}"
         - "${ACTION_MPC_IMAGE}:build_main"
-      target: deploy
     image: "${ACTION_MPC_IMAGE}:${TAG}"
     command: /bin/bash -c "ros2 launch model_predictive_control model_predictive_control.launch.py"

modules/docker-compose.simulation.yaml

@@ -5,11 +5,16 @@ services:
     image: carlasim/carla:0.9.13
     environment:
       - DISPLAY=1
-      - CUDA_VISIBLE_DEVICES=0,1,2
-      - NVIDIA_VISIBLE_DEVICES=0,1,2
-    runtime: nvidia
+      - CUDA_VISIBLE_DEVICES=0
+      - NVIDIA_VISIBLE_DEVICES=0
     restart: always
     command: /bin/bash -c "./CarlaUE4.sh -nosound -carla-server -RenderOffscreen -world-port=2000 -quality-level=Low"
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              capabilities: [gpu]
 
   carla_ros_bridge:
     build:
@@ -70,7 +75,7 @@ services:
       - SCENARIO_RUNNER_ROOT=/home/bolty/scenario_runner
       - DISPLAY=1
       - CUDA_VISIBLE_DEVICES=0
-      - NVIDIA_VISIBLE_DEVICES=1
+      - NVIDIA_VISIBLE_DEVICES=0
     container_name: ${COMPOSE_PROJECT_NAME:?}_carla_notebooks
     volumes:
       - ${MONO_DIR}/src/simulation/carla_notebooks:/home/bolty/carla_notebooks

src/action/model_predictive_control/model_predictive_control/boxconstraint.py

@@ -0,0 +1,70 @@
+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)

src/action/model_predictive_control/model_predictive_control/carla_core.py

@@ -0,0 +1,165 @@
+import carla
+import numpy as np
+import datetime
+import os
+import shutil
+
+SIM_DURATION = 500  # Simulation duration in time steps in Carla
+TIME_STEP = 0.05
+PREDICTION_HORIZON = 2.0 
+
+class CarlaCore:
+    def __init__(self, publish_state):
+        self.publish_state = publish_state
+
+        self.spectator = None
+        self.vehicles = None
+        self.spawn_point = None
+
+        self.setup_carla()
+
+        self.vehicle_state = {
+            'x': 0.0,
+            'y': 0.0,
+            'theta': 0.0,
+            'velocity': 0.0,
+            'iteration': 0
+        }
+
+        self.waypoints = []
+
+    def setup_carla(self):
+        ## 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)
+        self.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)
+
+        # Use recommended spawn points
+        self.spawn_points = mymap.get_spawn_points()
+        self.spawn_point = spawn_points[0]
+
+        # Spawn vehicle
+        self.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 get_waypoints(self):
+        """Generate and return the list of waypoints relative to the vehicle's spawn point."""
+
+        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
+        return [waypoint_x, waypoint_y]
+
+    def get_vehicle_state(self):
+        """
+        Retrieves the current state of the vehicle in the CARLA world.
+        The state includes position, orientation (theta), velocity, and iteration count.
+        """
+
+        #  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)
+
+        # Update vehicle state
+        self.vehicle_state['x'] = x0
+        self.vehicle_state['y'] = y0
+        self.vehicle_state['theta'] = theta0
+        self.vehicle_state['velocity'] = v0
+
+        return self.vehicle_state
+
+    def apply_control(self, steering_angle, throttle):
+        """
+        Applies the received control commands to the vehicle.
+        """
+        if throttle < 0:
+            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))
+
+    def start_main_loop(self):
+        """
+        Main loop to continuously publish vehicle states and waypoints.
+        """
+        N = int(PREDICTION_HORIZON/TIME_STEP)
+
+        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)
+
+            self.vehicle_state['iteration'] = i
+
+            move_spectator_to_vehicle()
+
+            # Draw current waypoints in CARLA
+            for waypoint in self.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))
+
+            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.set_transform(spectator_transform)
+
+    def publish_state(self):
+        """To be overridden by CarlaNode for publishing the vehicle state."""
+        pass

src/action/model_predictive_control/model_predictive_control/carla_node.py

@@ -0,0 +1,80 @@
+import rclpy
+from rclpy.node import Node
+
+from action_msgs import ControlCommands, VehicleState, WaypointArray 
+from carla_core import CarlaCore  # Import the CARLA core logic
+
+
+class CarlaNode(Node):
+    def __init__(self):
+        super().__init__('CarlaNode')
+
+        # Initialize CARLA core object (manages CARLA setup and running)
+        self.carla_core = CarlaCore(self.publish_state)
+
+        # Publishers
+        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
+        vehicle_state = self.carla_core.get_vehicle_state()
+        if vehicle_state:
+            # Publish the vehicle state to MPC node
+            state_msg = state_msgs()
+            state_msg.pos_x = vehicle_state['x']
+            state_msg.pos_y = vehicle_state['y']
+            state_msg.angle = vehicle_state['theta']
+            state_msg.velocity = vehicle_state['velocity']
+            self.vehicle_state_publisher.publish(state_msg)
+
+    def publish_waypoints(self):
+        # Get the current waypoints from carla_core
+        waypoints = self.carla_core.get_waypoints()
+        if waypoints:
+            # Create an instance of WaypointArray message
+            waypoint_array_msg = WaypointArray()
+
+            for wp in waypoints:
+                # Create a Waypoint message for each waypoint in the list
+                waypoint_msg = Waypoint()
+                waypoint_msg.x = wp[0]  # x-coordinate
+                waypoint_msg.y = wp[1]  # y-coordinate
+
+                # Append each Waypoint message to the WaypointArray message
+                waypoint_array_msg.waypoints.append(waypoint_msg)
+
+            # Publish the WaypointArray message
+            self.waypoints_publisher.publish(waypoint_array_msg)
+
+    def control_callback(self, msg):
+        """
+        This function will be called when a control message is received from the MPC Node.
+        It will forward the control commands to the carla_core.
+        """
+        # Extract steering and throttle from the message
+        steering_angle = msg.steering_angle
+        throttle = msg.throttle
+
+        # Send control commands to CARLA via carla_core
+        self.carla_core.apply_control(steering_angle, throttle)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    carla_node = CarlaNode()
+    rclpy.spin(carla_node)
+    carla_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()