Merge pull request #132 from UOA-FSAE/fs_driverless_sim

Fs driverless sim

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',
         ],
     },
 )

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
 

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):

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()

src/moa/moa_bringup/launch/fs_sim_launch.py

@@ -0,0 +1,72 @@
+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',
+        ),
+
+        # 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',
+        #     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',
+        ),
+    ])

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,66 +109,68 @@ 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
+        # self.get_logger().info(f"len of traj = {len(trajectories)}")
 
-    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)
+        return trajectories, angs
 
-    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}")
+    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
+        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]])