Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

159 feature intersection behaviour testing #173

Merged
merged 49 commits into from
Jan 30, 2024
Merged
Show file tree
Hide file tree
Changes from 48 commits
Commits
Show all changes
49 commits
Select commit Hold shift + click to select a range
9ee9ad6
Add publisher
MaxJa4 Dec 12, 2023
c426195
Publish segmented traffic lights
MaxJa4 Dec 12, 2023
420bb04
Implement TrafficLightNode
MaxJa4 Dec 16, 2023
3ecd862
Add TrafficLightState msg. WIP
MaxJa4 Dec 16, 2023
64e438a
Add publisher
MaxJa4 Dec 12, 2023
a6df99d
Publish segmented traffic lights
MaxJa4 Dec 12, 2023
2a18766
Implement TrafficLightNode
MaxJa4 Dec 16, 2023
b5a8a6f
Add TrafficLightState msg. WIP
MaxJa4 Dec 16, 2023
4b3d7fb
Merge branch '110-implement-traffic-light-detection' of https://githu…
MaxJa4 Dec 16, 2023
acb4ffe
Added side view classification
MaxJa4 Dec 16, 2023
810b20c
Finish traffic light node
MaxJa4 Dec 17, 2023
19b10dd
Add manual control launch file
MaxJa4 Dec 17, 2023
33ad2ad
Make linter happy
MaxJa4 Dec 17, 2023
15f7b83
Add documentation
MaxJa4 Dec 17, 2023
be4ab6f
Add missing traffic light detection model
MaxJa4 Dec 18, 2023
2af4521
Fix color issues in rviz
MaxJa4 Dec 18, 2023
4be8614
Limit simulator's max. RAM usage to prevent system crash
MaxJa4 Dec 18, 2023
b7ed596
Merge branch 'main' into 110-implement-traffic-light-detection
MaxJa4 Jan 11, 2024
8f0a76c
Merge remote-tracking branch 'origin/110-implement-traffic-light-dete…
JuliusMiller Jan 12, 2024
abd3821
feat: integrate traffic_light detection
JuliusMiller Jan 12, 2024
73acc16
feat: timeout traffic light status after 3s if no new info available
MaxJa4 Jan 13, 2024
85751ff
fix: smoother approach
JuliusMiller Jan 13, 2024
fd4c32c
Support more intersection types. More robust detection.
MaxJa4 Jan 13, 2024
9922dd3
Update docs about filtering
MaxJa4 Jan 13, 2024
e911442
Update code comment
MaxJa4 Jan 13, 2024
a3288db
Improve detection range
MaxJa4 Jan 14, 2024
bf6bed4
110 - Implement traffic light detection (#148)
MaxJa4 Jan 13, 2024
dd54d22
docs: behavior detailed
JuliusMiller Jan 14, 2024
bf697cb
feat: integrate trajectory planner in docker compose
samuelkuehnel Jan 18, 2024
2de1325
feat: intallation of Frenet tool in docker file
samuelkuehnel Jan 19, 2024
5c91085
feat: integrate trajectory planner in docker compose
samuelkuehnel Jan 18, 2024
314738d
feat: intallation of Frenet tool in docker file
samuelkuehnel Jan 19, 2024
7adcda8
Merge branch '165-feature-local-trajectory-planning-with-frenet-traje…
samuelkuehnel Jan 19, 2024
933a3ac
fix: rivz and behavior tree
samuelkuehnel Jan 19, 2024
736a22b
fix: orientation for short route
JuliusMiller Jan 19, 2024
85b6fb6
Merge branch '159-feature-intersection-behaviour-testing' of https://…
JuliusMiller Jan 19, 2024
ec19c41
Merge remote-tracking branch 'origin/165-feature-local-trajectory-pla…
JuliusMiller Jan 19, 2024
98f4f9f
fix: intersection, lane_change, curve_detection
JuliusMiller Jan 25, 2024
1e03144
fix: linter
JuliusMiller Jan 26, 2024
302e067
fix: linter
JuliusMiller Jan 26, 2024
d315375
fix: change control_loop for target_speed
JuliusMiller Jan 26, 2024
c7b5ac8
Merge remote-tracking branch 'origin/152-feat-traffic-light-detection…
JuliusMiller Jan 26, 2024
cedb764
fix: lane_change behavior
JuliusMiller Jan 26, 2024
0c887a7
fix: add green_light_counter
JuliusMiller Jan 26, 2024
989fb75
fix: update intersection
JuliusMiller Jan 28, 2024
530b4b0
fix: clean lane_change
JuliusMiller Jan 28, 2024
b3efbac
fix: blocked lane change
JuliusMiller Jan 28, 2024
a5f9df7
fix: lidar_distance = 20
JuliusMiller Jan 28, 2024
39348aa
Merge branch 'main' into 159-feature-intersection-behaviour-testing
JuliusMiller Jan 30, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion .flake8
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
[flake8]
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/src/behavior_agent/behaviours,
code/planning/__init__.py
3 changes: 2 additions & 1 deletion build/docker-compose.yml
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,8 @@ services:
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 && 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=carla-simulator --track=MAP"
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:
driver: "local"
environment:
Expand Down
17 changes: 17 additions & 0 deletions build/docker/agent/Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -162,6 +162,21 @@ 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 /erdos
RUN sudo mkdir /erdos/dependencies
RUN sudo mkdir /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

Expand All @@ -170,6 +185,8 @@ RUN source /opt/ros/noetic/setup.bash && catkin_make

ADD ./build/docker/agent/entrypoint.sh /entrypoint.sh



# set the default working directory to the code
WORKDIR /workspace/code

Expand Down
4 changes: 2 additions & 2 deletions code/agent/config/rviz_config.rviz
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ Visualization Manager:
Imu: false
Path: false
PointCloud2: false
Value: false
Value: true
Zoom Factor: 1
- Class: rviz/Image
Enabled: true
Expand Down Expand Up @@ -166,7 +166,7 @@ Visualization Manager:
Offset:
X: 0
Y: 0
Z: 39
Z: 703
Pose Color: 255; 85; 255
Pose Style: None
Queue Size: 10
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ def __call__(self, img):
else:
out = self.model(img)
_, prediction = torch.max(out.data, 1)
return prediction.item()
return (prediction.item(), out.data.cpu().numpy())


# main function for testing purposes
Expand Down
40 changes: 35 additions & 5 deletions code/perception/src/traffic_light_node.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
#!/usr/bin/env python3

from datetime import datetime
import threading
from time import sleep
from ros_compatibility.node import CompatibleNode
import ros_compatibility as roscomp
from rospy.numpy_msg import numpy_msg
Expand All @@ -18,6 +21,9 @@ def __init__(self, name, **kwargs):
self.role_name = self.get_param("role_name", "hero")
self.side = self.get_param("side", "Center")
self.classifier = TrafficLightInference(self.get_param("model", ""))
self.last_info_time: datetime = None
self.last_state = None
threading.Thread(target=self.auto_invalidate_state).start()

# publish / subscribe setup
self.setup_camera_subscriptions()
Expand All @@ -38,14 +44,38 @@ def setup_traffic_light_publishers(self):
qos_profile=1
)

def auto_invalidate_state(self):
while True:
sleep(1)

if self.last_info_time is None:
continue

if (datetime.now() - self.last_info_time).total_seconds() >= 2:
msg = TrafficLightState()
msg.state = 0
self.traffic_light_publisher.publish(msg)
self.last_info_time = None

def handle_camera_image(self, image):
result = self.classifier(self.bridge.imgmsg_to_cv2(image))
result, data = self.classifier(self.bridge.imgmsg_to_cv2(image))

if data[0][0] > 1e-15 and data[0][3] > 1e-15 or \
data[0][0] > 1e-10 or data[0][3] > 1e-10:
return # too uncertain, may not be a traffic light

# 1: Green, 2: Red, 4: Yellow, 0: Unknown
msg = TrafficLightState()
msg.state = result if result in [1, 2, 4] else 0
state = result if result in [1, 2, 4] else 0
if self.last_state == state:
# 1: Green, 2: Red, 4: Yellow, 0: Unknown
msg = TrafficLightState()
msg.state = state
self.traffic_light_publisher.publish(msg)
else:
self.last_state = state

self.traffic_light_publisher.publish(msg)
# Automatically invalidates state (state=0) in auto_invalidate_state()
if state != 0:
self.last_info_time = datetime.now()

def run(self):
self.spin()
Expand Down
13 changes: 9 additions & 4 deletions code/perception/src/vision_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -191,14 +191,19 @@ def process_traffic_lights(self, prediction, cv_image, image_header):
indices = (prediction.boxes.cls == 9).nonzero().squeeze().cpu().numpy()
indices = np.asarray([indices]) if indices.size == 1 else indices

min_x = 550
max_x = 700
min_prob = 0.35
max_y = 360 # middle of image
min_prob = 0.30

for index in indices:
box = prediction.boxes.cpu().data.numpy()[index]

if box[0] < min_x or box[2] > max_x or box[4] < min_prob:
if box[4] < min_prob:
continue

if (box[2] - box[0]) * 1.5 > box[3] - box[1]:
continue # ignore horizontal boxes

if box[1] > max_y:
continue

box = box[0:4].astype(int)
Expand Down
1 change: 1 addition & 0 deletions code/planning/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
import behavior_agent
3 changes: 3 additions & 0 deletions code/planning/src/behavior_agent/behaviours/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
from . import intersection, lane_change, maneuvers, meta, road_features
from . import topics2blackboard, traffic_objects
from . import behavior_speed
15 changes: 7 additions & 8 deletions code/planning/src/behavior_agent/behaviours/behavior_speed.py
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ def convert_to_ms(speed):
int_app_init = Behavior("int_app_init", convert_to_ms(30.0))

# No Traffic Light or Sign -> stop dynamically at Stopline
int_app_no_sign = Behavior("int_app_no_sign", -2)
int_app_to_stop = Behavior("int_app_to_stop", -2)

int_app_green = Behavior("int_app_green", convert_to_ms(30.0))

Expand All @@ -30,11 +30,7 @@ def convert_to_ms(speed):

# Enter

int_enter_no_light = Behavior("int_enter_no_light", convert_to_ms(50.0))

int_enter_empty_str = Behavior("int_enter_empty_string", convert_to_ms(18.0))

int_enter_light = Behavior("int_enter_light", convert_to_ms(50.0))
int_enter = Behavior("int_enter", convert_to_ms(50.0))

# Exit

Expand All @@ -45,13 +41,16 @@ def convert_to_ms(speed):

# Approach

lc_app_init = Behavior("lc_app_blocked", convert_to_ms(30.0))
lc_app_init = Behavior("lc_app_init", convert_to_ms(30.0))


# TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128)
lc_app_blocked = Behavior("lc_app_blocked", 0.5)
lc_app_blocked = Behavior("lc_app_blocked", -2)

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

# Wait
lc_wait = Behavior("lc_wait", 0)

# Has a publisher but doesnt publish anything ??

Expand Down
Loading
Loading