From 436cb9968822cc8aaba93e32eb75c78675fd7d6b Mon Sep 17 00:00:00 2001 From: hasan3773 Date: Mon, 22 Jul 2024 03:35:07 +0000 Subject: [PATCH 1/7] fixing cuda & nvidia variables --- modules/docker-compose.simulation.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/docker-compose.simulation.yaml b/modules/docker-compose.simulation.yaml index 241f4567..2b5d0039 100644 --- a/modules/docker-compose.simulation.yaml +++ b/modules/docker-compose.simulation.yaml @@ -5,8 +5,8 @@ services: image: carlasim/carla:0.9.13 environment: - DISPLAY=1 - - CUDA_VISIBLE_DEVICES=0,1,2 - - NVIDIA_VISIBLE_DEVICES=0,1,2 + - CUDA_VISIBLE_DEVICES=0 + - NVIDIA_VISIBLE_DEVICES=0 runtime: nvidia restart: always command: /bin/bash -c "./CarlaUE4.sh -nosound -carla-server -RenderOffscreen -world-port=2000 -quality-level=Low" From de0ca87f64d68440439da0d5cd2693c04cd716c5 Mon Sep 17 00:00:00 2001 From: hasan3773 Date: Fri, 23 Aug 2024 18:56:05 +0000 Subject: [PATCH 2/7] Seting up Carla in slurm --- .../model_predictive_control.Dockerfile | 64 +--- .../dev_overrides/docker-compose.action.yaml | 3 +- modules/docker-compose.action.yaml | 1 - modules/docker-compose.simulation.yaml | 7 +- .../model_predictive_control/boxconstraint.py | 70 +++++ src/action/model_predictive_control/mpc.py | 283 ++++++++++++++++++ .../model_predictive_control/mpc_node.py | 98 ++++++ .../model_predictive_control/package.xml | 2 +- watod-config.sh | 6 +- 9 files changed, 476 insertions(+), 58 deletions(-) create mode 100644 src/action/model_predictive_control/boxconstraint.py create mode 100644 src/action/model_predictive_control/mpc.py create mode 100644 src/action/model_predictive_control/mpc_node.py diff --git a/docker/action/model_predictive_control/model_predictive_control.Dockerfile b/docker/action/model_predictive_control/model_predictive_control.Dockerfile index a10d3c59..ff1e8e83 100644 --- a/docker/action/model_predictive_control/model_predictive_control.Dockerfile +++ b/docker/action/model_predictive_control/model_predictive_control.Dockerfile @@ -1,55 +1,19 @@ -ARG BASE_IMAGE=ghcr.io/watonomous/wato_monorepo/base:humble-ubuntu22.04 +FROM python:3.8.16-slim-bullseye +ARG CARLA_VERSION=0.9.13 -################################ Source ################################ -FROM ${BASE_IMAGE} as source +# Install system dependencies and pip packages +RUN apt-get update && apt-get install -y \ + curl git wget unzip \ + && apt-get clean \ + && rm -rf /var/lib/apt/lists/* -WORKDIR ${AMENT_WS}/src +# Install CARLA and other Python packages +RUN pip3 install carla==${CARLA_VERSION} tensorflow-probability numpy -# Copy in source code -COPY src/action/model_predictive_control model_predictive_control -COPY src/wato_msgs/sample_msgs sample_msgs +# Install CasADi +RUN pip3 install casadi -# Scan for rosdeps -RUN apt-get -qq update && rosdep update && \ - rosdep install --from-paths . --ignore-src -r -s \ - | grep 'apt-get install' \ - | awk '{print $3}' \ - | sort > /tmp/colcon_install_list +WORKDIR /home/bolty/carla_notebooks +COPY src/action/model_predictive_control /home/bolty/carla_notebooks -################################# Dependencies ################################ -FROM ${BASE_IMAGE} as dependencies - -# Install Rosdep requirements -COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list -RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) - -# Copy in source code from source stage -WORKDIR ${AMENT_WS} -COPY --from=source ${AMENT_WS}/src src - -# Dependency Cleanup -WORKDIR / -RUN apt-get -qq autoremove -y && apt-get -qq autoclean && apt-get -qq clean && \ - rm -rf /root/* /root/.ros /tmp/* /var/lib/apt/lists/* /usr/share/doc/* - -################################ Build ################################ -FROM dependencies as build - -# Build ROS2 packages -WORKDIR ${AMENT_WS} -RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ - colcon build \ - --cmake-args -DCMAKE_BUILD_TYPE=Release - -# Entrypoint will run before any CMD on launch. Sources ~/opt//setup.bash and ~/ament_ws/install/setup.bash -COPY docker/wato_ros_entrypoint.sh ${AMENT_WS}/wato_ros_entrypoint.sh -ENTRYPOINT ["./wato_ros_entrypoint.sh"] - -################################ Prod ################################ -FROM build as deploy - -# Source Cleanup and Security Setup -RUN chown -R $USER:$USER ${AMENT_WS} -RUN rm -rf src/* - -USER ${USER} +WORKDIR /home/bolty/carla_notebooks diff --git a/modules/dev_overrides/docker-compose.action.yaml b/modules/dev_overrides/docker-compose.action.yaml index 1984e6db..6cfbc0fd 100644 --- a/modules/dev_overrides/docker-compose.action.yaml +++ b/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 \ No newline at end of file + - ${MONO_DIR}/src/action/model_predictive_control:/home/bolty/model_predictive_control \ No newline at end of file diff --git a/modules/docker-compose.action.yaml b/modules/docker-compose.action.yaml index ab73667e..47efc9f7 100644 --- a/modules/docker-compose.action.yaml +++ b/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" diff --git a/modules/docker-compose.simulation.yaml b/modules/docker-compose.simulation.yaml index 2b5d0039..14926d08 100644 --- a/modules/docker-compose.simulation.yaml +++ b/modules/docker-compose.simulation.yaml @@ -7,9 +7,14 @@ services: - DISPLAY=1 - CUDA_VISIBLE_DEVICES=0 - NVIDIA_VISIBLE_DEVICES=0 - runtime: nvidia 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: diff --git a/src/action/model_predictive_control/boxconstraint.py b/src/action/model_predictive_control/boxconstraint.py new file mode 100644 index 00000000..9cfd33bb --- /dev/null +++ b/src/action/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) \ No newline at end of file diff --git a/src/action/model_predictive_control/mpc.py b/src/action/model_predictive_control/mpc.py new file mode 100644 index 00000000..837a9ac2 --- /dev/null +++ b/src/action/model_predictive_control/mpc.py @@ -0,0 +1,283 @@ +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] +control_dim = 2 # Dimension of the control input [steering angle, acceleration] + +# Initialize Opti object +opti = ca.Opti() + +# Declare variables +X = opti.variable(state_dim, N + 1) # state trajectory variables over prediction horizon +U = opti.variable(control_dim, N) # control trajectory variables over prediction horizon +P = opti.parameter(state_dim) # initial state parameter +Q_base = ca.MX.eye(state_dim) # Base state penalty matrix (emphasizes position states) +weight_increase_factor = 1.00 # Increase factor for each step in the prediction horizon +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): + Q = Q_base * (weight_increase_factor ** k) # Increase weight for each step in the prediction horizon + + x_k = X[:, k] # Current state + u_k = U[:, k] # Current control input + x_next = X[:, k + 1] # Next state + + x_ref = ca.vertcat(W[:, k], + ca.MX.zeros(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) + + # Quadratic cost with reference state and control input + obj += ca.mtimes([ca.mtimes(dx.T, Q), dx]) + ca.mtimes( + [ca.mtimes(du.T, R), du]) # Minimize quadratic cost and deviation from reference state + +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): + steering_angle_rad = U[0, k] * max_steering_angle_rad # Convert normalized steering angle to radians + + 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_state = prev_sol_x[:, 1] # Predicted next state from the previous solution + actual_state = np.array([x0, y0, theta0, v0]) # Current actual state from CARLA + 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/mpc_node.py b/src/action/model_predictive_control/mpc_node.py new file mode 100644 index 00000000..f308e1bd --- /dev/null +++ b/src/action/model_predictive_control/mpc_node.py @@ -0,0 +1,98 @@ +# 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 sample_msgs.msg import Unfiltered, Filtered, FilteredArray +from transformer.transformer_core import TransformerCore + + +class Transformer(Node): + + def __init__(self): + super().__init__('python_transformer') + # Declare and get the parameters + self.declare_parameter('version', 1) + self.declare_parameter('compression_method', 0) + self.declare_parameter('buffer_capacity', 5) + + self.__buffer_capacity = self.get_parameter('buffer_capacity') \ + .get_parameter_value().integer_value + + # Initialize Transformer Core Logic for Deserialization + self.__transformer = TransformerCore() + + # Initialize ROS2 Constructs + self.publisher_ = self.create_publisher(FilteredArray, '/filtered_topic', 10) + self.subscription = self.create_subscription( + Unfiltered, '/unfiltered_topic', self.unfiltered_callback, 10) + + self.__filtered_array_packets = [] + + def unfiltered_callback(self, msg): + if not self.check_msg_validity(msg): + self.get_logger().info('INVALID MSG') + return + + # Init message object + filtered_msg = Filtered() + + # Populate message object + filtered_msg.pos_x, filtered_msg.pos_y, filtered_msg.pos_z = self.__transformer \ + .deserialize_data(msg.data) + filtered_msg.timestamp = msg.timestamp + filtered_msg.metadata.version = self.get_parameter('version') \ + .get_parameter_value().integer_value + filtered_msg.metadata.compression_method = self.get_parameter('compression_method') \ + .get_parameter_value().integer_value + + # We send off a list of Filtered Messages (a message made of messages!) + if self.populate_packet(filtered_msg): + return + + # If we reach the buffer capacity, publish the filtered packets + filtered_array_msg = FilteredArray() + filtered_array_msg.packets = self.__filtered_array_packets + + self.get_logger().info('Buffer Capacity Reached. PUBLISHING...') + self.publisher_.publish(filtered_array_msg) + + # clear packets for next round of messages + self.__filtered_array_packets.clear() + + def populate_packet(self, filtered_msg): + self.__filtered_array_packets.append(filtered_msg) + return len(self.__filtered_array_packets) <= self.__buffer_capacity + + def check_msg_validity(self, msg): + return msg.valid + + +def main(args=None): + rclpy.init(args=args) + + python_transformer = Transformer() + + rclpy.spin(python_transformer) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + python_transformer.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/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 = "" From fe39113a9bec0cf55ec2358fe1622ee1f2a572f3 Mon Sep 17 00:00:00 2001 From: hasan3773 Date: Wed, 2 Oct 2024 13:07:38 +0000 Subject: [PATCH 3/7] creating control file archetecture --- .../config/params.yaml | 0 .../launch/mpc_launch.py | 0 .../boxconstraint.py | 0 .../{ => model_predictive_control}/mpc.py | 0 .../model_predictive_control/mpc_node.py | 37 +++ .../sample_node.py} | 0 .../model_predictive_control/test/mpc_test.py | 1 + .../carla_sample_node/mpc_script.py | 283 ++++++++++++++++++ 8 files changed, 321 insertions(+) create mode 100644 src/action/model_predictive_control/config/params.yaml create mode 100644 src/action/model_predictive_control/launch/mpc_launch.py rename src/action/model_predictive_control/{ => model_predictive_control}/boxconstraint.py (100%) rename src/action/model_predictive_control/{ => model_predictive_control}/mpc.py (100%) create mode 100644 src/action/model_predictive_control/model_predictive_control/mpc_node.py rename src/action/model_predictive_control/{mpc_node.py => model_predictive_control/sample_node.py} (100%) create mode 100644 src/action/model_predictive_control/test/mpc_test.py create mode 100644 src/simulation/carla_sample_node/carla_sample_node/mpc_script.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/boxconstraint.py b/src/action/model_predictive_control/model_predictive_control/boxconstraint.py similarity index 100% rename from src/action/model_predictive_control/boxconstraint.py rename to src/action/model_predictive_control/model_predictive_control/boxconstraint.py diff --git a/src/action/model_predictive_control/mpc.py b/src/action/model_predictive_control/model_predictive_control/mpc.py similarity index 100% rename from src/action/model_predictive_control/mpc.py rename to src/action/model_predictive_control/model_predictive_control/mpc.py 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..2f046c45 --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/mpc_node.py @@ -0,0 +1,37 @@ +# 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 + +import carla +import casadi as ca +import numpy as np +import datetime +import os +import shutil + +from action.model_predictive_control.model_predictive_control.boxconstraint import BoxConstraint +from sample_msgs.msg import Unfilterered, Filitered, FilteredArray +from transformer.transformer_core import TransformerCore + +SIM_DURATION = 500 # Simulation duration in time steps + +# somehow send a message to sim container to init carla + +class carla_com(Node): + def __init__(self): + super().__init__('python_transformer') + + diff --git a/src/action/model_predictive_control/mpc_node.py b/src/action/model_predictive_control/model_predictive_control/sample_node.py similarity index 100% rename from src/action/model_predictive_control/mpc_node.py rename to src/action/model_predictive_control/model_predictive_control/sample_node.py 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..7f36d1fc --- /dev/null +++ b/src/action/model_predictive_control/test/mpc_test.py @@ -0,0 +1 @@ +# create some trajectory and init mpc and carla nodes \ No newline at end of file diff --git a/src/simulation/carla_sample_node/carla_sample_node/mpc_script.py b/src/simulation/carla_sample_node/carla_sample_node/mpc_script.py new file mode 100644 index 00000000..4aaafd8e --- /dev/null +++ b/src/simulation/carla_sample_node/carla_sample_node/mpc_script.py @@ -0,0 +1,283 @@ +import carla +import casadi as ca +import numpy as np +import datetime +import os +import shutil + +from action.model_predictive_control.model_predictive_control.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] +control_dim = 2 # Dimension of the control input [steering angle, acceleration] + +# Initialize Opti object +opti = ca.Opti() + +# Declare variables +X = opti.variable(state_dim, N + 1) # state trajectory variables over prediction horizon +U = opti.variable(control_dim, N) # control trajectory variables over prediction horizon +P = opti.parameter(state_dim) # initial state parameter +Q_base = ca.MX.eye(state_dim) # Base state penalty matrix (emphasizes position states) +weight_increase_factor = 1.00 # Increase factor for each step in the prediction horizon +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): + Q = Q_base * (weight_increase_factor ** k) # Increase weight for each step in the prediction horizon + + x_k = X[:, k] # Current state + u_k = U[:, k] # Current control input + x_next = X[:, k + 1] # Next state + + x_ref = ca.vertcat(W[:, k], + ca.MX.zeros(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) + + # Quadratic cost with reference state and control input + obj += ca.mtimes([ca.mtimes(dx.T, Q), dx]) + ca.mtimes( + [ca.mtimes(du.T, R), du]) # Minimize quadratic cost and deviation from reference state + +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): + steering_angle_rad = U[0, k] * max_steering_angle_rad # Convert normalized steering angle to radians + + 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_state = prev_sol_x[:, 1] # Predicted next state from the previous solution + actual_state = np.array([x0, y0, theta0, v0]) # Current actual state from CARLA + 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 From 3929540f30203dbbeb487084bbb595e65964ecb1 Mon Sep 17 00:00:00 2001 From: Hasan3773 Date: Wed, 2 Oct 2024 13:15:47 +0000 Subject: [PATCH 4/7] checking something --- .../model_predictive_control.Dockerfile | 64 +++++++++++++++---- 1 file changed, 50 insertions(+), 14 deletions(-) diff --git a/docker/action/model_predictive_control/model_predictive_control.Dockerfile b/docker/action/model_predictive_control/model_predictive_control.Dockerfile index ff1e8e83..51d0d0ad 100644 --- a/docker/action/model_predictive_control/model_predictive_control.Dockerfile +++ b/docker/action/model_predictive_control/model_predictive_control.Dockerfile @@ -1,19 +1,55 @@ -FROM python:3.8.16-slim-bullseye -ARG CARLA_VERSION=0.9.13 +ARG BASE_IMAGE=ghcr.io/watonomous/wato_monorepo/base:humble-ubuntu22.04 -# Install system dependencies and pip packages -RUN apt-get update && apt-get install -y \ - curl git wget unzip \ - && apt-get clean \ - && rm -rf /var/lib/apt/lists/* +################################ Source ################################ +FROM ${BASE_IMAGE} as source -# Install CARLA and other Python packages -RUN pip3 install carla==${CARLA_VERSION} tensorflow-probability numpy +WORKDIR ${AMENT_WS}/src -# Install CasADi -RUN pip3 install casadi +# Copy in source code +COPY src/action/local_planning local_planning +COPY src/wato_msgs/sample_msgs sample_msgs -WORKDIR /home/bolty/carla_notebooks -COPY src/action/model_predictive_control /home/bolty/carla_notebooks +# Scan for rosdeps +RUN apt-get -qq update && rosdep update && \ + rosdep install --from-paths . --ignore-src -r -s \ + | grep 'apt-get install' \ + | awk '{print $3}' \ + | sort > /tmp/colcon_install_list -WORKDIR /home/bolty/carla_notebooks +################################# Dependencies ################################ +FROM ${BASE_IMAGE} as dependencies + +# Install Rosdep requirements +COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list +RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) + +# Copy in source code from source stage +WORKDIR ${AMENT_WS} +COPY --from=source ${AMENT_WS}/src src + +# Dependency Cleanup +WORKDIR / +RUN apt-get -qq autoremove -y && apt-get -qq autoclean && apt-get -qq clean && \ + rm -rf /root/* /root/.ros /tmp/* /var/lib/apt/lists/* /usr/share/doc/* + +################################ Build ################################ +FROM dependencies as build + +# Build ROS2 packages +WORKDIR ${AMENT_WS} +RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ + colcon build \ + --cmake-args -DCMAKE_BUILD_TYPE=Release + +# Entrypoint will run before any CMD on launch. Sources ~/opt//setup.bash and ~/ament_ws/install/setup.bash +COPY docker/wato_ros_entrypoint.sh ${AMENT_WS}/wato_ros_entrypoint.sh +ENTRYPOINT ["./wato_ros_entrypoint.sh"] + +################################ Prod ################################ +FROM build as deploy + +# Source Cleanup and Security Setup +RUN chown -R $USER:$USER ${AMENT_WS} +RUN rm -rf src/* + +USER ${USER} From 06fede2fbde60221db4e3a83f36502795b624795 Mon Sep 17 00:00:00 2001 From: Hasan3773 Date: Wed, 16 Oct 2024 23:35:40 +0000 Subject: [PATCH 5/7] adding mpc script dependencies --- .../carla_notebooks/boxconstraint.py | 70 +++++++++++++++++++ .../mpc_script.py | 2 +- src/simulation/carla_notebooks/mpc_test.py | 14 ++++ 3 files changed, 85 insertions(+), 1 deletion(-) create mode 100644 src/simulation/carla_notebooks/boxconstraint.py rename src/simulation/{carla_sample_node/carla_sample_node => carla_notebooks}/mpc_script.py (99%) create mode 100644 src/simulation/carla_notebooks/mpc_test.py diff --git a/src/simulation/carla_notebooks/boxconstraint.py b/src/simulation/carla_notebooks/boxconstraint.py new file mode 100644 index 00000000..9cfd33bb --- /dev/null +++ b/src/simulation/carla_notebooks/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) \ No newline at end of file diff --git a/src/simulation/carla_sample_node/carla_sample_node/mpc_script.py b/src/simulation/carla_notebooks/mpc_script.py similarity index 99% rename from src/simulation/carla_sample_node/carla_sample_node/mpc_script.py rename to src/simulation/carla_notebooks/mpc_script.py index 4aaafd8e..837a9ac2 100644 --- a/src/simulation/carla_sample_node/carla_sample_node/mpc_script.py +++ b/src/simulation/carla_notebooks/mpc_script.py @@ -5,7 +5,7 @@ import os import shutil -from action.model_predictive_control.model_predictive_control.boxconstraint import BoxConstraint +from boxconstraint import BoxConstraint SIM_DURATION = 500 # Simulation duration in time steps diff --git a/src/simulation/carla_notebooks/mpc_test.py b/src/simulation/carla_notebooks/mpc_test.py new file mode 100644 index 00000000..10d6a7b0 --- /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() + From 77fa537ac0444c09e742d38ec596b17b1229f71d Mon Sep 17 00:00:00 2001 From: Hasan3773 Date: Sat, 2 Nov 2024 20:58:12 +0000 Subject: [PATCH 6/7] Setting up carla sim for mpc --- modules/docker-compose.simulation.yaml | 2 +- .../model_predictive_control/carla_core.py | 165 +++++++++++++ .../model_predictive_control/carla_node.py | 80 ++++++ .../model_predictive_control/mpc_core.py | 227 ++++++++++++++++++ .../model_predictive_control/mpc_node.py | 52 ++-- .../model_predictive_control/sample_node.py | 98 -------- .../carla_sim/carla_sim/carla_core.py | 165 +++++++++++++ .../carla_sim/carla_sim/carla_node.py | 80 ++++++ .../carla_sim/launch/carla_sim.launch.py | 25 ++ src/simulation/carla_sim/package.xml | 20 ++ src/simulation/carla_sim/setup.cfg | 4 + src/simulation/carla_sim/setup.py | 29 +++ src/simulation/carla_sim/temp.txt | 1 - 13 files changed, 833 insertions(+), 115 deletions(-) create mode 100644 src/action/model_predictive_control/model_predictive_control/carla_core.py create mode 100644 src/action/model_predictive_control/model_predictive_control/carla_node.py create mode 100644 src/action/model_predictive_control/model_predictive_control/mpc_core.py delete mode 100644 src/action/model_predictive_control/model_predictive_control/sample_node.py create mode 100644 src/simulation/carla_sim/carla_sim/carla_core.py create mode 100644 src/simulation/carla_sim/carla_sim/carla_node.py create mode 100644 src/simulation/carla_sim/launch/carla_sim.launch.py create mode 100644 src/simulation/carla_sim/package.xml create mode 100644 src/simulation/carla_sim/setup.cfg create mode 100644 src/simulation/carla_sim/setup.py delete mode 100644 src/simulation/carla_sim/temp.txt diff --git a/modules/docker-compose.simulation.yaml b/modules/docker-compose.simulation.yaml index 14926d08..20b7fc3b 100644 --- a/modules/docker-compose.simulation.yaml +++ b/modules/docker-compose.simulation.yaml @@ -75,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 diff --git a/src/action/model_predictive_control/model_predictive_control/carla_core.py b/src/action/model_predictive_control/model_predictive_control/carla_core.py new file mode 100644 index 00000000..afb6fe20 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/action/model_predictive_control/model_predictive_control/carla_node.py b/src/action/model_predictive_control/model_predictive_control/carla_node.py new file mode 100644 index 00000000..6db285c5 --- /dev/null +++ b/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() \ No newline at end of file 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..0508ceee --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/mpc_core.py @@ -0,0 +1,227 @@ +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 + + +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] + 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 + 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) + 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 + + 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() + + def update_waypoints(self, waypoints_msg): + raw_waypoints[] = waypoints_msg.waypoints + + # Process and convert each waypoint to CasADi format + for wp in raw_waypoints: + if hasattr(wp, 'x') and hasattr(wp, 'y'): # Ensure waypoint has x and y + # Convert to CasADi format and add to the waypoints list + self.waypoints.append(generate_waypoint(wp.x, wp.y)) + # else: + # # 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 + return ca.vertcat(x, y) + + def setup_mpc(self): + """ + Setup the MPC problem with CasADi + """ + + for k in range(self.N): + Q = self.Q_base * (self.weight_increase_factor ** k) # Increase weight for each step in the prediction horizon + + 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 + + dx = x_k - x_ref # Deviation of state from reference state + du = u_k # Control input deviation (assuming a desired control input of zero) + + # Quadratic cost with reference state and control input + self.obj += ca.mtimes([ca.mtimes(dx.T, Q), dx]) + ca.mtimes( + [ca.mtimes(du.T, self.R), du]) # Minimize quadratic cost and deviation from reference state + + 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): + steering_angle_rad = self.U[0, k] * self.max_steering_angle_rad # Convert normalized steering angle to radians + + 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): + + self.opti.subject_to(self.X[:, 0] == self.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(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, vehicle_state): + """ + 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(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 + i = vehicle_state.interation + + print("Current x: ", x0) + print("Current y: ", y0) + print("Current theta: ", theta0) + print("Current velocity: ", v0) + + if 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 + + 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_state = self.prev_sol_x[:, 1] # Predicted next state from the previous solution + 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 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 index 2f046c45..26983697 100644 --- a/src/action/model_predictive_control/model_predictive_control/mpc_node.py +++ b/src/action/model_predictive_control/model_predictive_control/mpc_node.py @@ -15,23 +15,45 @@ import rclpy from rclpy.node import Node -import carla -import casadi as ca -import numpy as np -import datetime -import os -import shutil +from action_msgs import ControlCommands, VehicleState, WaypointArray +from model_predictive_control.mpc_core import MPCCore -from action.model_predictive_control.model_predictive_control.boxconstraint import BoxConstraint -from sample_msgs.msg import Unfilterered, Filitered, FilteredArray -from transformer.transformer_core import TransformerCore -SIM_DURATION = 500 # Simulation duration in time steps +class MPCNode(Node): + def __init__(self): + super().__init__('MPCNode') -# somehow send a message to sim container to init carla + self.mpc_core = MPCCore() + + # Subscribe to vehicle state + self.state_subscription = self.create_subscription( + VehicleState, '/carla/vehicle_state', self.vehicle_state_callback, 10) + + # 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): + steering_angle, throttle = self.mpc_core.compute_control(msg) + + control_msg = control_msgs() + control_msg.steering_angle = steering_angle + control_msg.throttle = throttle + self.control_publisher.publish(control_msg) + + def waypoints_callback(self, msg): + self.mpc_core.update_waypoints(msg) + + +def main(args=None): + rclpy.init(args=args) + mpc_node = MPCNode() + rclpy.spin(mpc_node) + mpc_node.destroy_node() + rclpy.shutdown() -class carla_com(Node): - def __init__(self): - super().__init__('python_transformer') - +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/action/model_predictive_control/model_predictive_control/sample_node.py b/src/action/model_predictive_control/model_predictive_control/sample_node.py deleted file mode 100644 index f308e1bd..00000000 --- a/src/action/model_predictive_control/model_predictive_control/sample_node.py +++ /dev/null @@ -1,98 +0,0 @@ -# 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 sample_msgs.msg import Unfiltered, Filtered, FilteredArray -from transformer.transformer_core import TransformerCore - - -class Transformer(Node): - - def __init__(self): - super().__init__('python_transformer') - # Declare and get the parameters - self.declare_parameter('version', 1) - self.declare_parameter('compression_method', 0) - self.declare_parameter('buffer_capacity', 5) - - self.__buffer_capacity = self.get_parameter('buffer_capacity') \ - .get_parameter_value().integer_value - - # Initialize Transformer Core Logic for Deserialization - self.__transformer = TransformerCore() - - # Initialize ROS2 Constructs - self.publisher_ = self.create_publisher(FilteredArray, '/filtered_topic', 10) - self.subscription = self.create_subscription( - Unfiltered, '/unfiltered_topic', self.unfiltered_callback, 10) - - self.__filtered_array_packets = [] - - def unfiltered_callback(self, msg): - if not self.check_msg_validity(msg): - self.get_logger().info('INVALID MSG') - return - - # Init message object - filtered_msg = Filtered() - - # Populate message object - filtered_msg.pos_x, filtered_msg.pos_y, filtered_msg.pos_z = self.__transformer \ - .deserialize_data(msg.data) - filtered_msg.timestamp = msg.timestamp - filtered_msg.metadata.version = self.get_parameter('version') \ - .get_parameter_value().integer_value - filtered_msg.metadata.compression_method = self.get_parameter('compression_method') \ - .get_parameter_value().integer_value - - # We send off a list of Filtered Messages (a message made of messages!) - if self.populate_packet(filtered_msg): - return - - # If we reach the buffer capacity, publish the filtered packets - filtered_array_msg = FilteredArray() - filtered_array_msg.packets = self.__filtered_array_packets - - self.get_logger().info('Buffer Capacity Reached. PUBLISHING...') - self.publisher_.publish(filtered_array_msg) - - # clear packets for next round of messages - self.__filtered_array_packets.clear() - - def populate_packet(self, filtered_msg): - self.__filtered_array_packets.append(filtered_msg) - return len(self.__filtered_array_packets) <= self.__buffer_capacity - - def check_msg_validity(self, msg): - return msg.valid - - -def main(args=None): - rclpy.init(args=args) - - python_transformer = Transformer() - - rclpy.spin(python_transformer) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - python_transformer.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/simulation/carla_sim/carla_sim/carla_core.py b/src/simulation/carla_sim/carla_sim/carla_core.py new file mode 100644 index 00000000..afb6fe20 --- /dev/null +++ b/src/simulation/carla_sim/carla_sim/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 \ No newline at end of file diff --git a/src/simulation/carla_sim/carla_sim/carla_node.py b/src/simulation/carla_sim/carla_sim/carla_node.py new file mode 100644 index 00000000..6db285c5 --- /dev/null +++ b/src/simulation/carla_sim/carla_sim/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() \ No newline at end of file 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 From 9e87ec4c5b568e5317ddc7852225c4243348093c Mon Sep 17 00:00:00 2001 From: Hasan3773 Date: Fri, 8 Nov 2024 13:52:22 +0000 Subject: [PATCH 7/7] pr comments --- .../model_predictive_control.Dockerfile | 2 +- modules/dev_overrides/docker-compose.action.yaml | 9 +++++---- modules/docker-compose.action.yaml | 1 + watod-config.sh | 6 +++--- 4 files changed, 10 insertions(+), 8 deletions(-) diff --git a/docker/action/model_predictive_control/model_predictive_control.Dockerfile b/docker/action/model_predictive_control/model_predictive_control.Dockerfile index 51d0d0ad..a10d3c59 100644 --- a/docker/action/model_predictive_control/model_predictive_control.Dockerfile +++ b/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/local_planning local_planning +COPY src/action/model_predictive_control model_predictive_control COPY src/wato_msgs/sample_msgs sample_msgs # Scan for rosdeps diff --git a/modules/dev_overrides/docker-compose.action.yaml b/modules/dev_overrides/docker-compose.action.yaml index 6cfbc0fd..478fdd1f 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,13 +33,14 @@ 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/bolty/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/docker-compose.action.yaml b/modules/docker-compose.action.yaml index 47efc9f7..12ea3c2a 100644 --- a/modules/docker-compose.action.yaml +++ b/modules/docker-compose.action.yaml @@ -41,5 +41,6 @@ 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" diff --git a/watod-config.sh b/watod-config.sh index eafc5ed9..3255dd40 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -15,7 +15,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -ACTIVE_MODULES="simulation action" +# ACTIVE_MODULES="" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. @@ -23,11 +23,11 @@ ACTIVE_MODULES="simulation action" ## - deploy (default) : runs production-grade containers (non-editable) ## - develop : runs developer containers (editable) -MODE_OF_OPERATION="develop" +# MODE_OF_OPERATION="" ############################## ADVANCED CONFIGURATIONS ############################## ## Name to append to docker containers. DEFAULT = "" -COMPOSE_PROJECT_NAME="hasan3773" +# COMPOSE_PROJECT_NAME="" ## Tag to use. Images are formatted as : with forward slashes replaced with dashes. ## DEFAULT = ""