Skip to content

Commit

Permalink
Merge pull request #211 from una-auxme/192-feature-finish-overtake-be…
Browse files Browse the repository at this point in the history 
…havior

192 feature finish overtake behavior
  • Loading branch information
@okrusch
okrusch committed Mar 11, 2024
2 parents 908d383 + 6aeeebb commit 7f5c521
Show file tree
Hide file tree
Showing 20 changed files with 605 additions and 284 deletions.
4 changes: 3 additions & 1 deletion .flake8
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,4 +2,6 @@
exclude= code/planning/src/behavior_agent/behavior_tree.py,
code/planning/src/behavior_agent/behaviours/__init__.py,
code/planning/src/behavior_agent/behaviours,
code/planning/__init__.py
code/planning/__init__.py,
doc/02_development/templates/template_class_no_comments.py,
doc/02_development/templates/template_class.py
2 changes: 1 addition & 1 deletion build/docker-compose.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ services:
tty: true
shm_size: 2gb
#command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS"
#command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
# command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP"

logging:
Expand Down
2 changes: 1 addition & 1 deletion code/agent/launch/dev.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
<arg name="objects_definition_file" value='$(find agent)/config/dev_objects.json'/>
</include>

<include file="$(find agent)/launch/agent_manual.launch">
<include file="$(find agent)/launch/agent.launch">
<arg name='role_name' value='$(arg role_name)'/>
</include>

Expand Down
130 changes: 66 additions & 64 deletions code/perception/src/vision_node.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,7 +191,7 @@ def setup_camera_publishers(self):

def setup_object_distance_publishers(self):
self.distance_publisher = self.new_publisher(
msg_type=numpy_msg(Float32MultiArray),
msg_type=Float32MultiArray,
topic=f"/paf/{self.role_name}/{self.side}/object_distance",
qos_profile=1)

Expand Down Expand Up @@ -227,6 +227,7 @@ def handle_camera_image(self, image):
if side == "Right":
self.publisher_right.publish(img_msg)

# locals().clear()
# print(f"Published Image on Side: {side}")
pass

Expand Down Expand Up @@ -254,6 +255,7 @@ def handle_dist_array(self, dist_array):
desired_encoding='passthrough')
# print("RECEIVED DIST")
self.dist_arrays = dist_array
# locals().clear()

def predict_torch(self, image):
self.model.eval()
Expand Down Expand Up @@ -285,40 +287,6 @@ def predict_ultralytics(self, image):

output = self.model(cv_image, half=True, verbose=False)

"""distance_output = []
c_boxes = []
c_labels = []
for r in output:
boxes = r.boxes
for box in boxes:
cls = box.cls.item()
pixels = box.xyxy[0]
print(pixels)
if len(self.depth_images) > 0:
distances = np.asarray(
[self.depth_images[i][int(pixels[1]):int(pixels[3]):1,
int(pixels[0]):int(pixels[2]):1]
for i in range(len(self.depth_images))])
non_zero_filter = distances[distances != 0]
if len(non_zero_filter) > 0:
obj_dist = np.min(non_zero_filter)
else:
obj_dist = np.inf
c_boxes.append(torch.tensor(pixels))
c_labels.append(f"Class: {cls}, Meters: {obj_dist}")
distance_output.append([cls, obj_dist])
# print(distance_output)
# self.logerr(distance_output)
self.distance_publisher.publish(
Float32MultiArray(data=distance_output))
transposed_image = np.transpose(cv_image, (2, 0, 1))
image_np_with_detections = torch.tensor(transposed_image,
dtype=torch.uint8)"""

# handle distance of objects
distance_output = []
c_boxes = []
Expand All @@ -339,14 +307,30 @@ def predict_ultralytics(self, image):
distances_copy[distances_copy == 0] = np.inf

if len(non_zero_filter) > 0:
sorted_indices = np.argsort(distances_copy[:, :, 0],
axis=None)
x1, y1 = np.unravel_index(sorted_indices[0],
distances_copy.shape[:2])
x2, y2 = np.unravel_index(sorted_indices[1],
min_x_sorted_indices = np.argsort(
distances_copy[:, :, 0],
axis=None)
x1, y1 = np.unravel_index(min_x_sorted_indices[0],
distances_copy.shape[:2])
"""x2, y2 = np.unravel_index(min_x_sorted_indices[1],
distances_copy.shape[:2])"""

abs_distance_copy = np.abs(distances_copy.copy())
min_y_sorted_indices = np.argsort(
abs_distance_copy[:, :, 1],
axis=None)
x3, y3 = np.unravel_index(min_y_sorted_indices[0],
abs_distance_copy.shape[:2])
"""
x4, y4 = np.unravel_index(min_y_sorted_indices[1],
abs_distance_copy.shape[:2])
"""

obj_dist1 = distances_copy[x1][y1].copy()
obj_dist2 = distances_copy[x2][y2].copy()
# obj_dist2 = distances_copy[x2][y2].copy()
obj_dist3 = distances_copy[x3][y3].copy()
# obj_dist4 = distances_copy[x4][y4].copy()
# print(obj_dist1, obj_dist3)

abs_distance = np.sqrt(
obj_dist1[0]**2 +
Expand All @@ -355,38 +339,53 @@ def predict_ultralytics(self, image):

# create 2d glass plane at object
# with box dimension
scale_width = abs(obj_dist1[1] - obj_dist2[1])\
/ abs(y1-y2)
scale_height = abs(obj_dist1[2] - obj_dist2[2])\
/ abs(x1-x2)
width = distances_copy.shape[1] * scale_width
height = distances_copy.shape[0] * scale_height

# upper left
ul_x = obj_dist1[0]
ul_y = obj_dist1[1] - (-y1 + scale_width)
ul_z = obj_dist1[2] - (-x1 + scale_height)

# lower right
lr_x = obj_dist1[0]
lr_y = ul_y + width
lr_z = ul_z + height
# width_diff = abs(y1-y2)
# height_diff = abs(x1-x2)

"""if width_diff > 0 and height_diff > 0:
scale_width = abs(obj_dist1[1] - obj_dist2[1])\
/ width_diff
scale_height = abs(obj_dist1[2] - obj_dist2[2])\
/ height_diff
width = distances_copy.shape[1] * scale_width
height = distances_copy.shape[0] * scale_height
# upper left
ul_x = obj_dist1[0]
ul_y = obj_dist1[1] - (-y1 + scale_width)
ul_z = obj_dist1[2] - (-x1 + scale_height)
# lower right
lr_x = obj_dist1[0]
lr_y = ul_y + width
lr_z = ul_z + height
# -5 < y < 8"""

distance_output.append(float(cls))
distance_output.append(float(obj_dist1[0]))
distance_output.append(float(obj_dist3[1]))

else:
obj_dist1 = (np.inf, np.inf, np.inf)
obj_dist3 = (np.inf, np.inf, np.inf)
abs_distance = np.inf
"""distance_output.append(float(cls))
distance_output.append(float(abs_distance))
distance_output.append(float(np.inf))
distance_output.append(float(np.inf))
distance_output.append(float(np.inf))
distance_output.append(float(np.inf))
distance_output.append(float(np.inf))
distance_output.append(float(np.inf))"""

c_boxes.append(torch.tensor(pixels))
c_labels.append(f"Class: {cls},"
f"Meters: {round(abs_distance, 2)},"
f"({round(float(obj_dist1[0]), 2)},"
f"{round(float(obj_dist1[1]), 2)},"
f"{round(float(obj_dist1[2]), 2)})")
distance_output.append([cls,
abs_distance,
ul_x, ul_y, ul_z,
lr_x, lr_y, lr_z])
f"{round(float(obj_dist3[1]), 2)})")

# print("DISTANCE_ARRAY: ", distance_output)
self.distance_publisher.publish(
Float32MultiArray(data=distance_output))

Expand All @@ -398,7 +397,6 @@ def predict_ultralytics(self, image):
self.process_traffic_lights(output[0], cv_image, image.header)

c_boxes = torch.stack(c_boxes)
# print(image_np_with_detections.shape, c_boxes.shape, c_labels)
box = draw_bounding_boxes(image_np_with_detections,
c_boxes,
c_labels,
Expand All @@ -408,7 +406,9 @@ def predict_ultralytics(self, image):
np_box_img = np.transpose(box.detach().numpy(),
(1, 2, 0))
box_img = cv2.cvtColor(np_box_img, cv2.COLOR_BGR2RGB)
# locals().clear()
return box_img

# return output[0].plot()

def process_traffic_lights(self, prediction, cv_image, image_header):
Expand Down Expand Up @@ -438,6 +438,8 @@ def process_traffic_lights(self, prediction, cv_image, image_header):
traffic_light_image.header = image_header
self.traffic_light_publisher.publish(traffic_light_image)

# locals().clear()

def create_mask(self, input_image, model_output):
output_predictions = torch.argmax(model_output, dim=0)
for i in range(21):
Expand Down
6 changes: 3 additions & 3 deletions code/planning/launch/planning.launch
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,19 @@
<launch>
<node pkg="planning" type="collision_check.py" name="CollisionCheck" output="screen">
<param name="role_name" value="hero" />
<param name="control_loop_rate" value="0.3" />
<param name="control_loop_rate" value="0.05" />
</node>
<node pkg="planning" type="ACC.py" name="ACC" output="screen">
<param name="role_name" value="hero" />
<param name="control_loop_rate" value="0.5" />
<param name="control_loop_rate" value="0.3" />
</node>
<!-- <node pkg="local_planner" type="local_planner/dev_collision_publisher.py" name="DevCollisionCheck" output="screen">
<param name="role_name" value="hero" />
<param name="control_loop_rate" value="0.3" />
</node> -->
<node pkg="planning" type="motion_planning.py" name="MotionPlanning" output="screen">
<param name="role_name" value="hero" />
<param name="control_loop_rate" value="0.3" />
<param name="control_loop_rate" value="0.1" />
</node>

<!-- <node pkg="planning" type="dev_global_route.py" name="DevGlobalRoute" output="screen">
Expand Down
2 changes: 1 addition & 1 deletion code/planning/src/behavior_agent/behaviours/__init__.py
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,3 @@
from . import intersection, lane_change, maneuvers, meta, road_features
from . import intersection, lane_change, overtake, maneuvers, meta, road_features
from . import topics2blackboard, traffic_objects
from . import behavior_speed
15 changes: 14 additions & 1 deletion code/planning/src/behavior_agent/behaviours/behavior_speed.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ def convert_to_ms(speed):
# TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128)
lc_app_blocked = Behavior("lc_app_blocked", -2)

lc_app_free = Behavior("lc_app_free", convert_to_ms(20))
lc_app_free = Behavior("lc_app_free", convert_to_ms(30.0))

# Wait
lc_wait = Behavior("lc_wait", 0)
Expand All @@ -63,6 +63,19 @@ def convert_to_ms(speed):
lc_exit = Behavior("lc_exit", -1) # Get SpeedLimit dynamically


# Overtake

# Approach
ot_app_blocked = Behavior("ot_app_blocked", -2)
ot_app_free = Behavior("ot_app_free", -1)
# Wait
ot_wait_stopped = Behavior("ot_wait_stopped", convert_to_ms(0.0))
ot_wait_free = Behavior("ot_wait_free", -1)
# Enter
ot_enter_init = Behavior("ot_enter_init", -1)
ot_enter_slow = Behavior("ot_enter_slow", -2)
# Exit
ot_leave = Behavior("ot_leave", -1)
# Cruise

cruise = Behavior("Cruise", -1)
Loading

0 comments on commit 7f5c521

Please sign in to comment.