From 88dff83e6ddd8b72ab6e83f1ae78b8773534a973 Mon Sep 17 00:00:00 2001
From: Tanish Bhatt <Tanish.Bhatt@fsae.co.nz>
Date: Fri, 30 Aug 2024 16:32:30 +1200
Subject: [PATCH 1/2] tested simulator nodes with planning - seems to work

---
 .../simulator/simulator/get_cones.py          |  4 +-
 src/moa/moa_bringup/launch/fs_sim_launch.py   | 65 +++++++++++++++++
 .../{sim_launch.py => unity_sim_launch.py}    |  0
 .../path_planning/trajectory_generation.py    | 69 ++++++++++---------
 .../trajectory_optimisation_CS.py             | 55 ++++++++-------
 .../visualizer.py                             | 42 +++++------
 6 files changed, 155 insertions(+), 80 deletions(-)
 create mode 100644 src/moa/moa_bringup/launch/fs_sim_launch.py
 rename src/moa/moa_bringup/launch/{sim_launch.py => unity_sim_launch.py} (100%)

diff --git a/src/driverless_sim/simulator/simulator/get_cones.py b/src/driverless_sim/simulator/simulator/get_cones.py
index e288a6d..df0aa07 100644
--- a/src/driverless_sim/simulator/simulator/get_cones.py
+++ b/src/driverless_sim/simulator/simulator/get_cones.py
@@ -21,7 +21,7 @@ class get_cones(Node):
     def __init__(self):
         super().__init__("get_cones")
 
-        self.plot = True
+        self.plot = False
         # connect to the simulator 
         self.client = fsds.FSDSClient(ip=os.environ['WSL_HOST_IP'])
         # Check network connection, exit if not connected
@@ -31,7 +31,7 @@ def __init__(self):
         # create publisher
         self.sim_cone_pub = self.create_publisher(ConeMap, "cone_map", 10)
 
-        self.get_cones_from_simulator()
+        self.create_timer(1.0, self.get_cones_from_simulator)
 
 
     def get_cones_from_simulator(self):
diff --git a/src/moa/moa_bringup/launch/fs_sim_launch.py b/src/moa/moa_bringup/launch/fs_sim_launch.py
new file mode 100644
index 0000000..b8fdcbe
--- /dev/null
+++ b/src/moa/moa_bringup/launch/fs_sim_launch.py
@@ -0,0 +1,65 @@
+import launch
+import launch_ros.actions
+
+def generate_launch_description():
+    return launch.LaunchDescription([
+        # get cones
+        launch_ros.actions.Node(
+            package='simulator',
+            executable='get_cones',
+            name='get_cones',
+        ),
+
+        # car position
+        launch_ros.actions.Node(
+            package='simulator',
+            executable='get_car_position',
+            name='get_car_position',
+        ),
+
+        # path generation
+        launch_ros.actions.Node(
+            package='path_planning',
+            executable='trajectory_generation',
+            name='trajectory_generation',
+            parameters=[{'debug': True, 
+                        'timer': 1.0}],
+        ),
+
+        # path optimization
+        launch_ros.actions.Node(
+            package='path_planning',
+            executable='trajectory_optimisation',
+            name='trajectory_optimisation',
+            parameters=[{'delete': False,
+                        'interpolate': False}],
+        ),
+
+        # controller
+        launch_ros.actions.Node(
+            package='stanley_controller',
+            executable='controller',
+            name='controller',
+        ),
+
+        # steer torque
+        # launch_ros.actions.Node(
+        #     package='steer_torque_from_ackermann',
+        #     executable='steer_torque_from_ackermann',
+        #     name='steer_torque_from_ackermann',
+        # ),
+
+        # path viz
+        launch_ros.actions.Node(
+            package='path_planning_visualization',
+            executable='visualize',
+            name='path_viz',
+        ),
+
+        # track viz
+        launch_ros.actions.Node(
+            package='cone_map_foxglove_visualizer',
+            executable='visualizer',
+            name='track_viz',
+        ),
+    ])
diff --git a/src/moa/moa_bringup/launch/sim_launch.py b/src/moa/moa_bringup/launch/unity_sim_launch.py
similarity index 100%
rename from src/moa/moa_bringup/launch/sim_launch.py
rename to src/moa/moa_bringup/launch/unity_sim_launch.py
diff --git a/src/planning/path_planning/path_planning/trajectory_generation.py b/src/planning/path_planning/path_planning/trajectory_generation.py
index 9b9dfcf..55c1e71 100644
--- a/src/planning/path_planning/path_planning/trajectory_generation.py
+++ b/src/planning/path_planning/path_planning/trajectory_generation.py
@@ -49,7 +49,7 @@ def set_current_speed_cb(self, msg:AckermannDrive) -> None: self._current_speed
     def set_cone_map_cb(self, msg:ConeMap) -> None: self._cone_map = msg
 
     def set_car_pose_cb(self, pose_msg: Pose) -> None: 
-        self._car_position = pose_msg
+        self._car_pose = pose_msg
 
 
     # BEST TRAJECTORY PUBLISHER
@@ -60,11 +60,12 @@ def generate_trajectories(self):
 
         self.get_logger().info(f"{self._timer} seconds up - generating trajectories")
         self.get_logger().info(f"current speed: {hasattr(self,'_current_speed')}"\
-                               f" | cone map: {hasattr(self,'_cone_map')}")
+                               f" | cone map: {hasattr(self,'_cone_map')}"\
+                               f" | car position: {hasattr(self,'_car_pose')}")
 
-        if hasattr(self,"_current_speed") and hasattr(self,"_cone_map"):
+        if hasattr(self,"_current_speed") and hasattr(self,"_car_pose"):
             # generate trajectories
-            paths, states = self.my_trajectory_generator(car_pose=self._car_position, radius=2, npoints=400)
+            paths, states = self.my_trajectory_generator(car_pose=self._car_pose, radius=2, npoints=400)
 
             # publish states and trajectories
             state_list = []
@@ -96,8 +97,9 @@ def my_trajectory_generator(self, car_pose, radius, npoints):
         """generate straight trajectory based on given radius from origin (0,0)"""
         trajectories = []
         # 1. initial point
-        first_point = car_pose.position
-        self._car_pose, rotation_matrix = self.get_transformation_matrix(car_pose)
+        first_cone = car_pose.position
+        car_position_orientation = self.get_position_of_cart(car_pose)
+        car_position, rotation_matrix = self.get_transformation_matrix(car_position_orientation)
         # cor = [first_cone.x, first_cone.y]
         cor = [0,0]
         # 2. radius
@@ -107,60 +109,61 @@ def my_trajectory_generator(self, car_pose, radius, npoints):
         x = np.linspace(-r, r, n, endpoint=False)[1:]
         n -= 1
         y = np.zeros(n)
-        self.angs = np.zeros(n)
+        angs = np.zeros(n)
 
         for i, val in enumerate(x):
-            self.tmp_path = PoseArray()
+            tmp_path = PoseArray()
             # append starting point
-            self.append_first_point(first_point)
+            tmp_pose = Pose()
+            tmp_pose.position.x = first_cone.x
+            tmp_pose.position.y = first_cone.y
+            tmp_path.poses.append(tmp_pose)
 
             # 4. find y using equation of circle
             y[i] = np.sqrt(np.round(r**2-(val-cor[0])**2, 3)) + cor[1]
-
             # 5. calculate steering angle for each trajectory
-            self.set_angle(x[i], y[i])
-            
+            dy = y[i] - cor[1]
+            dx = val - cor[0]
+            # inverse tan is in radians
+            angle = np.arctan(dx/dy)
+            angs[i] = (angle if dx < 0 else angle)
+            # print(f"passed angle = {angs[i]} for dx = {dx}")
             
             # transform coordinates from fixed to car 
-            post_trans_pose = self.apply_transformation(car_pose, rotation_matrix, val, y[i])
+            post_trans_pose = self.apply_transformation(car_position, rotation_matrix, val, y[i])
             # append new points to pose array
             x[i] = post_trans_pose[0][0]
             y[i] = post_trans_pose[1][0]
             tmp_pose2 = Pose()
             tmp_pose2.position.x = x[i]
             tmp_pose2.position.y = y[i]
-            self.tmp_path.poses.append(tmp_pose2)
+            tmp_path.poses.append(tmp_pose2)
 
-            if x[i] is np.nan or y[i] is np.nan or self.angs[i] is np.nan:
+            if x[i] is np.nan or y[i] is np.nan or angs[i] is np.nan:
                 print("NAN!")
 
             # plotting
             # plt.plot([cor[0],x[i]],[cor[1],y[i]],label=f'trajectories')
 
-            # append trajectory
-            trajectories.append(self.tmp_path)
+            # append trajecotry
+            trajectories.append(tmp_path)
 
         # plt.show()
 
         # list of points as tuples
         points = [(x[i],y[i]) for i in range(n)]
 
-        return trajectories, self.angs
-
-    def append_first_point(self, first_point):
-        tmp_pose = Pose()
-        tmp_pose.position.x = first_point.x
-        tmp_pose.position.y = first_point.y
-        self.tmp_path.poses.append(tmp_pose)
-
-    def set_angle(self):
-        
-        dy = y[i] - cor[1]
-        dx = val - cor[0]
-        # inverse tan is in radians
-        angle = np.arctan(dx/dy)
-        self.self.angs[i] = (-angle if dx < 0 else angle)
-        print(f"passed angle = {self.angs[i]} for dx = {dx}")
+        # self.get_logger().info(f"len of traj = {len(trajectories)}")
+
+        return trajectories, angs
+
+    def get_position_of_cart(self, car_pose):
+        # first cone
+        localization_data = car_pose
+        x = localization_data.position.x
+        y = localization_data.position.y
+        theta = localization_data.orientation.w
+        return x, y, theta
 
     def get_transformation_matrix(self, position_and_orientation):
         # theta = position_and_orientation[2] - np.pi/2
diff --git a/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py b/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py
index 7bff9c0..854c1f5 100644
--- a/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py
+++ b/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py
@@ -34,7 +34,7 @@ def __init__(self):
         )
 
         # attributes
-        self._once = True
+        self._once = False
         self._delete = self.get_parameter("delete").get_parameter_value().bool_value
         self._interpolate = self.get_parameter("interpolate").get_parameter_value().bool_value
 
@@ -42,6 +42,7 @@ def __init__(self):
         self.create_subscription(AllStates, "moa/states", self.set_states, 10)
         self.create_subscription(AllTrajectories, "moa/trajectories", self.set_generated_trajectories, 10)
         self.create_subscription(ConeMap, "cone_map", self.callback, 10)
+        self.create_subscription(Pose, "car_position", self.set_car_position, 10)
         # self.create_subscription(AckermannDrive, "moa/cur_vel", self.set_current_speed, 10)
 
         # publishers
@@ -58,14 +59,15 @@ def set_generated_trajectories(self, msg: AllTrajectories) -> None:
         self._trajectories_msg = msg
         self.get_logger().info(f"traj len = {len(msg.trajectories)}")
 
+    def set_car_position(self, msg:Pose) -> None:
+        self.car_position = [msg.position.x, msg.position.y]
+
     def callback(self, msg:ConeMap) -> None:
         '''retrives cone msg and find trajectory closest to centerline'''
 
+        self.get_logger().info(f"all states: {hasattr(self,'_state_msg')}")   
+        
         if hasattr(self, "_state_msg") and hasattr(self, "_trajectories_msg"):
-            self.get_logger().info(f"all states: {hasattr(self,'_state_msg')}"
-                            f" | current speed: {hasattr(self,'_current_speed')}" \
-                            f" | left boundaries: {hasattr(self,'_leftboundary')}"\
-                            f" | right boundaries: {hasattr(self,'_rightboundary')}")   
             
             # yup = 1
             # xup = -0.5
@@ -74,14 +76,18 @@ def callback(self, msg:ConeMap) -> None:
                 # msg.cones[0].pose.pose.position.x += xup
                 # msg.cones[0].pose.pose.position.y += yup
 
-            leftboundary, rightboundary, car_position = self.get_boundaries(msg.cones)    # get boundaries.
-            if len(leftboundary) < 2:
-                return
-            if len(rightboundary) < 2:
+            leftboundary, rightboundary = self.get_boundaries(msg)    # get boundaries.
+            car_position = self.car_position
+
+            try:
+                assert len(leftboundary) > 1 and len(rightboundary) > 1 # check if enough boundary points are given
+            except Exception as e:
+                self.get_logger().error(e)
                 return
-            position_orientation = self.get_position_of_cart(msg)
-            if self._interpolate:
-                leftboundary, rightboundary = self.get_relative_boundaries(leftboundary, rightboundary, car_position, position_orientation) # get local boundaries
+            
+            # position_orientation = self.get_position_of_cart(msg)
+            # if self._interpolate:
+            #     leftboundary, rightboundary = self.get_relative_boundaries(leftboundary, rightboundary, car_position, position_orientation) # get local boundaries
 
             # get center line
             centerline = self.get_center_line(leftboundary, rightboundary, interpolate=self._interpolate)
@@ -130,7 +136,7 @@ def callback(self, msg:ConeMap) -> None:
                     self.within_boundary_trajectories_publisher.publish(AllTrajectories(**args2)) # within bound pub
                     self.best_steering_angle_pub.publish(Float32(**args3)) # optimal steering angle pub
 
-                    # self.get_logger().info("OPTIMAL TRAJECTORY COMPUTED")
+                    self.get_logger().info("msgs published")
             else:
                 self.get_logger().info(f"Ids state:{self._state_msg.id} and trajectory:{self._trajectories_msg.id} do not match")
                 
@@ -138,20 +144,21 @@ def callback(self, msg:ConeMap) -> None:
 
 
     def get_boundaries(self, cones):
+        lb, rb = cones.left_cones, cones.right_cones
         leftboundary = []
         rightboundary = []
-        for i in range(len(cones)):
-            x = cones[i].pose.pose.position.x   # x point
-            y = cones[i].pose.pose.position.y   # y point
-            if i != 0:
-                if cones[i].colour == 0:    # 0 is blue which is left
-                    leftboundary.append([x,y])
-                elif cones[i].colour == 2:  # 2 is yellow which is right
-                    rightboundary.append([x,y])
-            else:
-                car_position = [x,y]    # first cone is car position
+
+        for i in range(len(lb)):
+            x = lb[i].x   # x point
+            y = lb[i].y   # y point
+            leftboundary.append([x,y])
+        
+        for i in range(len(rb)):
+            x = rb[i].x   # x point
+            y = rb[i].y   # y point
+            rightboundary.append([x,y])
         
-        return leftboundary, rightboundary, car_position
+        return leftboundary, rightboundary
     
     def get_relative_boundaries(self, leftboundary, rightboundary, car_position, position_orientation):
         leftboundary = np.array(leftboundary).copy()
diff --git a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/cone_map_foxglove_visualizer/visualizer.py b/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/cone_map_foxglove_visualizer/visualizer.py
index f6d1f14..78b3d4b 100644
--- a/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/cone_map_foxglove_visualizer/visualizer.py
+++ b/src/visualization/foxglove_visualizer/cone_map_foxglove_visualizer/cone_map_foxglove_visualizer/visualizer.py
@@ -22,10 +22,10 @@ def __init__(self):
     def cone_map_callback(self, msg):
         left_cones = msg.left_cones
         right_cones = msg.right_cones
-        all_cones = left_cones.copy()
-        all_cones.append(right_cones.copy())
-        list_of_cones = all_cones
-        self.publish_cones(list_of_cones)
+        # all_cones = left_cones.copy()
+        # all_cones.append(right_cones.copy())
+        # list_of_cones = all_cones
+        self.publish_cones(left_cones, right_cones)
 
     def localization_callback(self, msg):
         self.publish_transform(msg)
@@ -63,22 +63,24 @@ def publish_car(self, pose_of_car):
         id_assign = 0
         Markers.markers.append(self.delete_all_cone())
 
-        car_cone = Cone()
-        car_cone.pose.pose = pose_of_car
-        Markers.markers.append(self.convert_to_visualization(car_cone, is_localization, id_assign))
+        Markers.markers.append(self.convert_to_visualization(pose_of_car.position, is_localization, id_assign, colour=3))
         self.localization_marker_publisher.publish(Markers)
 
-    def publish_cones(self, rest_of_cones):
+    def publish_cones(self, left_cones, right_cones):
         Markers = MarkerArray()
         is_localization = False
         id_assign = 1
         Markers.markers.append(self.delete_all_cone())
-        for cone in rest_of_cones:
-            Markers.markers.append(self.convert_to_visualization(cone, is_localization, id_assign))
+        colour = 0
+        for cone in left_cones:
+            Markers.markers.append(self.convert_to_visualization(cone, is_localization, id_assign, colour))
+            id_assign += 1
+        for cone in right_cones:
+            Markers.markers.append(self.convert_to_visualization(cone, is_localization, id_assign, colour+2))
             id_assign += 1
         self.markers_publisher_.publish(Markers)
 
-    def convert_to_visualization(self, cone, is_localization, id_assign):
+    def convert_to_visualization(self, cone_point, is_localization, id_assign, colour):
         marker = Marker()
         marker.header.frame_id = "global_frame"  # Adjust the frame ID as needed
         marker.header.stamp = self.get_clock().now().to_msg()
@@ -88,16 +90,14 @@ def convert_to_visualization(self, cone, is_localization, id_assign):
         marker.type = Marker.CUBE
         marker.action = Marker.ADD
 
-        marker.pose.position.x = cone.pose.pose.position.x
-        marker.pose.position.y = cone.pose.pose.position.y
-        marker.pose.position.z = cone.pose.pose.position.z
+        marker.pose.position = cone_point
 
         if is_localization:
             # Need to do later
-            marker.pose.orientation.x = self.convert_rotation_to_quaternion(cone.pose.pose.orientation.w)[0]
-            marker.pose.orientation.y = self.convert_rotation_to_quaternion(cone.pose.pose.orientation.w)[1]
-            marker.pose.orientation.z = self.convert_rotation_to_quaternion(cone.pose.pose.orientation.w)[2]
-            marker.pose.orientation.w = self.convert_rotation_to_quaternion(cone.pose.pose.orientation.w)[3]
+            # marker.pose.orientation.x = self.convert_rotation_to_quaternion(cone.orientation.w)[0]
+            # marker.pose.orientation.y = self.convert_rotation_to_quaternion(cone.orientation.w)[1]
+            # marker.pose.orientation.z = self.convert_rotation_to_quaternion(cone.orientation.w)[2]
+            # marker.pose.orientation.w = self.convert_rotation_to_quaternion(cone.orientation.w)[3]
 
             marker.scale = Vector3(x=1.0, y=2.0, z=1.0)  # Scale of the cone (x, y, z)
 
@@ -114,19 +114,19 @@ def convert_to_visualization(self, cone, is_localization, id_assign):
 
             marker.scale = Vector3(x=0.1, y=0.1, z=0.1)  # Scale of the cone (x, y, z)
 
-            if cone.colour == 0:
+            if colour == 0:
                 # Blue
                 marker.color.r = 0.0
                 marker.color.g = 0.0
                 marker.color.b = 1.0
                 marker.color.a = 1.0  # Alpha (opacity)
-            elif cone.colour == 1:
+            elif colour == 1:
                 # Orange
                 marker.color.r = 1.0
                 marker.color.g = 0.5
                 marker.color.b = 0.0
                 marker.color.a = 1.0  # Alpha (opacity)
-            elif cone.colour == 2:
+            elif colour == 2:
                 # Yellow
                 marker.color.r = 1.0
                 marker.color.g = 1.0

From 1f0e8e5f0168644c590cd5fa9da0e0a702f1a5cd Mon Sep 17 00:00:00 2001
From: Tanish Bhatt <Tanish.Bhatt@fsae.co.nz>
Date: Fri, 30 Aug 2024 18:14:34 +1200
Subject: [PATCH 2/2] sim controller node added planning doesn't work in the
 sim atm

---
 src/driverless_sim/simulator/setup.py         |  1 +
 .../simulator/simulator/get_car_position.py   |  7 +-
 .../simulator/simulator/set_car_controls.py   | 66 +++++++++++++++++++
 src/moa/moa_bringup/launch/fs_sim_launch.py   |  7 ++
 .../path_planning/trajectory_generation.py    |  5 +-
 .../trajectory_optimisation_CS.py             |  2 +-
 6 files changed, 83 insertions(+), 5 deletions(-)
 create mode 100644 src/driverless_sim/simulator/simulator/set_car_controls.py

diff --git a/src/driverless_sim/simulator/setup.py b/src/driverless_sim/simulator/setup.py
index df13e12..2a4aa20 100644
--- a/src/driverless_sim/simulator/setup.py
+++ b/src/driverless_sim/simulator/setup.py
@@ -22,6 +22,7 @@
         'console_scripts': [
             'get_cones = simulator.get_cones:main',
             'get_car_position = simulator.get_car_position:main',
+            'set_car_controls = simulator.set_car_controls:main',
         ],
     },
 )
diff --git a/src/driverless_sim/simulator/simulator/get_car_position.py b/src/driverless_sim/simulator/simulator/get_car_position.py
index ae06868..135404a 100644
--- a/src/driverless_sim/simulator/simulator/get_car_position.py
+++ b/src/driverless_sim/simulator/simulator/get_car_position.py
@@ -2,7 +2,7 @@
 import rclpy
 from rclpy.node import Node
 
-from geometry_msgs.msg import Pose, Point
+from geometry_msgs.msg import Pose, Point, Quaternion
 
 import os
 import sys
@@ -33,10 +33,13 @@ def __init__(self):
 
     def get_car_position(self):
         """publishes current position of the simulator car"""
-        position = self.client.getCarState().kinematics_estimated.position  # car kinetmatics in ENU coordinates
+        state = self.client.getCarState()
+        position = state.kinematics_estimated.position  # car kinetmatics in ENU coordinates
+        orientation = state.kinematics_estimated.orientation
         
         msg = Pose()
         msg.position = Point(x=position.x_val, y=position.y_val, z=0.0) # add position information to Pose msg
+        msg.orientation = Quaternion(x=orientation.x_val,y=orientation.y_val,z=orientation.z_val,w=orientation.w_val,)
 
         self.sim_car_pub.publish(msg)   # publish msg
 
diff --git a/src/driverless_sim/simulator/simulator/set_car_controls.py b/src/driverless_sim/simulator/simulator/set_car_controls.py
new file mode 100644
index 0000000..6937dbf
--- /dev/null
+++ b/src/driverless_sim/simulator/simulator/set_car_controls.py
@@ -0,0 +1,66 @@
+#!/usr/bin/python3
+import rclpy
+from rclpy.node import Node
+
+from ackermann_msgs.msg import AckermannDrive
+
+import os
+import sys
+import numpy as np
+
+## adds the fsds package located the parent directory to the pyhthon path
+path = os.path.abspath(os.path.join('Formula-Student-Driverless-Simulator', 'python'))
+sys.path.insert(0, path)
+# sys.path.append('/home/Formula-Student-Driverless-Simulator/python')
+# print(sys.path)
+import fsds
+
+class set_car_controls(Node):
+    def __init__(self):
+        super().__init__("set_car_controls")
+
+        self.max_throttle = 21  # m/s
+        self.max_steering = 25  # degrees
+        # connect to the simulator 
+        self.client = fsds.FSDSClient(ip=os.environ['WSL_HOST_IP'])
+        # Check network connection, exit if not connected
+        self.client.confirmConnection()
+        # After enabling setting trajectory setpoints via the api. 
+        self.client.enableApiControl(True)
+        # create publisher
+        self.create_subscription(AckermannDrive, 'drive', self.callback, 10)
+
+
+    def callback(self, msg:AckermannDrive):
+        """Set the car controls in the simulator using the given ackerman msg"""
+        steering = msg.steering_angle
+        speed = msg.speed
+
+        steering, throttle = self.get_simulator_controls(steering, speed)
+
+        # class is part of types.py file in sim repo
+        self.client.setCarControls(fsds.CarControls(throttle=throttle,steering=steering,brake=0.0))
+
+    def get_simulator_controls(self, steering, speed):
+        throttle = speed/self.max_throttle # max throttle is 1 in sim where the max speed is ~20m/s
+        steering = steering/self.max_steering # max absolute steering is 1 in sim where the max steering angle is 25 degrees by default
+
+        return steering, throttle
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    simulator_car_controls = set_car_controls()
+
+    rclpy.spin(simulator_car_controls)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    simulator_car_controls.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
diff --git a/src/moa/moa_bringup/launch/fs_sim_launch.py b/src/moa/moa_bringup/launch/fs_sim_launch.py
index b8fdcbe..35aa7a5 100644
--- a/src/moa/moa_bringup/launch/fs_sim_launch.py
+++ b/src/moa/moa_bringup/launch/fs_sim_launch.py
@@ -42,6 +42,13 @@ def generate_launch_description():
             name='controller',
         ),
 
+        # simulator controller
+        launch_ros.actions.Node(
+            package='simulator',
+            executable='set_car_controls',
+            name='set_car_controls'
+        ),
+
         # steer torque
         # launch_ros.actions.Node(
         #     package='steer_torque_from_ackermann',
diff --git a/src/planning/path_planning/path_planning/trajectory_generation.py b/src/planning/path_planning/path_planning/trajectory_generation.py
index 55c1e71..5d8bda7 100644
--- a/src/planning/path_planning/path_planning/trajectory_generation.py
+++ b/src/planning/path_planning/path_planning/trajectory_generation.py
@@ -166,10 +166,11 @@ def get_position_of_cart(self, car_pose):
         return x, y, theta
 
     def get_transformation_matrix(self, position_and_orientation):
-        # theta = position_and_orientation[2] - np.pi/2
+        theta = position_and_orientation[2] - np.deg2rad(90+55)
         cart_x = position_and_orientation[0]
         cart_y = position_and_orientation[1]
-        theta = position_and_orientation[2]
+        # theta = position_and_orientation[2]
+        self.get_logger().info(f"angle = {theta}")
         # 2d trasformation matrix 
         rotation_matrix = np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta), np.cos(theta)]])
         position_vector = np.array([[cart_x], [cart_y]])
diff --git a/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py b/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py
index 854c1f5..e5a311f 100644
--- a/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py
+++ b/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py
@@ -67,7 +67,7 @@ def callback(self, msg:ConeMap) -> None:
 
         self.get_logger().info(f"all states: {hasattr(self,'_state_msg')}")   
         
-        if hasattr(self, "_state_msg") and hasattr(self, "_trajectories_msg"):
+        if hasattr(self, "_state_msg") and hasattr(self, "_trajectories_msg") and hasattr(self,"car_position"):
             
             # yup = 1
             # xup = -0.5