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

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

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

         """
         self.waypoints = []  # Clear old waypoints
 
-        if len(self.raw_waypoints) % 2 != 0: # Check if raw_waypoints is even
+        if len(self.raw_waypoints) % 2 != 0:  # Check if raw_waypoints is even
             print("Error: raw_waypoints length is not even. Ignoring the last unpaired value.")
             self.raw_waypoints = self.raw_waypoints[:-1]
 
         for i in range(0, len(self.raw_waypoints), 2):
             x, y = self.raw_waypoints[i], self.raw_waypoints[i + 1]
             waypoint = self.generate_waypoint(x, y)
-            self.waypoints.append(waypoint)  
-
-    def generate_waypoint(x, y): # Convert to CasADi format and add to the waypoints list
+            self.waypoints.append(waypoint)
+
+    def generate_waypoint(x, y):  # Convert to CasADi format and add to the waypoints list
         return ca.vertcat(x, y)
 
     def setup_mpc(self):

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

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

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

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

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

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

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

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

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

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

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

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

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

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

 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import rclpy 
+import rclpy
 from rclpy.node import Node
 
 from wato_msgs/simulation/path_planning_msgs import CarlaEgoVehicleControl, CarlaEgoVehicleStatus

         # Subscribe to vehicle state (only retrieving velocity)
         self.state_subscription = self.create_subscription(
             CarlaEgoVehicleStatus, '/carla/ego/vehicle_status', self.vehicle_state_callback, 10)
-        
+
         # Subscribe to get vehicle position/orientation (x, y, w)
         self.state_odom_subscription = self.create_subscription(
             Odometry, 'T/carla/ego/odometry', self.state_odom_callback, 10)
-            
-        self.control_publisher = self.create_publisher(CarlaEgoVehicleControl, '/carla/ego/vehicle_control_cmd', 10)
+
+        self.control_publisher = self.create_publisher(
+            CarlaEgoVehicleControl, '/carla/ego/vehicle_control_cmd', 10)
 
         self.goal_publisher = self.create_publisher(Pose, '/carla/ego/goal', 10)
 
         # Subscribe to waypoints from CARLA
         self.waypoints_subscription = self.create_subscription(
             Path, '/carla/ego/waypoints', self.waypoints_callback, 10)
-    
+
         self.goal_point_x = 10
         self.goal_point_y = 10
         publish_goal(self.goal_point_x, self.goal_point_y)
 
-
     def vehicle_state_callback(self, msg):
-        mpc_core.v0 =  msg.velocity
+        mpc_core.v0 = msg.velocity
 
         # Extract theta/yaw/orientation of the car in the x-y plane from quaternion
         quaternion = [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]
         _, _, mpc_core.theta0 = euler_from_quaternion(quaternion)
-
 
     def waypoints_callback(self, msg):
         for pose_stamped in msg.poses:

 
         goal_msg.position.x = x
         goal_msg.position.y = y
-        goal_msg.position.z = 0.0 
+        goal_msg.position.z = 0.0
         goal_msg.orientation.x = 0.0
         goal_msg.orientation.y = 0.0
         goal_msg.orientation.z = 0.0
-        goal_msg.orientation.w = 1.0  
+        goal_msg.orientation.w = 1.0
 
         self.goal_publisher.publish(goal_msg)
 
     def start_main_loop(self):
-        for i in range(self.mpc_core.SIM_DURATION - self.mpc_core.N):  # Subtract N since we need to be able to predict N steps into the future
+        # Subtract N since we need to be able to predict N steps into the future
+        for i in range(self.mpc_core.SIM_DURATION - self.mpc_core.N):
             steering_angle, throttle = self.mpc_core.compute_control(i)
-            
+
             control_msg = CarlaEgoVehicleControl()
             control_msg.steer = steering_angle
             control_msg.throttle = throttle

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

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

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

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