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