Merge pull request #177 from una-auxme:165-feature-local-trajectory-p…

…lanning-with-frenet-trajectory-planner

165 feature local trajectory planning with frenet trajectory planner

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
 

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

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

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,24 +114,60 @@ 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,
             f"/paf/{self.role_name}/emergency",
             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,84 +177,70 @@ 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
 
         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()