219 feature tune planning component to respond to new simulator fork

build/docker/agent/Dockerfile

@@ -162,19 +162,6 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt
 # Add agent code
 COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/
 
-# Install 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
-
-ENV FREENET_HOME=/erdos/dependencies/frenet_optimal_trajectory_planner
-RUN sudo chown $USERNAME:$USERNAME $PYLOT_HOME
-RUN sudo chown $USERNAME:$USERNAME $FREENET_HOME
-
-RUN git clone https://github.com/erdos-project/frenet_optimal_trajectory_planner.git $FREENET_HOME
-RUN cd $FREENET_HOME && source ./build.sh
-
-ENV PYTHONPATH=$PYTHONPATH:/erdos/dependencies
 # Link code into catkin workspace
 RUN ln -s /workspace/code /catkin_ws/src
 

code/acting/launch/acting.launch

@@ -14,12 +14,12 @@
     </node>
 
      <node pkg="acting" type="vehicle_controller.py" name="vehicle_controller" output="screen">
-        <param name="control_loop_rate" value="$(arg control_loop_rate)" /> <!-- leaderboard expects commands every 0.05 seconds OTHERWISE IT LAGS REALLY BADLY-->
+        <param name="control_loop_rate" value="0.1" /> <!-- leaderboard expects commands every 0.05 seconds OTHERWISE IT LAGS REALLY BADLY-->
         <param name="role_name" value="$(arg role_name)" />
     </node>
 
     <node pkg="acting" type="MainFramePublisher.py" name="MainFramePublisher" output="screen">
-        <param name="control_loop_rate" value="0.05" />
+        <param name="control_loop_rate" value="0.1" />
         <param name="role_name" value="$(arg role_name)" />
     </node>
 

code/planning/src/behavior_agent/behaviours/overtake.py

@@ -5,6 +5,8 @@
 import numpy as np
 
 from . import behavior_speed as bs
+import planning  # noqa: F401
+from local_planner.utils import NUM_WAYPOINTS
 
 """
 Source: https://github.com/ll7/psaf2
@@ -15,8 +17,8 @@ def convert_to_ms(speed):
     return speed / 3.6
 
 
-# Varaible to determine if overtake is currently exec
-OVERTAKE_EXECUTING = False
+# Varaible to determine the distance to overtak the object
+OVERTAKE_EXECUTING = 0
 
 
 class Approach(py_trees.behaviour.Behaviour):
@@ -413,7 +415,7 @@ def update(self):
         self.current_pos = np.array([data.pose.position.x,
                                     data.pose.position.y])
         distance = np.linalg.norm(self.first_pos - self.current_pos)
-        if distance > OVERTAKE_EXECUTING:
+        if distance > OVERTAKE_EXECUTING + NUM_WAYPOINTS:
             rospy.loginfo(f"Left Overtake: {self.current_pos}")
             return py_trees.common.Status.FAILURE
         else:

code/planning/src/behavior_agent/behaviours/road_features.py

@@ -5,6 +5,7 @@
 import numpy as np
 from scipy.spatial.transform import Rotation
 import rospy
+
 """
 Source: https://github.com/ll7/psaf2
 """
@@ -206,6 +207,7 @@ def setup(self, timeout):
         :return: True, as the set up is successful.
         """
         self.blackboard = py_trees.blackboard.Blackboard()
+
         return True
 
     def initialise(self):

code/planning/src/local_planner/ACC.py

@@ -8,8 +8,8 @@
 from nav_msgs.msg import Path
 # from std_msgs.msg import String
 from std_msgs.msg import Float32MultiArray, Float32, Bool
-from collision_check import CollisionCheck
 import numpy as np
+from utils import interpolate_speed, calculate_rule_of_thumb
 
 
 class ACC(CompatibleNode):
@@ -207,22 +207,20 @@ def loop(timer_event=None):
                     self.__current_velocity is not None:
                 # If we have obstalce speed and distance, we can
                 # calculate the safe speed
-                safety_distance = CollisionCheck.calculate_rule_of_thumb(
+                safety_distance = calculate_rule_of_thumb(
                     False, self.__current_velocity)
                 if self.obstacle_distance < safety_distance:
                     # If safety distance is reached, we want to reduce the
                     # speed to meet the desired distance
-                    # Lerp factor:
                     # https://encyclopediaofmath.org/index.php?title=Linear_interpolation
                     safe_speed = self.obstacle_speed * \
                         (self.obstacle_distance / safety_distance)
-                    lerp_factor = 0.2
-                    safe_speed = (1 - lerp_factor) * self.__current_velocity +\
-                        lerp_factor * safe_speed
+
+                    safe_speed = interpolate_speed(safe_speed,
+                                                   self.__current_velocity)
                     if safe_speed < 1.0:
                         safe_speed = 0
-                    self.logerr("ACC: Safe speed: " + str(safe_speed) +
-                                " Distance: " + str(self.obstacle_distance))
+                    self.logerr("ACC: Safe speed: " + str(safe_speed))
                     self.velocity_pub.publish(safe_speed)
                 else:
                     # If safety distance is reached just hold current speed
@@ -240,6 +238,8 @@ def loop(timer_event=None):
                 # If we have no obstacle, we want to drive with the current
                 # speed limit
                 # self.logerr("ACC: Speed limit: " + str(self.speed_limit))
+                # interpolated_speed = interpolate_speed(self.speed_limit,
+                #                                        self.__current_velocity)
                 self.velocity_pub.publish(self.speed_limit)
             else:
                 self.velocity_pub.publish(0)

code/planning/src/local_planner/collision_check.py

@@ -11,7 +11,7 @@
 # from std_msgs.msg import String
 from std_msgs.msg import Float32, Float32MultiArray
 from std_msgs.msg import Bool
-from utils import filter_vision_objects
+from utils import filter_vision_objects, calculate_rule_of_thumb
 
 
 class CollisionCheck(CompatibleNode):
@@ -97,7 +97,7 @@ def __set_distance(self, data: Float32MultiArray):
         if nearest_object is None and \
                 self.__object_last_position is not None and \
                 rospy.get_rostime() - self.__object_last_position[0] > \
-                rospy.Duration(3):
+                rospy.Duration(2):
             self.update_distance(True)
             return
         elif nearest_object is None:
@@ -116,7 +116,7 @@ def __set_distance_oncoming(self, data: Float32MultiArray):
         if (nearest_object is None and
                 self.__last_position_oncoming is not None and
                 rospy.get_rostime() - self.__last_position_oncoming[0] >
-                rospy.Duration(3)):
+                rospy.Duration(2)):
             self.update_distance_oncoming(True)
             return
         elif nearest_object is None:
@@ -151,7 +151,6 @@ def calculate_obstacle_speed(self):
                 self.__object_last_position is None:
             return
         # If distance is np.inf no car is in front
-
         # Calculate time since last position update
         rospy_time_difference = self.__object_last_position[0] - \
             self.__object_first_position[0]
@@ -209,26 +208,6 @@ def meters_to_collision(self, obstacle_speed, distance):
         return self.time_to_collision(obstacle_speed, distance) * \
             self.__current_velocity
 
-    @staticmethod
-    def calculate_rule_of_thumb(emergency, speed):
-        """Calculates the rule of thumb as approximation
-        for the braking distance
-
-        Args:
-            emergency (bool): if emergency brake is initiated
-            speed (float): speed of the vehicle (km/h)
-
-        Returns:
-            float: distance calculated with rule of thumb
-        """
-        reaction_distance = speed * 0.5
-        braking_distance = (speed * 0.36)**2
-        if emergency:
-            # Emergency brake is really effective in Carla
-            return reaction_distance + braking_distance / 4
-        else:
-            return reaction_distance + braking_distance
-
     def check_crash(self, obstacle):
         """ Checks if and when the ego vehicle will crash
             with the obstacle in front
@@ -242,12 +221,10 @@ def check_crash(self, obstacle):
         collision_time = self.time_to_collision(obstacle_speed, distance)
         # collision_meter = self.meters_to_collision(obstacle_speed, distance)
         # safe_distance2 = self.calculate_rule_of_thumb(False)
-        emergency_distance2 = self.calculate_rule_of_thumb(
+        emergency_distance2 = calculate_rule_of_thumb(
             True, self.__current_velocity)
         if collision_time > 0:
             if distance < emergency_distance2:
-                # Initiate emergency brake
-                self.logerr(f"Emergency Brake: {distance}")
                 self.emergency_pub.publish(True)
             # When no emergency brake is needed publish collision object
             data = Float32MultiArray(data=[distance, obstacle_speed])
@@ -262,14 +239,6 @@ def run(self):
         Control loop
         :return:
         """
-        # def loop(timer_event=None):
-        #     """
-        #     Checks if distance to a possible object is too small and
-        #     publishes the desired speed to motion planning
-        #     """
-        #     self.update_distance()
-        #     self.calculate_obstacle_speed()
-        # self.new_timer(self.control_loop_rate, loop)
         self.spin()
 
 

code/planning/src/local_planner/motion_planning.py

@@ -17,7 +17,7 @@
 import planning  # noqa: F401
 from behavior_agent.behaviours import behavior_speed as bs
 
-from utils import convert_to_ms, spawn_car
+from utils import convert_to_ms, spawn_car, NUM_WAYPOINTS
 
 # from scipy.spatial._kdtree import KDTree
 
@@ -233,7 +233,7 @@ def overtake_fallback(self, distance, pose_list, unstuck=False):
         normal_x_offset = 2
         unstuck_x_offset = 3.5  # could need adjustment with better steering
         selection = pose_list[int(currentwp):int(currentwp) +
-                              int(distance) + 7]
+                              int(distance) + NUM_WAYPOINTS]
         waypoints = self.convert_pose_to_array(selection)
 
         if unstuck is True:
@@ -268,7 +268,8 @@ def overtake_fallback(self, distance, pose_list, unstuck=False):
         path.header.stamp = rospy.Time.now()
         path.header.frame_id = "global"
         path.poses = pose_list[:int(currentwp)] + \
-            result + pose_list[int(currentwp + distance + 7):]
+            result + pose_list[int(currentwp + distance + NUM_WAYPOINTS):]
+
         self.trajectory = path
 
     def __set_trajectory(self, data: Path):
@@ -540,8 +541,7 @@ def __get_speed_overtake(self, behavior: str) -> float:
         elif behavior == bs.ot_enter_slow.name:
             speed = self.__calc_speed_to_stop_overtake()
         elif behavior == bs.ot_leave.name:
-            speed = convert_to_ms(10.)
-
+            speed = convert_to_ms(30.)
         return speed
 
     def __get_speed_cruise(self) -> float: