diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile
index 93b677b3..bee277af 100644
--- a/build/docker/agent/Dockerfile
+++ b/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
diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch
index ec4e7663..076f9660 100644
--- a/code/acting/launch/acting.launch
+++ b/code/acting/launch/acting.launch
@@ -14,12 +14,12 @@
-
+
-
+
diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py
index bf0941be..08d6d5b1 100644
--- a/code/planning/src/behavior_agent/behaviours/overtake.py
+++ b/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:
diff --git a/code/planning/src/behavior_agent/behaviours/road_features.py b/code/planning/src/behavior_agent/behaviours/road_features.py
index a00d566b..5ee2ac5e 100755
--- a/code/planning/src/behavior_agent/behaviours/road_features.py
+++ b/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):
diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py
index 8cefc85b..b8fad55b 100755
--- a/code/planning/src/local_planner/ACC.py
+++ b/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)
diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py
index 85540e21..14df6e8e 100755
--- a/code/planning/src/local_planner/collision_check.py
+++ b/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()
diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py
index b54468b1..d0a87b21 100755
--- a/code/planning/src/local_planner/motion_planning.py
+++ b/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:
diff --git a/code/planning/src/local_planner/test_traj.py b/code/planning/src/local_planner/test_traj.py
deleted file mode 100644
index 05a00c73..00000000
--- a/code/planning/src/local_planner/test_traj.py
+++ /dev/null
@@ -1,68 +0,0 @@
-import numpy as np
-
-from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \
- import run_fot
-
-import matplotlib.pyplot as plt
-
-
-wp = np.r_[[np.full((50), 983.5889666959667)],
- [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T
-obs = np.array([[983.568124548765, 5384.0219828457075,
- 983.628124548765, 5386.0219828457075]])
-initial_conditions = {
- 'ps': 0,
- 'target_speed': 6,
- 'pos': np.array([983.5807552562393, 5370.014637890163]),
- 'vel': np.array([5, 1]),
- 'wp': wp,
- 'obs': obs
-}
-
-hyperparameters = {
- "max_speed": 25.0,
- "max_accel": 15.0,
- "max_curvature": 20.0,
- "max_road_width_l": 3.0,
- "max_road_width_r": 0,
- "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": 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": 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)
- print("yaw!", iyaw)
- fig, ax = plt.subplots(1, 2)
-
- ax[0].scatter(wp[:, 0], wp[:, 1], label="original")
- ax[0].scatter([obs[0, 0], obs[0, 2]],
- [obs[0, 1], obs[0, 3]], label="object")
- ax[0].set_xticks([obs[0, 0], obs[0, 2]])
- ax[1].scatter(result_x, result_y, label="frenet")
- ax[1].scatter([obs[0, 0], obs[0, 2]],
- [obs[0, 1], obs[0, 3]], label="object")
- ax[1].set_xticks([obs[0, 0], obs[0, 2]])
- plt.legend()
- plt.show()
-else:
- print("Failure!")
diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py
index 0871b552..e1bece7d 100644
--- a/code/planning/src/local_planner/utils.py
+++ b/code/planning/src/local_planner/utils.py
@@ -1,32 +1,23 @@
from scipy.spatial.transform import Rotation
import numpy as np
import math
+import carla
+import os
# import rospy
-hyperparameters = {
- "max_speed": 15,
- "max_accel": 4.0,
- "max_curvature": 30.0,
- "max_road_width_l": 4,
- "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": 1.5,
- "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
-}
+"""
+This file represents the utility functions for the local planner and other
+components.
+It containes parameters and utility functions to reduce code in the ros nodes.
+"""
+
+# Number of waypoints to be used for the overtaking maneuver
+NUM_WAYPOINTS = 7
+# Factor for linear interpolation of target speed values for the ACC
+LERP_FACTOR = 0.5
+# Earth radius in meters for location_to_GPS
+EARTH_RADIUS_EQUA = 6378137.0
def get_distance(pos_1, pos_2):
@@ -61,7 +52,6 @@ def location_to_gps(lat_ref: float, lon_ref: float, x: float, y: float):
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) *
@@ -77,6 +67,26 @@ def location_to_gps(lat_ref: float, lon_ref: float, x: float, y: float):
return {'lat': lat, 'lon': lon, 'z': z}
+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 / 2
+ braking_distance = (speed * 0.36)**2
+ if emergency:
+ # Emergency brake is really effective in Carla
+ return reaction_distance + braking_distance
+ else:
+ return reaction_distance + braking_distance
+
+
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
@@ -139,8 +149,6 @@ def spawn_car(distance):
Args:
distance (float): distance
"""
- import carla
- import os
CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1')
CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000'))
@@ -161,21 +169,18 @@ def spawn_car(distance):
spawnPoint = carla.Transform(ego_vehicle.get_location() +
carla.Location(y=distance.data),
ego_vehicle.get_transform().rotation)
- spawnpoint2 = carla.Transform(ego_vehicle.get_location() +
- carla.Location(x=2.5, y=distance.data + 1),
- ego_vehicle.get_transform().rotation)
+ # spawnpoint2 = carla.Transform(ego_vehicle.get_location() +
+ # carla.Location(x=2.5, y=distance.data + 1),
+ # ego_vehicle.get_transform().rotation)
vehicle = world.spawn_actor(bp, spawnPoint)
- vehicle2 = world.spawn_actor(bp, spawnpoint2)
- vehicle2.set_autopilot(False)
+ # vehicle2 = world.spawn_actor(bp, spawnpoint2)
+ # vehicle2.set_autopilot(False)
vehicle.set_autopilot(False)
- # vehicle.set_location(loc)
- # coords = vehicle.get_location()
- # get spectator
- spectator = world.get_spectator()
- # set spectator to follow ego vehicle with offset
- spectator.set_transform(
- carla.Transform(ego_vehicle.get_location() + carla.Location(z=50),
- carla.Rotation(pitch=-90)))
+ # vehicle.set_target_velocity(carla.Vector3D(0, 6, 0))
+
+
+def interpolate_speed(speed_target, speed_current):
+ return (1 - LERP_FACTOR) * speed_current + LERP_FACTOR * speed_target
def filter_vision_objects(float_array, oncoming):