diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index acf7edf7..2c6ce5ac 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -163,9 +163,7 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/ # Install Frenet Optimal Trajectory Planner -RUN sudo mkdir /erdos -RUN sudo mkdir /erdos/dependencies -RUN sudo mkdir /erdos/dependencies/frenet_optimal_trajectory_planner +RUN sudo mkdir -p /erdos/dependencies/frenet_optimal_trajectory_planner # Needed to resolve dependencies correctly inside freent_optimal_trajectory_planner ENV PYLOT_HOME=/erdos diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index e39cb4be..41ee798c 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -74,7 +74,7 @@ def __init__(self): self.path_pub = self.new_publisher( msg_type=Path, - topic='/paf/' + self.role_name + '/trajectory', + topic='/paf/' + self.role_name + '/trajectory_global', qos_profile=1) self.speed_limit_pub = self.new_publisher( @@ -257,16 +257,16 @@ def run(self): :return: """ - def loop(timer_event=None): - if len(self.path_backup.poses) < 1: - return + # def loop(timer_event=None): + # if len(self.path_backup.poses) < 1: + # return - # Continuously update paths time to update car position in rviz - # TODO: remove next lines when local planner exists - self.path_backup.header.stamp = rospy.Time.now() - self.path_pub.publish(self.path_backup) + # # # Continuously update paths time to update car position in rviz + # # # TODO: remove next lines when local planner exists + # self.path_backup.header.stamp = rospy.Time.now() + # self.path_pub.publish(self.path_backup) - self.new_timer(self.control_loop_rate, loop) + # self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 4b7c2b9b..e09bc575 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -47,7 +47,7 @@ def __init__(self): # Get trajectory to determine current speed limit self.trajectory_sub: Subscriber = self.new_subscription( Path, - f"/paf/{self.role_name}/trajectory", + f"/paf/{self.role_name}/trajectory_global", self.__set_trajectory, qos_profile=1) @@ -66,6 +66,14 @@ def __init__(self): Float32, f"/paf/{self.role_name}/acc_velocity", qos_profile=1) + self.wp_publisher: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/current_wp", + qos_profile=1) + self.speed_limit_publisher: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/speed_limit", + qos_profile=1) # List of all speed limits, sorted by waypoint index self.__speed_limits_OD: [float] = [] @@ -88,7 +96,7 @@ def _set_distance(self, data: Float32): """Get min distance to object in front from perception Args: - data (MinDistance): Minimum Distance from LIDAR + data (Float32): Minimum Distance from LIDAR """ self.obstacle_distance = data.data @@ -148,8 +156,10 @@ def __current_position_callback(self, data: PoseStamped): if d_new < d_old: # update current waypoint and corresponding speed limit self.__current_wp_index += 1 + self.wp_publisher.publish(self.__current_wp_index) self.speed_limit = \ self.__speed_limits_OD[self.__current_wp_index] + self.speed_limit_publisher.publish(self.speed_limit) def run(self): """ diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index f6b05087..e72396b5 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -1,5 +1,4 @@ #!/usr/bin/env python -# import rospy # import tf.transformations import ros_compatibility as roscomp import rospy @@ -10,8 +9,7 @@ from std_msgs.msg import String, Float32, Bool from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion - - +from carla_msgs.msg import CarlaSpeedometer import numpy as np from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ @@ -20,15 +18,10 @@ import planning # noqa: F401 from behavior_agent.behaviours import behavior_speed as bs -# from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion -# from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo -# from nav_msgs.msg import Path -# from std_msgs.msg import String -# from std_msgs.msg import Float32MultiArray - +from utils import convert_to_ms, approx_obstacle_pos, \ + hyperparameters -def convert_to_ms(speed): - return speed / 3.6 +# from scipy.spatial._kdtree import KDTree class MotionPlanning(CompatibleNode): @@ -49,17 +42,43 @@ def __init__(self): self.__acc_speed = 0.0 self.__stopline = None # (Distance, isStopline) self.__change_point = None # (Distance, isLaneChange, roadOption) + self.published = False + self.current_pos = None + self.current_heading = None + self.trajectory = None + self.overtaking = False + self.overtake_start = rospy.get_rostime() + self.current_wp = None + self.enhanced_path = None + self.current_speed = None + self.speed_limit = None + self.counter = 0 self.speed_list = [] self.__first_trajectory = None self.__corners = None self.__in_corner = False - # Subscriber + self.speed_limit_sub = self.new_subscription( + Float32, + f"/paf/{self.role_name}/speed_limit", + self.__set_speed_limit, + qos_profile=1) + self.velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + qos_profile=1) + self.head_sub = self.new_subscription( + Float32, + f"/paf/{self.role_name}/current_heading", + self.__set_heading, + qos_profile=1) + self.trajectory_sub = self.new_subscription( Path, - f"/paf/{self.role_name}/trajectory", - self.__save_trajectory, + f"/paf/{self.role_name}/trajectory_global", + self.__set_trajectory, qos_profile=1) self.current_pos_sub = self.new_subscription( PoseStamped, @@ -95,15 +114,20 @@ def __init__(self): qos_profile=1) # Publisher - # self.traj_pub: Publisher = self.new_publisher( - # Path, - # f"/paf/{self.role_name}/trajectory", - # qos_profile=1) + self.traj_pub: Publisher = self.new_publisher( + msg_type=Path, + topic=f"/paf/{self.role_name}/trajectory", + qos_profile=1) self.velocity_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/target_velocity", qos_profile=1) + self.wp_subs = self.new_subscription( + Float32, + f"/paf/{self.role_name}/current_wp", + self.__set_wp, + qos_profile=1) # Publisher for emergency stop self.emergency_pub = self.new_publisher( Bool, @@ -111,8 +135,39 @@ def __init__(self): qos_profile=1) self.logdebug("MotionPlanning started") - self.published = False - self.current_pos = None + self.counter = 0 + + def __set_speed_limit(self, data: Float32): + """Set current speed limit + + Args: + data (Float32): Current speed limit + """ + self.speed_limit = data.data + + def __get_current_velocity(self, data: CarlaSpeedometer): + """Get current velocity from CarlaSpeedometer + + Args: + data (CarlaSpeedometer): Current velocity + """ + self.current_speed = float(data.speed) + + def __set_wp(self, data: Float32): + """Recieve current waypoint index from ACC + + Args: + data (Float32): Waypoint index + """ + self.current_wp = data.data + + def __set_heading(self, data: Float32): + """Set current Heading + + Args: + data (Float32): Current Heading vom Subscriber + """ + self.current_heading = data.data def __save_trajectory(self, data): if self.__first_trajectory is None: @@ -122,12 +177,62 @@ def __save_trajectory(self, data): def __set_current_pos(self, data: PoseStamped): """set current position - Args: data (PoseStamped): current position """ self.current_pos = np.array([data.pose.position.x, - data.pose.position.y]) + data.pose.position.y, + data.pose.position.z]) + + def change_trajectory(self, distance: float): + self.overtake_start = rospy.get_rostime() + limit_waypoints = 30 + np_array = np.array(self.trajectory.poses) + obstacle_position = approx_obstacle_pos(distance, + self.current_heading, + self.current_pos, + self.current_speed) + # trajectory_np = self.convert_pose_to_array(np_array) + # wp = KDTree(trajectory_np[:, :2]).query(obstacle_position[0][:2])[1] + selection = np_array[int(self.current_wp):int(self.current_wp) + + int(distance + limit_waypoints)] + waypoints = self.convert_pose_to_array(selection) + + initial_conditions = { + 'ps': 0, + 'target_speed': self.current_speed, + 'pos': np.array([self.current_pos[0], self.current_pos[1]]), + 'vel': np.array([obstacle_position[2][0], + obstacle_position[2][1]]), + 'wp': waypoints, + 'obs': np.array([[obstacle_position[0][0], + obstacle_position[0][1], + obstacle_position[1][0], + obstacle_position[1][1]]]) + } + result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ + speeds_y, misc, costs, success = run_fot(initial_conditions, + hyperparameters) + if success: + result = [] + for i in range(len(result_x)): + position = Point(result_x[i], result_y[i], 0) + quaternion = tf.transformations.quaternion_from_euler(0, + 0, + iyaw[i]) + orientation = Quaternion(x=quaternion[0], y=quaternion[1], + z=quaternion[2], w=quaternion[3]) + pose = Pose(position, orientation) + pos = PoseStamped() + pos.header.frame_id = "global" + pos.pose = pose + result.append(pos) + path = Path() + path.header.stamp = rospy.Time.now() + path.header.frame_id = "global" + path.poses = list(np_array[:int(self.current_wp)]) + \ + result + list(np_array[int(self.current_wp + 25 + 30):]) + self.trajectory = path def __set_trajectory(self, data: Path): """get current trajectory global planning @@ -135,71 +240,7 @@ def __set_trajectory(self, data: Path): Args: data (Path): Trajectory waypoints """ - if len(data.poses) and not self.published > 0: - self.published = True - np_array = np.array(data.poses) - selection = np_array[5:17] - waypoints = self.convert_pose_to_array(selection) - self.logerr(waypoints) - obs = np.array( - [[waypoints[5][0]-0.5, waypoints[5][1], waypoints[5][0]+0.5, - waypoints[5][1]-2]]) - initial_conditions = { - 'ps': 0, - 'target_speed': self.__acc_speed, - 'pos': np.array([983.5807552562393, 5370.014637890163]), - 'vel': np.array([5, 1]), - 'wp': waypoints, - 'obs': obs - } - hyperparameters = { - "max_speed": 25.0, - "max_accel": 15.0, - "max_curvature": 15.0, - "max_road_width_l": 3.0, - "max_road_width_r": 0.5, - "d_road_w": 0.5, - "dt": 0.2, - "maxt": 20.0, - "mint": 6.0, - "d_t_s": 0.5, - "n_s_sample": 2.0, - "obstacle_clearance": 0.1, - "kd": 1.0, - "kv": 0.1, - "ka": 0.1, - "kj": 0.1, - "kt": 0.1, - "ko": 0.1, - "klat": 1.0, - "klon": 1.0, - "num_threads": 0, # set 0 to avoid using threaded algorithm - } - result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ - speeds_y, misc, costs, success = run_fot(initial_conditions, - hyperparameters) - if success: - print("Success!") - print("result_x: ", result_x) - print("result_y: ", result_y) - result = [] - for i in range(len(result_x)): - position = Point(result_x[i], result_y[i], 703) - quat = tf.transformations.quaternion_from_euler(0, 0, - iyaw[i]) - orientation = Quaternion(x=quat[0], y=quat[1], - z=quat[2], w=quat[3]) - pose = Pose(position, orientation) - pos = PoseStamped() - pos.header.stamp = rospy.Time.now() - pos.header.frame_id = "global" - pos.pose = pose - result.append(PoseStamped) - path = Path() - path.header.stamp = rospy.Time.now() - path.path_backup.header.frame_id = "global" - path.poses = list(np_array[:5]) + result + list(np_array[17:]) - self.traj_pub.publish(path) + self.trajectory = data def __calc_corner_points(self): coords = self.convert_pose_to_array(np.array(self.__first_trajectory)) @@ -444,6 +485,8 @@ def loop(timer_event=None): self.__corners is not None): self.update_target_speed(self.__acc_speed, self.__curr_behavior) + self.trajectory.header.stamp = rospy.Time.now() + self.traj_pub.publish(self.trajectory) self.new_timer(self.control_loop_rate, loop) self.spin() @@ -455,7 +498,6 @@ def loop(timer_event=None): :param args: """ roscomp.init('MotionPlanning') - try: node = MotionPlanning() node.run() diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py new file mode 100644 index 00000000..dee2fc25 --- /dev/null +++ b/code/planning/src/local_planner/utils.py @@ -0,0 +1,114 @@ +from scipy.spatial.transform import Rotation +import numpy as np +import math + +hyperparameters = { + "max_speed": 15, + "max_accel": 4.0, + "max_curvature": 30.0, + "max_road_width_l": 0.1, + "max_road_width_r": 4, + "d_road_w": 0.2, + "dt": 0.2, + "maxt": 30, + "mint": 6.0, + "d_t_s": 0.5, + "n_s_sample": 2.0, + "obstacle_clearance": 2, + "kd": 1.0, + "kv": 0.1, + "ka": 0.1, + "kj": 0.1, + "kt": 0.1, + "ko": 0.1, + "klat": 1.0, + "klon": 1.0, + "num_threads": 1, # set 0 to avoid using threaded algorithm +} + + +def location_to_gps(lat_ref: float, lon_ref: float, x: float, y: float): + """Convert world coordinates to (lat,lon,z) coordinates + Copied from: + https://github.com/carla-simulator/scenario_runner/blob/master/srunner/tools/route_manipulation.py + + Args: + lat_ref (float): reference lat value + lon_ref (float): reference lat value + x (float): x-Coordinate value + y (float): y-Coordinate value + + Returns: + dict: Dictionary with (lat,lon,z) coordinates + """ + + EARTH_RADIUS_EQUA = 6378137.0 # pylint: disable=invalid-name + scale = math.cos(lat_ref * math.pi / 180.0) + mx = scale * lon_ref * math.pi * EARTH_RADIUS_EQUA / 180.0 + my = scale * EARTH_RADIUS_EQUA * math.log(math.tan((90.0 + lat_ref) * + math.pi / 360.0)) + mx += x + my -= y + + lon = mx * 180.0 / (math.pi * EARTH_RADIUS_EQUA * scale) + lat = 360.0 * math.atan(math.exp(my / (EARTH_RADIUS_EQUA * scale))) /\ + math.pi - 90.0 + z = 703 + + return {'lat': lat, 'lon': lon, 'z': z} + + +def approx_obstacle_pos(distance: float, heading: float, + ego_pos: np.array, speed: float): + """calculate the position of the obstacle in the global coordinate system + based on ego position, heading and distance + + Args: + speed (float): Speed of the ego vehicle + distance (float): Distance to the obstacle + heading (float): Ego vehivle heading + ego_pos (np.array): Position in [x, y, z] + + Returns: + np.array: approximated position of the obstacle + """ + rotation_matrix = Rotation.from_euler('z', heading) + + # Create distance vector with 0 rotation + relative_position_local = np.array([distance, 0, 0]) + + # speed vector + speed_vector = rotation_matrix.apply(np.array([speed, 0, 0])) + # Rotate distance vector to match heading + absolute_position_local = rotation_matrix.apply(relative_position_local) + + # Add egomposition vector with distance vetor to get absolute position + vehicle_position_global_start = ego_pos + absolute_position_local + + length = np.array([3, 0, 0]) + length_vector = rotation_matrix.apply(length) + + offset = np.array([1, 0, 0]) + rotation_adjusted = Rotation.from_euler('z', heading + math.radians(90)) + offset_front = rotation_adjusted.apply(offset) + + rotation_adjusted = Rotation.from_euler('z', heading + math.radians(270)) + offset_back = rotation_adjusted.apply(offset) + + vehicle_position_global_end = vehicle_position_global_start + \ + length_vector + offset_back + + return vehicle_position_global_start + offset_front, \ + vehicle_position_global_end, speed_vector + + +def convert_to_ms(speed: float): + """Convert km/h to m/s + + Args: + speed (float): speed in km/h + + Returns: + float: speed in m/s + """ + return speed / 3.6 diff --git a/doc/03_research/03_planning/00_paf23/05_local_trajectory_planning.md b/doc/03_research/03_planning/00_paf23/05_local_trajectory_planning.md new file mode 100644 index 00000000..9d9f2bc1 --- /dev/null +++ b/doc/03_research/03_planning/00_paf23/05_local_trajectory_planning.md @@ -0,0 +1,17 @@ +# Local trajectory planning + +**Summary:** Explanation on how the frenet trajectory planner is used inside the motion planning component. + +--- + +## Author + +Samuel Kühnel + +## Date + +23.01.2024 + +## Position calculation from Obstacle + +The position from a possible obstacle that we need to overtake is calculated via the current heading from the ego vehicle, the current position and the measured distance. The