diff --git a/.gitignore b/.gitignore index bf3c7859..b667dead 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,5 @@ code/output # Byte-compiled / optimized / DLL files __pycache__/ + +*.tsv diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1545f945..34db1e5e 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -26,6 +26,10 @@ services: # image: carlasim/carla:0.9.14 image: ghcr.io/una-auxme/paf23:leaderboard-2.0 init: true + deploy: + resources: + limits: + memory: 16G expose: - 2000 - 2001 diff --git a/code/agent/launch/agent.launch b/code/agent/launch/agent.launch index 4291717f..d076596d 100644 --- a/code/agent/launch/agent.launch +++ b/code/agent/launch/agent.launch @@ -7,7 +7,7 @@ - + diff --git a/code/agent/launch/agent_manual.launch b/code/agent/launch/agent_manual.launch new file mode 100644 index 00000000..32e33eb6 --- /dev/null +++ b/code/agent/launch/agent_manual.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + diff --git a/code/perception/CMakeLists.txt b/code/perception/CMakeLists.txt index 564502a6..70e340c4 100644 --- a/code/perception/CMakeLists.txt +++ b/code/perception/CMakeLists.txt @@ -52,6 +52,7 @@ add_message_files( FILES Waypoint.msg LaneChange.msg + TrafficLightState.msg MinDistance.msg ) diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 1100405d..500635fb 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -2,9 +2,9 @@ - - - @@ -34,7 +34,7 @@ - "Old" (Buggy for demonstration purposes only) --> - + @@ -60,7 +60,13 @@ - deeplabv3_resnet101 - yolov8x-seg --> - + + + + + + + diff --git a/code/perception/msg/TrafficLightState.msg b/code/perception/msg/TrafficLightState.msg new file mode 100644 index 00000000..9da503af --- /dev/null +++ b/code/perception/msg/TrafficLightState.msg @@ -0,0 +1,5 @@ +int8 state +int8 GREEN=1 +int8 YELLOW=4 +int8 RED=2 +int8 UNKNOWN=0 diff --git a/code/perception/src/traffic_light_detection/.gitignore b/code/perception/src/traffic_light_detection/.gitignore index af0e7a09..d55dcb8e 100644 --- a/code/perception/src/traffic_light_detection/.gitignore +++ b/code/perception/src/traffic_light_detection/.gitignore @@ -1,2 +1 @@ /dataset -/models diff --git a/code/perception/src/traffic_light_detection/dataset.dvc b/code/perception/src/traffic_light_detection/dataset.dvc index 73aa6bd3..2f10a027 100644 --- a/code/perception/src/traffic_light_detection/dataset.dvc +++ b/code/perception/src/traffic_light_detection/dataset.dvc @@ -1,5 +1,5 @@ outs: -- md5: 3a559397ebc58c1ecf142dea18d03367.dir +- md5: 86f14bb96e1ac5735051b8e873f07a9f.dir size: 13745063 nfiles: 2723 hash: md5 diff --git a/code/perception/src/traffic_light_detection/dvc.lock b/code/perception/src/traffic_light_detection/dvc.lock index d9f625ce..6438d251 100644 --- a/code/perception/src/traffic_light_detection/dvc.lock +++ b/code/perception/src/traffic_light_detection/dvc.lock @@ -4,31 +4,32 @@ stages: cmd: python src/traffic_light_detection/traffic_light_training.py deps: - path: dataset - md5: 3a559397ebc58c1ecf142dea18d03367.dir + hash: md5 + md5: 86f14bb96e1ac5735051b8e873f07a9f.dir size: 13745063 nfiles: 2723 - path: src hash: md5 - md5: b6c9cb867c89ad6e86403d9c33538136.dir - size: 23777 - nfiles: 10 + md5: 34c981a61e886a858d135daee17a82ba.dir + size: 35849 + nfiles: 17 params: params.yaml: train: - epochs: 100 + epochs: 20 batch_size: 32 outs: - path: dvclive/metrics.json hash: md5 - md5: af33de699558fbfd3edee1607ba88f81 - size: 218 + md5: 8566265bcdc76cb55d17230f82fc1517 + size: 219 - path: dvclive/plots hash: md5 - md5: 774919de9e9d6820ac6821d0819829c1.dir - size: 8900 + md5: f8f99f42fc42e0ed3c80c8e8f05c1528.dir + size: 8870 nfiles: 4 - path: models hash: md5 - md5: ee67bac2f189d2cc5a199d91ba3295ac.dir - size: 10815 + md5: 16f96ecc475d20123051719908af9d4d.dir + size: 11071 nfiles: 1 diff --git a/code/perception/src/traffic_light_detection/dvclive/metrics.json b/code/perception/src/traffic_light_detection/dvclive/metrics.json new file mode 100644 index 00000000..52d49ebd --- /dev/null +++ b/code/perception/src/traffic_light_detection/dvclive/metrics.json @@ -0,0 +1,11 @@ +{ + "train": { + "accuracy": 92.26495726495727, + "loss": 0.03128106306251298 + }, + "validation": { + "accuracy": 91.36125654450262, + "loss": 0.031184170882739323 + }, + "step": 99 +} diff --git a/code/perception/src/traffic_light_detection/models/model_acc_92.48_val_91.88.pt b/code/perception/src/traffic_light_detection/models/model_acc_92.48_val_91.88.pt new file mode 100644 index 00000000..584f75f5 Binary files /dev/null and b/code/perception/src/traffic_light_detection/models/model_acc_92.48_val_91.88.pt differ diff --git a/code/perception/src/traffic_light_detection/params.yaml b/code/perception/src/traffic_light_detection/params.yaml index 7b90494c..efce6997 100644 --- a/code/perception/src/traffic_light_detection/params.yaml +++ b/code/perception/src/traffic_light_detection/params.yaml @@ -1,3 +1,3 @@ train: - epochs: 100 + epochs: 20 batch_size: 32 diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_config.py b/code/perception/src/traffic_light_detection/src/traffic_light_config.py index 6187e901..e1c720c2 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_config.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_config.py @@ -17,7 +17,7 @@ def __init__(self): # Amount of epochs to train # One epoch: Training with all images from training dataset once self.NUM_WORKERS = 4 - self.NUM_CLASSES = 4 # Traffic light states: green, yellow, red, back + self.NUM_CLASSES = 5 # States: green, yellow, red, back, side self.NUM_CHANNELS = 3 # RGB encoded images # Inference diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py index 4631660b..ada59c5f 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py @@ -2,11 +2,12 @@ import torch.cuda import torchvision.transforms as t -from data_generation.transforms import Normalize, ResizeAndPadToSquare, \ - load_image -from traffic_light_detection.classification_model import ClassificationModel +from traffic_light_detection.src.traffic_light_detection.transforms \ + import Normalize, ResizeAndPadToSquare, load_image +from traffic_light_detection.src.traffic_light_detection.classification_model \ + import ClassificationModel from torchvision.transforms import ToTensor -from traffic_light_config import TrafficLightConfig +from traffic_light_detection.src.traffic_light_config import TrafficLightConfig def parse_args(): @@ -49,7 +50,8 @@ def __init__(self, model_path): self.class_dict = {0: 'Backside', 1: 'Green', 2: 'Red', - 3: 'Yellow'} + 3: 'Side', + 4: 'Yellow'} def __call__(self, img): """ diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py index 7f15e3c9..1322d024 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py @@ -11,8 +11,8 @@ import sys import os sys.path.append(os.path.abspath(sys.path[0] + '/..')) -from data_generation.transforms import Normalize, ResizeAndPadToSquare, \ - load_image # noqa: E402 +from traffic_light_detection.transforms import Normalize, \ + ResizeAndPadToSquare, load_image # noqa: E402 from data_generation.weights_organizer import WeightsOrganizer # noqa: E402 from traffic_light_detection.classification_model import ClassificationModel \ # noqa: E402 diff --git a/code/perception/src/traffic_light_detection/src/data_generation/transforms.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/transforms.py similarity index 100% rename from code/perception/src/traffic_light_detection/src/data_generation/transforms.py rename to code/perception/src/traffic_light_detection/src/traffic_light_detection/transforms.py diff --git a/code/perception/src/traffic_light_node.py b/code/perception/src/traffic_light_node.py new file mode 100755 index 00000000..6f67b5b1 --- /dev/null +++ b/code/perception/src/traffic_light_node.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 + +from ros_compatibility.node import CompatibleNode +import ros_compatibility as roscomp +from rospy.numpy_msg import numpy_msg +from sensor_msgs.msg import Image as ImageMsg +from perception.msg import TrafficLightState +from cv_bridge import CvBridge +from traffic_light_detection.src.traffic_light_detection.traffic_light_inference \ + import TrafficLightInference # noqa: E501 + + +class TrafficLightNode(CompatibleNode): + def __init__(self, name, **kwargs): + super().__init__(name, **kwargs) + # general setup + self.bridge = CvBridge() + self.role_name = self.get_param("role_name", "hero") + self.side = self.get_param("side", "Center") + self.classifier = TrafficLightInference(self.get_param("model", "")) + + # publish / subscribe setup + self.setup_camera_subscriptions() + self.setup_traffic_light_publishers() + + def setup_camera_subscriptions(self): + self.new_subscription( + msg_type=numpy_msg(ImageMsg), + callback=self.handle_camera_image, + topic=f"/paf/{self.role_name}/{self.side}/segmented_traffic_light", + qos_profile=1 + ) + + def setup_traffic_light_publishers(self): + self.traffic_light_publisher = self.new_publisher( + msg_type=TrafficLightState, + topic=f"/paf/{self.role_name}/{self.side}/traffic_light_state", + qos_profile=1 + ) + + def handle_camera_image(self, image): + result = self.classifier(self.bridge.imgmsg_to_cv2(image)) + + # 1: Green, 2: Red, 4: Yellow, 0: Unknown + msg = TrafficLightState() + msg.state = result if result in [1, 2, 4] else 0 + + self.traffic_light_publisher.publish(msg) + + def run(self): + self.spin() + + +if __name__ == "__main__": + roscomp.init("TrafficLightNode") + node = TrafficLightNode("TrafficLightNode") + node.run() diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index 06dab72a..e9681a4c 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -85,6 +85,7 @@ def __init__(self, name, **kwargs): # publish / subscribe setup self.setup_camera_subscriptions() self.setup_camera_publishers() + self.setup_traffic_light_publishers() self.image_msg_header = Header() self.image_msg_header.frame_id = "segmented_image_frame" @@ -127,6 +128,13 @@ def setup_camera_publishers(self): qos_profile=1 ) + def setup_traffic_light_publishers(self): + self.traffic_light_publisher = self.new_publisher( + msg_type=numpy_msg(ImageMsg), + topic=f"/paf/{self.role_name}/{self.side}/segmented_traffic_light", + qos_profile=1 + ) + def handle_camera_image(self, image): # free up cuda memory if self.device == "cuda": @@ -140,12 +148,10 @@ def handle_camera_image(self, image): # publish image to rviz img_msg = self.bridge.cv2_to_imgmsg(vision_result, - encoding="passthrough") + encoding="rgb8") img_msg.header = image.header self.publisher.publish(img_msg) - pass - def predict_torch(self, image): self.model.eval() cv_image = self.bridge.imgmsg_to_cv2(img_msg=image, @@ -174,10 +180,35 @@ def predict_ultralytics(self, image): desired_encoding='passthrough') cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) - output = self.model(cv_image) + output = self.model(cv_image, half=True, verbose=False) + + if 9 in output[0].boxes.cls: + self.process_traffic_lights(output[0], cv_image, image.header) return output[0].plot() + 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 + + 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: + continue + + box = box[0:4].astype(int) + segmented = cv_image[box[1]:box[3], box[0]:box[2]] + + traffic_light_image = self.bridge.cv2_to_imgmsg(segmented, + encoding="rgb8") + traffic_light_image.header = image_header + self.traffic_light_publisher.publish(traffic_light_image) + def create_mask(self, input_image, model_output): output_predictions = torch.argmax(model_output, dim=0) for i in range(21): diff --git a/code/planning/local_planner/CMakeLists.txt b/code/planning/CMakeLists.txt old mode 100644 new mode 100755 similarity index 93% rename from code/planning/local_planner/CMakeLists.txt rename to code/planning/CMakeLists.txt index eda4abbe..34c3ccd0 --- a/code/planning/local_planner/CMakeLists.txt +++ b/code/planning/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(local_planner) +project(planning) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -7,18 +7,21 @@ project(local_planner) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) find_package(catkin REQUIRED COMPONENTS perception + rospy + roslaunch + std_msgs ) +roslaunch_add_file_check(launch) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## @@ -102,7 +105,7 @@ catkin_package( # LIBRARIES planning # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib - CATKIN_DEPENDS perception + CATKIN_DEPENDS perception rospy ) ########### @@ -113,7 +116,7 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( # include -# ${catkin_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library @@ -200,3 +203,12 @@ include_directories( ## Add folders to be run by python nosetests # catkin_add_nosetests(test) + +# catkin_install_python( +# PROGRAMS +# src/behavior_agent/behavior_tree.py +# src/behavior_agent/__init__.py +# DESTINATION +# ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) +install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) \ No newline at end of file diff --git a/code/planning/behavior_agent/src/behavior_agent/__init__.py b/code/planning/__init__.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/__init__.py rename to code/planning/__init__.py diff --git a/code/planning/behavior_agent/CMakeLists.txt b/code/planning/behavior_agent/CMakeLists.txt deleted file mode 100644 index 7d505925..00000000 --- a/code/planning/behavior_agent/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(behavior_agent) - -find_package(catkin REQUIRED COMPONENTS rospy roslaunch) - -catkin_python_setup() - -roslaunch_add_file_check(launch) - -catkin_package(CATKIN_DEPENDS rospy) - -catkin_install_python( - PROGRAMS - src/behavior_agent/behavior_tree.py - src/behavior_agent/__init__.py - DESTINATION - ${CATKIN_PACKAGE_BIN_DESTINATION} -) - - -install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) diff --git a/code/planning/behavior_agent/launch/behavior_agent.launch b/code/planning/behavior_agent/launch/behavior_agent.launch deleted file mode 100644 index 6be8134a..00000000 --- a/code/planning/behavior_agent/launch/behavior_agent.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/code/planning/behavior_agent/package.xml b/code/planning/behavior_agent/package.xml deleted file mode 100644 index 373ad321..00000000 --- a/code/planning/behavior_agent/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - behavior_agent - 0.0.0 - The decision tree - We - MIT - catkin - rospy - roslaunch - rospy - rospy - carla_msgs - sensor_msgs - std_msgs - - - diff --git a/code/planning/behavior_agent/readme.md b/code/planning/behavior_agent/readme.md deleted file mode 100644 index 9d57917f..00000000 --- a/code/planning/behavior_agent/readme.md +++ /dev/null @@ -1,190 +0,0 @@ -[//]: # () -[//]: # (""") - -[//]: # (Source: https://github.com/ll7/psaf2) - -[//]: # (""") - -# behaviour_agent - -**Disclaimer**: As we mainly built our decision tree on the previous [PAF project](https://github.com/ll7/psaf2), most part of the documentation was added here and adjusted to the changes we made. - -## About - -This Package implements a behaviour agent for our autonomous car using **Behaviour Trees**. It uses the `py_trees` Framework, that works well with ROS. -For visualization at runtime you might want to also install this [rqt-Plugin](https://wiki.ros.org/rqt_py_trees). - -## Our behaviour tree - -The following section describes the behaviour tree we use for normal driving using all functionality provided by the agent. In the actual implementation this is part of a bigger tree, that handles things like writing topics to the blackboard, starting and finishing the decision tree. -The following description is not complete, it just contains the most common behaviours and subtrees. For a complete description have a look at the [tree-description](../../../doc/07_planning/behaviortree.xml) and the [bt-specs](../../../doc/07_planning/behavior_tree_spec.md). -Note that we didn't actually implement all the behaviours from this design, due to time and functionality limitations. - -### Legend - -The following notation is used in this documentation: - -![BT Legend](../../../doc/00_assets/legend_bt.png) - -#### Behavior - -Represent an action the decision tree should execute. It has three return values representing the state of the behavior: - -* `SUCCESS`: The action has been performed successfully. -* `RUNNING`: The action is still being executed. -* `Failure`: The action couldn't be executed. - -#### Selector - -Tries to execute each of its child behaviors in turn. It has a priority hierarchy from left (high priority) to right (low priority). - -#### Sequence - -Executes all of its child behaviors sequentially after one another until all behaviors have returned `SUCCESS` or one behavior returned `FAILURE`. In that case, the sequence is aborted. - -#### Condition - -Is always the first child of a sequence. It decides if the sequence should be executed or aborted. - -#### Subtree - -Represents a specific task/scenario which is handled by the decision tree. - -### Big Picture - -![BT Big Picture](../../../doc/00_assets/top-level.png) - -This top-level tree consists mainly of subtrees that are explained below. If none of the subtrees fit the current situation, the behaviour_agent goes into `Cruising`-behaviour, where it just follows the Path at an appropriate speed. - -### Intersection - -![BT Intersection](../../../doc/00_assets/intersection.png) - -If there is an intersection coming up, the agent executes the following sequence of behaviours: - -* Approach Intersection - - Slows down and stops at line if a stop sign or a yellow or red traffic light is detected - -* Wait at Intersection - - Waits for traffic lights or higher priority traffic - -* Enter Intersection - - Enters the intersection and follows it predetermined path through the intersection - -* Leave Intersection - - Leaves the intersection in the right direction - -### Overtaking - -The Overtaking subtree is quite big to accommodate for different overtaking scenarios. Here is an overview of that subtree further refining it. - -![Overtaking](../../../doc/00_assets/overtaking_overview.png) - -Please have a look at the [tree-description](../../../doc/07_planning/behaviortree.xml) and the [bt-specs](../../../doc/07_planning/behavior_tree_spec.md) for a more detailed description. The Multi-Lane Overtaking Subtree looks like this: - -![BT Overtaking](../../../doc/00_assets/multi_lane.png) - -* Multi Lane? - - Checks the map data: does the current road have more than one lane? - -* Left Lane available? - - The lane detection checks if there is a lane to the left - -* Wait for Left Lane free - - Waits for the left lane to be free. This has a timeout. - -* Switch Lane Left - - Triggers a lane switch to the left by calling the local planner - -### Right-Hand Driving - -This subtree makes the ego vehicle switch back to the right lane, if the road ahead is free enough. It is quite similar to the Multi-Lane Overtaking Subtree, just with reversed directions. - -![BT Right Hand](../../../doc/00_assets/Right_lane.png) - -## Developing guide - -### Tree Definition - -The tree is defined in the `grow_a_tree()`-function inside `code/planning/behavior_agent/behavior_tree.py`, which is also the main node. It can be visualized using an [rqt-Plugin](https://wiki.ros.org/rqt_py_trees). This is also the place to change the execution rate of the tree: - -``` python -... -behaviour_tree.tick_tock(500) -... -``` - -### Behaviours - -`Behaviours` are implemented in the `code/planning/behavior_agent/behaviours/` directory. All the behaviours used in the current version of the tree are contained as skeletons. - -#### Blackboard - -To deal with the asynchronicity of ROS, all the topics this tree subscribes to, should be written to the Blackboard at the beginning of each tick. A node is available, that automates this task. Just add your node to the list in `src/behavior_agent/behaviours/topics2blackboard.py`: - -``` python -... -topics =[ - {'name':f"/carla/{role_name}/Speed", 'msg':CarlaSpeedoMeter, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - ... - ] -... -``` - -After that you can access them from everywhere in your Behaviour-Code using: - -``` python -... -self.blackboard = py_trees.blackboard.Blackboard() -... -speed = self.blackboard.get("/carla/hero/Speed") -... -``` - -Note that you still need to resolve the data-fields of the message (i.e. `blackboardmessage.data` for a `Float64`). - -### Guidelines - -When implementing new behaviours you should adhere to the following guidelines: - -#### Non-Blocking - -You should avoid doing complicated calculations inside the behaviours. Use asynchronous Callbacks instead, and return ```RUNNING``` while another node does the computing. - -Generally conditions should never return ```RUNNING``` and Action-Behaviours should only return ```FAILURE``` in special cases. - -#### Functions - -Behaviours generally provide five functions (you can define more of course). Short explanation when they get called and how to use them: - -##### `__init__()` - -You should probably never use this. - -##### `setup()` - -Gets called whenever the tree gets set up for the first time. Use this to setup local variables that don't need to change, like ```self.blackboard = py_trees.blackboard.Blackboard()``` or middleware like ROS-Publishers (Subscribers should be setup using the method mentioned above). - -##### `initialise()` - -Gets called every time the behaviour is entered for a new execution. Add code that only needs to be called once at the beginning of a behaviour (i.e. publishing a new target speed). - -##### `update()` - -Main function of a behaviour, that gets called everytime the behaviour is ticked. Here you need to return ```SUCCESS```, ```RUNNING``` or ```FAILURE```. - -##### `terminate()` - -This gets called, whenever a behaviour is cancelled by a higher priority branch. Use to terminate middleware connections or asynchronous Calculations, whose results are not needed anymore. - -## Authors - -Josef Kircher diff --git a/code/planning/behavior_agent/setup.py b/code/planning/behavior_agent/setup.py deleted file mode 100755 index 4ba971a2..00000000 --- a/code/planning/behavior_agent/setup.py +++ /dev/null @@ -1,15 +0,0 @@ -""" -Source: https://github.com/ll7/psaf2 - -behavior_agent -""" - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['behavior_agent'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py deleted file mode 100755 index f8984b65..00000000 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -from . import topics2blackboard, road_features -from . import intersection, traffic_objects, maneuvers, meta, lane_change \ No newline at end of file diff --git a/code/planning/global_planner/CMakeLists.txt b/code/planning/global_planner/CMakeLists.txt deleted file mode 100644 index 3344bc47..00000000 --- a/code/planning/global_planner/CMakeLists.txt +++ /dev/null @@ -1,202 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(planning) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES planning -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include -# ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/planning.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/planning_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_planning.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch deleted file mode 100644 index 398c35ce..00000000 --- a/code/planning/global_planner/launch/global_planner.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/code/planning/global_planner/package.xml b/code/planning/global_planner/package.xml deleted file mode 100644 index 9a2f8896..00000000 --- a/code/planning/global_planner/package.xml +++ /dev/null @@ -1,59 +0,0 @@ - - - planning - 0.0.0 - The planning package - - - - - carla - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - - - - - - - - diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch new file mode 100644 index 00000000..c6377c79 --- /dev/null +++ b/code/planning/launch/planning.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch deleted file mode 100644 index 5fb371a5..00000000 --- a/code/planning/local_planner/launch/local_planner.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/code/planning/local_planner/setup.py b/code/planning/local_planner/setup.py deleted file mode 100644 index e5a88c1c..00000000 --- a/code/planning/local_planner/setup.py +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -setup_args = generate_distutils_setup(packages=['local_planner'], - package_dir={'': 'src'}) -setup(**setup_args) diff --git a/code/planning/local_planner/package.xml b/code/planning/package.xml old mode 100644 new mode 100755 similarity index 87% rename from code/planning/local_planner/package.xml rename to code/planning/package.xml index a1e4978c..a321a109 --- a/code/planning/local_planner/package.xml +++ b/code/planning/package.xml @@ -1,8 +1,8 @@ - local_planner + planning 0.0.0 - The local planning package + The planning package @@ -47,9 +47,20 @@ - perception + rospy + roslaunch + + rospy perception + + rospy + carla_msgs + sensor_msgs + std_msgs perception + + perception + catkin diff --git a/code/planning/planning_runner/CMakeLists.txt b/code/planning/planning_runner/CMakeLists.txt deleted file mode 100644 index 5111471e..00000000 --- a/code/planning/planning_runner/CMakeLists.txt +++ /dev/null @@ -1,205 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(planning_runner) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - std_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES planning_runner -# CATKIN_DEPENDS rospy std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/planning_runner.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/planning_runner_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_planning_runner.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/code/planning/planning_runner/launch/planning_runner.launch b/code/planning/planning_runner/launch/planning_runner.launch deleted file mode 100755 index b5597b71..00000000 --- a/code/planning/planning_runner/launch/planning_runner.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/code/planning/planning_runner/package.xml b/code/planning/planning_runner/package.xml deleted file mode 100644 index 4efbae8d..00000000 --- a/code/planning/planning_runner/package.xml +++ /dev/null @@ -1,65 +0,0 @@ - - - planning_runner - 0.0.0 - The planning_runner package - - - - - imech039 - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - std_msgs - rospy - std_msgs - rospy - std_msgs - - - - - - - - diff --git a/code/planning/global_planner/setup.py b/code/planning/setup.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/global_planner/setup.py rename to code/planning/setup.py diff --git a/code/planning/global_planner/__init__.py b/code/planning/src/behavior_agent/__init__.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/global_planner/__init__.py rename to code/planning/src/behavior_agent/__init__.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py similarity index 94% rename from code/planning/behavior_agent/src/behavior_agent/behavior_tree.py rename to code/planning/src/behavior_agent/behavior_tree.py index 1ac702c1..cacf8619 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -1,21 +1,19 @@ #!/usr/bin/env python import functools -import behavior_agent -import py_trees from py_trees.behaviours import Running import py_trees_ros -import py_trees.console as console import rospy import sys -from behavior_agent import behaviours +import behaviours from py_trees.composites import Parallel, Selector, Sequence -from py_trees.decorators import Inverter """ Source: https://github.com/ll7/psaf2 """ +# flake8: noqa: E501 + def grow_a_tree(role_name): @@ -61,7 +59,7 @@ def grow_a_tree(role_name): ("Leave Change") ]) ]), - + ]), behaviours.maneuvers.Cruise("Cruise") ]) @@ -92,7 +90,7 @@ def main(): rospy.on_shutdown(functools.partial(shutdown, behaviour_tree)) if not behaviour_tree.setup(timeout=15): - rospy.loginfo("Tree Setup failed") + rospy.logerr("Tree Setup failed") sys.exit(1) rospy.loginfo("tree setup worked") r = rospy.Rate(5) diff --git a/code/planning/local_planner/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/local_planner/__init__.py rename to code/planning/src/behavior_agent/behaviours/__init__.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py rename to code/planning/src/behavior_agent/behaviours/behavior_speed.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py b/code/planning/src/behavior_agent/behaviours/intersection.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py rename to code/planning/src/behavior_agent/behaviours/intersection.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py b/code/planning/src/behavior_agent/behaviours/lane_change.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py rename to code/planning/src/behavior_agent/behaviours/lane_change.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py rename to code/planning/src/behavior_agent/behaviours/maneuvers.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/meta.py b/code/planning/src/behavior_agent/behaviours/meta.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/meta.py rename to code/planning/src/behavior_agent/behaviours/meta.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/road_features.py b/code/planning/src/behavior_agent/behaviours/road_features.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/road_features.py rename to code/planning/src/behavior_agent/behaviours/road_features.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/topics2blackboard.py rename to code/planning/src/behavior_agent/behaviours/topics2blackboard.py diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/traffic_objects.py b/code/planning/src/behavior_agent/behaviours/traffic_objects.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/traffic_objects.py rename to code/planning/src/behavior_agent/behaviours/traffic_objects.py diff --git a/code/planning/global_planner/src/dev_global_route.py b/code/planning/src/global_planner/dev_global_route.py similarity index 100% rename from code/planning/global_planner/src/dev_global_route.py rename to code/planning/src/global_planner/dev_global_route.py diff --git a/code/planning/global_planner/src/global_planner.py b/code/planning/src/global_planner/global_planner.py similarity index 99% rename from code/planning/global_planner/src/global_planner.py rename to code/planning/src/global_planner/global_planner.py index fdc542d6..a113c787 100755 --- a/code/planning/global_planner/src/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -77,7 +77,7 @@ def __init__(self): msg_type=Float32MultiArray, topic=f"/paf/{self.role_name}/speed_limits_OpenDrive", qos_profile=1) - self.loginfo('PrePlanner-Node started') + self.logdebug('PrePlanner-Node started') def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/global_planner/src/global_route.txt b/code/planning/src/global_planner/global_route.txt old mode 100644 new mode 100755 similarity index 100% rename from code/planning/global_planner/src/global_route.txt rename to code/planning/src/global_planner/global_route.txt diff --git a/code/planning/global_planner/src/help_functions.py b/code/planning/src/global_planner/help_functions.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/global_planner/src/help_functions.py rename to code/planning/src/global_planner/help_functions.py diff --git a/code/planning/global_planner/src/preplanning_trajectory.py b/code/planning/src/global_planner/preplanning_trajectory.py old mode 100644 new mode 100755 similarity index 100% rename from code/planning/global_planner/src/preplanning_trajectory.py rename to code/planning/src/global_planner/preplanning_trajectory.py diff --git a/code/planning/local_planner/src/ACC.py b/code/planning/src/local_planner/ACC.py similarity index 99% rename from code/planning/local_planner/src/ACC.py rename to code/planning/src/local_planner/ACC.py index d13357fa..3849dc6a 100755 --- a/code/planning/local_planner/src/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -83,6 +83,8 @@ def __init__(self): # Current speed limit self.speed_limit: float = None # m/s + self.logdebug("ACC initialized") + def _set_distance(self, data: MinDistance): """Get min distance to object in front from perception diff --git a/code/planning/local_planner/src/behavior_speed.py b/code/planning/src/local_planner/behavior_speed.py similarity index 100% rename from code/planning/local_planner/src/behavior_speed.py rename to code/planning/src/local_planner/behavior_speed.py diff --git a/code/planning/local_planner/src/collision_check.py b/code/planning/src/local_planner/collision_check.py similarity index 98% rename from code/planning/local_planner/src/collision_check.py rename to code/planning/src/local_planner/collision_check.py index 5fe0d95a..3cc107d3 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -23,9 +23,6 @@ def __init__(self): super(CollisionCheck, self).__init__('CollisionCheck') self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", 1) - # self.current_speed = 50 / 3.6 # m/ss - # TODO: Add Subscriber for Speed and Obstacles - self.loginfo("CollisionCheck started") # self.obstacle_sub: Subscriber = self.new_subscription( # ) @@ -60,6 +57,7 @@ def __init__(self): # Variables to save vehicle data self.__current_velocity: float = None self.__object_last_position: tuple = None + self.logdebug("CollisionCheck started") def calculate_obstacle_speed(self, new_dist: MinDistance): """Caluclate the speed of the obstacle in front of the ego vehicle diff --git a/code/planning/local_planner/src/dev_collision_publisher.py b/code/planning/src/local_planner/dev_collision_publisher.py similarity index 100% rename from code/planning/local_planner/src/dev_collision_publisher.py rename to code/planning/src/local_planner/dev_collision_publisher.py diff --git a/code/planning/local_planner/src/motion_planning.py b/code/planning/src/local_planner/motion_planning.py similarity index 99% rename from code/planning/local_planner/src/motion_planning.py rename to code/planning/src/local_planner/motion_planning.py index 7b1e5982..92949cad 100755 --- a/code/planning/local_planner/src/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -34,7 +34,6 @@ def __init__(self): super(MotionPlanning, self).__init__('MotionPlanning') self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", 0.5) - self.logdebug("MotionPlanning started") self.target_speed = 0.0 self.__curr_behavior = None @@ -73,6 +72,8 @@ def __init__(self): f"/paf/{self.role_name}/target_velocity", qos_profile=1) + self.logdebug("MotionPlanning started") + def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) diff --git a/doc/06_perception/13_traffic_light_detection.md b/doc/06_perception/13_traffic_light_detection.md new file mode 100644 index 00000000..c627e3cf --- /dev/null +++ b/doc/06_perception/13_traffic_light_detection.md @@ -0,0 +1,37 @@ +# Traffic Light Detection + +## Vision Node + +For each analyzed image, it is checked whether an object with the ID=9 (traffic light) is detected. +If that is the case, `process_traffic_lights()` is called which applies the bounding box of the predicition to cut out the found object (e.g. traffic light). + +Only if the object is within `min_x`, `max_x` and `min_prob` (probability), it will be published to `"/paf/{self.role_name}/{self.side}/segmented_traffic_light"`. + +## TrafficLightNode + +The `traffic_light_node.py` file is part of a larger system that handles traffic light detection. It contains a class `TrafficLightNode` that extends from `CompatibleNode`. + +This class is responsible for setting up the traffic light detection system and handling the incoming camera images. + +### Initialization + +The `TrafficLightNode` class is initialized with a name and additional keyword arguments. During initialization, it sets up the following: + +- A `CvBridge` instance for converting between ROS image messages and OpenCV images. +- The role name and side, which are parameters that can be set externally. +- A `TrafficLightInference` instance for performing traffic light detection. + +### Methods + +#### `setup_camera_subscriptions()` + +This method sets up a subscription to the camera images. It subscribes to the topic `"/paf/{self.role_name}/{self.side}/segmented_traffic_light"` and calls the `handle_camera_image` method whenever a new image message is received. + +#### `setup_traffic_light_publishers()` + +This method sets up a publisher for the traffic light state. It publishes to the topic `"/paf/{self.role_name}/{self.side}/traffic_light_state"` in the format of `TrafficLightState.msg` which uses an int8-based enum for the traffic light state. + +#### `handle_camera_image(image)` + +This method is called whenever a new image message is received. It performs traffic light detection by using `traffic_light_inference.py` on the image and publishes the result. +The result is a `TrafficLightState` message where the state is set to the detected traffic light state (1 for green, 2 for red, 4 for yellow, 0 for unknown). diff --git a/doc/06_perception/experiments/object-detection-model_evaluation/yolo.py b/doc/06_perception/experiments/object-detection-model_evaluation/yolo.py index f7ff342d..39d727b7 100644 --- a/doc/06_perception/experiments/object-detection-model_evaluation/yolo.py +++ b/doc/06_perception/experiments/object-detection-model_evaluation/yolo.py @@ -1,5 +1,8 @@ ''' -Docs: https://docs.ultralytics.com/modes/predict/, https://docs.ultralytics.com/tasks/detect/#models, https://docs.ultralytics.com/models/yolo-nas +Docs: +https://docs.ultralytics.com/modes/predict/ +https://docs.ultralytics.com/tasks/detect/#models +https://docs.ultralytics.com/models/yolo-nas ''' import os @@ -35,6 +38,7 @@ image_path = os.path.join(IMAGE_BASE_FOLDER, IMAGES_FOR_TEST[p]) img = Image.open(image_path) - _ = model.predict(source=img, save=True, save_conf=True, line_width=1, half=True) + _ = model.predict(source=img, save=True, save_conf=True, + line_width=1, half=True) del model