From 8f16333880f829e3a481df794f9b8427c6b682eb Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 21 Dec 2023 16:35:32 +0100 Subject: [PATCH 1/5] feat: restructure planning package --- build/docker-compose.yml | 4 +- code/agent/launch/agent.launch | 2 +- .../{local_planner => }/CMakeLists.txt | 22 +- .../src/behavior_agent => }/__init__.py | 0 code/planning/behavior_agent/CMakeLists.txt | 21 -- .../launch/behavior_agent.launch | 8 - code/planning/behavior_agent/package.xml | 18 -- code/planning/behavior_agent/readme.md | 190 ---------------- code/planning/behavior_agent/setup.py | 15 -- code/planning/global_planner/CMakeLists.txt | 202 ----------------- .../launch/global_planner.launch | 15 -- code/planning/global_planner/package.xml | 59 ----- code/planning/launch/planning.launch | 35 +++ code/planning/local_planner/__init__.py | 0 .../local_planner/launch/local_planner.launch | 18 -- code/planning/local_planner/setup.py | 7 - code/planning/{local_planner => }/package.xml | 17 +- code/planning/planning_runner/CMakeLists.txt | 205 ------------------ .../launch/planning_runner.launch | 7 - code/planning/planning_runner/package.xml | 65 ------ code/planning/{global_planner => }/setup.py | 0 .../behavior_agent}/__init__.py | 0 .../src/behavior_agent/behavior_tree.py | 2 +- .../src/behavior_agent/behaviours/__init__.py | 0 .../behaviours/behavior_speed.py | 0 .../behavior_agent/behaviours/intersection.py | 0 .../behavior_agent/behaviours/lane_change.py | 0 .../behavior_agent/behaviours/maneuvers.py | 0 .../src/behavior_agent/behaviours/meta.py | 0 .../behaviours/road_features.py | 0 .../behaviours/topics2blackboard.py | 0 .../behaviours/traffic_objects.py | 0 .../global_planner}/dev_global_route.py | 0 .../global_planner}/global_planner.py | 2 +- .../global_planner}/global_route.txt | 0 .../global_planner}/help_functions.py | 0 .../global_planner}/preplanning_trajectory.py | 0 .../src => src/local_planner}/ACC.py | 2 + .../local_planner}/behavior_speed.py | 0 .../local_planner}/collision_check.py | 4 +- .../local_planner}/dev_collision_publisher.py | 0 .../local_planner}/motion_planning.py | 2 +- 42 files changed, 75 insertions(+), 847 deletions(-) rename code/planning/{local_planner => }/CMakeLists.txt (93%) mode change 100644 => 100755 rename code/planning/{behavior_agent/src/behavior_agent => }/__init__.py (100%) mode change 100644 => 100755 delete mode 100644 code/planning/behavior_agent/CMakeLists.txt delete mode 100644 code/planning/behavior_agent/launch/behavior_agent.launch delete mode 100644 code/planning/behavior_agent/package.xml delete mode 100644 code/planning/behavior_agent/readme.md delete mode 100755 code/planning/behavior_agent/setup.py delete mode 100644 code/planning/global_planner/CMakeLists.txt delete mode 100644 code/planning/global_planner/launch/global_planner.launch delete mode 100644 code/planning/global_planner/package.xml create mode 100644 code/planning/launch/planning.launch delete mode 100644 code/planning/local_planner/__init__.py delete mode 100644 code/planning/local_planner/launch/local_planner.launch delete mode 100644 code/planning/local_planner/setup.py rename code/planning/{local_planner => }/package.xml (87%) mode change 100644 => 100755 delete mode 100644 code/planning/planning_runner/CMakeLists.txt delete mode 100755 code/planning/planning_runner/launch/planning_runner.launch delete mode 100644 code/planning/planning_runner/package.xml rename code/planning/{global_planner => }/setup.py (100%) mode change 100644 => 100755 rename code/planning/{global_planner => src/behavior_agent}/__init__.py (100%) mode change 100644 => 100755 rename code/planning/{behavior_agent => }/src/behavior_agent/behavior_tree.py (98%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/__init__.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/behavior_speed.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/intersection.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/lane_change.py (100%) mode change 100644 => 100755 rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/maneuvers.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/meta.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/road_features.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/topics2blackboard.py (100%) rename code/planning/{behavior_agent => }/src/behavior_agent/behaviours/traffic_objects.py (100%) rename code/planning/{global_planner/src => src/global_planner}/dev_global_route.py (100%) rename code/planning/{global_planner/src => src/global_planner}/global_planner.py (99%) rename code/planning/{global_planner/src => src/global_planner}/global_route.txt (100%) mode change 100644 => 100755 rename code/planning/{global_planner/src => src/global_planner}/help_functions.py (100%) mode change 100644 => 100755 rename code/planning/{global_planner/src => src/global_planner}/preplanning_trajectory.py (100%) mode change 100644 => 100755 rename code/planning/{local_planner/src => src/local_planner}/ACC.py (99%) rename code/planning/{local_planner/src => src/local_planner}/behavior_speed.py (100%) rename code/planning/{local_planner/src => src/local_planner}/collision_check.py (98%) rename code/planning/{local_planner/src => src/local_planner}/dev_collision_publisher.py (100%) rename code/planning/{local_planner/src => src/local_planner}/motion_planning.py (99%) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index a44d725e..078d97bc 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -57,8 +57,8 @@ 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 && 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 && 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" logging: driver: "local" 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/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..4997236d --- 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/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 cdb6dec9..00000000 --- a/code/planning/global_planner/launch/global_planner.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - 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/__init__.py b/code/planning/local_planner/__init__.py deleted file mode 100644 index e69de29b..00000000 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 98% rename from code/planning/behavior_agent/src/behavior_agent/behavior_tree.py rename to code/planning/src/behavior_agent/behavior_tree.py index 1ac702c1..480abba9 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -92,7 +92,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/behavior_agent/src/behavior_agent/behaviours/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py similarity index 100% rename from code/planning/behavior_agent/src/behavior_agent/behaviours/__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..e61bd0fd 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.logerr('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..6a19518d 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.logerr("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..8bea2f82 100755 --- a/code/planning/local_planner/src/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -23,9 +23,7 @@ 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.logerr("CollisionCheck started") # self.obstacle_sub: Subscriber = self.new_subscription( # ) 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..3f969b2d 100755 --- a/code/planning/local_planner/src/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -34,7 +34,7 @@ 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.logerr("MotionPlanning started") self.target_speed = 0.0 self.__curr_behavior = None From f5aab0dc1e445ab112bc7ba0db02d3b8a0d4eb5c Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 21 Dec 2023 17:12:06 +0100 Subject: [PATCH 2/5] fix: startup behavior agent --- code/planning/CMakeLists.txt | 14 +++++++------- code/planning/src/behavior_agent/behavior_tree.py | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/code/planning/CMakeLists.txt b/code/planning/CMakeLists.txt index 4997236d..34c3ccd0 100755 --- a/code/planning/CMakeLists.txt +++ b/code/planning/CMakeLists.txt @@ -204,11 +204,11 @@ 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} -) +# 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/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index 480abba9..990b9e1e 100755 --- a/code/planning/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -1,14 +1,14 @@ #!/usr/bin/env python import functools -import behavior_agent +# 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 From fad590da354db18d2a45330799977bc6bf826884 Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Fri, 22 Dec 2023 12:01:58 +0100 Subject: [PATCH 3/5] fix: Logging fxed --- code/planning/src/global_planner/global_planner.py | 2 +- code/planning/src/local_planner/ACC.py | 2 +- code/planning/src/local_planner/collision_check.py | 2 +- code/planning/src/local_planner/motion_planning.py | 3 ++- 4 files changed, 5 insertions(+), 4 deletions(-) diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index e61bd0fd..a113c787 100755 --- a/code/planning/src/global_planner/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.logerr('PrePlanner-Node started') + self.logdebug('PrePlanner-Node started') def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 6a19518d..3849dc6a 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -83,7 +83,7 @@ def __init__(self): # Current speed limit self.speed_limit: float = None # m/s - self.logerr("ACC initialized") + self.logdebug("ACC initialized") def _set_distance(self, data: MinDistance): """Get min distance to object in front from perception diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 8bea2f82..3cc107d3 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -23,7 +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.logerr("CollisionCheck started") # self.obstacle_sub: Subscriber = self.new_subscription( # ) @@ -58,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/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 3f969b2d..92949cad 100755 --- a/code/planning/src/local_planner/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.logerr("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) From 83971192c3ea9636b1eb7329540832c6cef6214a Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 11 Jan 2024 17:06:06 +0100 Subject: [PATCH 4/5] fix: removed old folder --- .../global_planner/launch/global_planner.launch | 15 --------------- 1 file changed, 15 deletions(-) delete mode 100644 code/planning/global_planner/launch/global_planner.launch 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 cdb6dec9..00000000 --- a/code/planning/global_planner/launch/global_planner.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - From bda01122a77319b52a9f1cca7dff33d3a4e9b094 Mon Sep 17 00:00:00 2001 From: MaxJa4 <74194322+MaxJa4@users.noreply.github.com> Date: Sat, 13 Jan 2024 15:35:18 +0100 Subject: [PATCH 5/5] 110 - Implement traffic light detection (#148) * Add publisher * Publish segmented traffic lights * Implement TrafficLightNode * Add TrafficLightState msg. WIP * Add publisher * Publish segmented traffic lights * Implement TrafficLightNode * Add TrafficLightState msg. WIP * Added side view classification * Finish traffic light node * Add manual control launch file * Make linter happy * Add documentation * Add missing traffic light detection model * Fix color issues in rviz * Limit simulator's max. RAM usage to prevent system crash * fix: Linter fixes for other team's code --- .gitignore | 2 + build/docker-compose.yml | 4 ++ code/agent/launch/agent_manual.launch | 31 ++++++++++ code/perception/CMakeLists.txt | 1 + code/perception/launch/perception.launch | 18 ++++-- code/perception/msg/TrafficLightState.msg | 5 ++ .../src/traffic_light_detection/.gitignore | 1 - .../src/traffic_light_detection/dataset.dvc | 2 +- .../src/traffic_light_detection/dvc.lock | 23 +++---- .../dvclive/metrics.json | 11 ++++ .../models/model_acc_92.48_val_91.88.pt | Bin 0 -> 11071 bytes .../src/traffic_light_detection/params.yaml | 2 +- .../src/traffic_light_config.py | 2 +- .../traffic_light_inference.py | 12 ++-- .../traffic_light_training.py | 4 +- .../transforms.py | 0 code/perception/src/traffic_light_node.py | 57 ++++++++++++++++++ code/perception/src/vision_node.py | 39 ++++++++++-- .../src/behavior_agent/behavior_tree.py | 8 +-- .../src/behavior_agent/behaviours/__init__.py | 2 - .../13_traffic_light_detection.md | 37 ++++++++++++ .../object-detection-model_evaluation/yolo.py | 8 ++- 22 files changed, 228 insertions(+), 41 deletions(-) create mode 100644 code/agent/launch/agent_manual.launch create mode 100644 code/perception/msg/TrafficLightState.msg create mode 100644 code/perception/src/traffic_light_detection/dvclive/metrics.json create mode 100644 code/perception/src/traffic_light_detection/models/model_acc_92.48_val_91.88.pt rename code/perception/src/traffic_light_detection/src/{data_generation => traffic_light_detection}/transforms.py (100%) create mode 100755 code/perception/src/traffic_light_node.py create mode 100644 doc/06_perception/13_traffic_light_detection.md 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 92f54ef1..70123763 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_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 be3c7d27..300976a3 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -2,7 +2,7 @@ - + - - + - 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 0000000000000000000000000000000000000000..584f75f5cc2f038cf86088583d5d9ed96f7d9c55 GIT binary patch literal 11071 zcmb`N2Ut``*T7uxVG?mcQ=!o>Vgs4zabW&P!YG{U?3M*%37NjKkUYew4cuX`a-|-a{uY}}qQ9#E@ z6;>fgMpDOyRSZ&;+DMNIceP=a{07U2Smkj7o|!;iASX}|D2Q0TnLr@uFZnFFq*4?s zRi-Lp)r9OdmOdhM*0n-w( zy@hO_APosDB3u+18=91yn)vCC`*wlavHdzj`%9tPAE7!@sBQ;zfQZ!-vIB#9eg{? z*bze3{gc<){pqzHU9=BnM|Q?|`jtvJUJ{Pf>Et_{&f827^{3N~>Y)3G*wI2(@X6`y ztlK-&w+q{u_3O+YBW;(lAKS%W+AiZdU_ucaAY{jXl4}2_)Cpa1E^J_D+(bdChzKqZ9EoA2gss4xB-u3gkG9B2Q&dgkC@XBjP=CS!wkp&&NLJ?ae zWap=6u?vFu5~O70io&Hqoh@#UZY5?xlCwN^VHUfnJ(6Zbr>03lG+Qc=NefbtTxt)= z?BXo8EVlieR9L6b#g=EW6_P8m|EX=qF3DmmJ8Da9CWe0#nv$HHV9PGeVyimN|4FuI ztFzc;f30iBew)SCbgk>aF3)0X|613ct;=HTJI+g!!L)>Mkt9d34O#4p|D26VO-@Np z7qQ=Eu`4^yN|QlH$FZxj*wyVP)6*iX{RK+O>n*2}Mo9%}@&DW0X~^^VLhg*n6h$XX z6P=g)u>UD}O7d<(oJf=qEmf8WH~UMKOUvIY)8`HTf-8PfU9KQGvl*O$Z|(2P9whJ);V|3%cUHDzF{Jc z+i?{g8$ICWoIy~eCr2{W8X$f5b2rCe4sGV~h`gEunnl==5d()eZ4flUy*rMi?*b>H zH`fjW%a^((9Md6gzou|z>HW!YD|5G}Yiu~5G6kaG`wZUJwQ?($R^XYo>3HF2H9R`| zmRa|B1*f@r1q^SkWMn~z%JUY&?D+4wM>$c9mHZ7Z>Utu2^|yw~fYss)k8X%>2o-75 z=oRA8DNQ&bcnMB>70X2(|C-q&Z~(cWT>K;E2{&HzG)6|Ab}8LogCk?lf!&~dW`45| zcsm2$xPPswy60ui*k~*~xcm|(FVdjvuOC8hhwI3*^mUv0Tn+Qo4KboB7J{|1F`()O zH`~4(Pv#ZD&Z|pt`_ao#(C;{EFV$kEHJ*k+VeX7yT`pePh+ISTDiqCBrPbda=58r@ zLS%NEcy^FH1}2@yfFr$`qhdYUQ~xVE)B$@!^O-PlMPHoP&7=J>xs2&gWGB6cnE*?4CZ$y|K7_y|{VFac@HUq}b1X0u zJh_LfZ^6&~Wk{M|DlQ+m4~{8q0;~AB;OV2}`s?rOK-O{{9FS+x*W(Fiug-_%#re=j z_7wE#` zoC=CddeD&VLtq&H9ye_7E;#S<3a##z;8nMW+`+tk=n%7gX_Vc$t|5y0E>-eXuZ-h zurQkfUbk)0Z^U&bzrjT8dm$Jc+}b!9?#JEdwiSR)>U=KLVLi(9@?^BVS3*I(s@sFY zbEr7(gu~YKNTy<(4n8#4#r=?;An}`X@N5}}0fU-x@;rWzGsfAcrs zrtlVw_ti7@mKzyalYTH;s{uFDEKof$2TYA$qRdfyocB@}d;hW$?`W~GTZo+g@2k1s zlzbeh#lu+&iugUw!@1|ypira=*?uc9NV5cwyc!E{Qsn3Zlh&qhZcM=g)!Mjyxfcj^ z?{Z0dwXv7DnMv`y2n_0hX5I<7SENn+VroGeDW@H*J-9_O>yef(MN0SXBBiaDJNd_v z^1n}#Dwp%=_zicM)`U=+>wkuk>Ki#k#Fsv^OUvKu8^jybtv*R8AMj;?qh1|{a<4~> zn$1lVXz_{Ee&FbcmVd{-q=CAz?`S%M9E{{+^feP&KkYRantu!CKeyp(eZuLoiAnUz zZ>7Y!*A5s~Uy6seomlS6AsS&GAC%Js47~eS&IwG%%{&v2GUVnN9jver) za=(~R6{Zn9zkUVk1w~lII|Ebq`I09i_ro!-v811^Avqb~zqg^q=0nh2_ zGVveq2TZXPqQ}BLC^v93=i4@rUdxZ7XPzF%%CPA)?74tO4St1Ee--RJa(uXCIw37z z8ZWlpju$&GcdIXtm(NG>s+WA~xa4OvP*%m~$3^zJv9o-q5&)1nc$;aFl=F~TUxIi z-2Gxc*^(LotJh1OTLuJ>?cc}}?adeQ^*SE4ycACFrklYEph%swF{0ox?)d2xbEN-I zVDM`avupDpn3QKp>aLj(pEZ^6BrTEnEii@ZHp8Y_y#8SQav|q=_82Ps77D@Jc+mQ? zC-wWK2RfQ7a2rqP5|`{{VDjuG=QLg!@8<;4=C|W;^jUxxrgNG1lJVPQ|HxI?<21U% zN_?@p3G~X{Nb9cyI6+1ntf;SoO{&*%;tVx9;`~KC(W?;0G$dj}iU!qKd;>?FTf0YV zff7AFv6$GIAB0nNi||my9QtgU4CKZ&1CF={ep=P|kiE zHi)vOqp8^ddsGc5!<&OcxPU4VoK|-Rw@DggibGE_d;eVcP*V!4ldEuDjWHTNt3&pF ze_XfnIv#pbfODp*(15Zaj2!JjC-v-BNivsHOxB+8iOr-;|Wpod88eWw5QP z!1-l=V6wIr%xP-|xk5#%ej@?Y8hP0NW&?O$)&rGh9w{%qjH_NwBJ8D}WaR@bqLMtA z>>ThKW0&rL4GuOm=CBjx`Nq=4v+StW>MO8do&hO3?}mwo2Ga3Yyy-NzvDCMu0e8Jz zjHJ$(>M1@&EA2is?_Dpn^B7L1FLQ!#)YszUx-2LP;lXq+3{@8wbB=8x82WCWIQPwQ z)Tqm29^O=g%s3S5So=}C&3t;*dkoz)#}qXWPJrgwCvjpgReJTe9L8(PKJom$qv?~H z-l%$T0_VZ=qmliTu%?tpFaP50s$1^M{dDy?R9|o>5l?Li^R)$iAH5rT>6jCvtWo62 zQ$7+MXU41_<&K|Q43*n1BZtvkY?=mrq|tzOQ)bYWGw0(^F>3V1;5!(){~9RI8%SP` za%6m3dr)4u2Zk7zp^?`-lHT?lrfjl+*{`!9@V*P#GHVtdyYPr}c+mpQJA2XxS@k%f ztO2~Axsq|Fdr>Pi7Q76$fU?VD*ejz7%U%Cy>ic~iaTLAwiU*jRzg~ZfYiyI$F0Mbix0fo z&O9jKlSLkD@k&r2zMsR8aM4jFo_mfzTwlRNeLoZKrQ|m`e|rG<+ujY`Y*9vLm})Td zyVg}?Rf!(H)g<0{Tk;P0&AMn*K? zi&<%~em2M4X)2|CHdJs${Z`-&^|RP-$#*ovPJ;^GPj^!dIE--U2_Dy-M@6?5fUd6q z>H=+vw;+Og9x^6OtpaJydCEi_=t0A5J*ZXr02IsR(dw<|aONpBe6%1Pb0%2O$@W!X zxN|O47LB8el$4qqGs~HRVSP!#l$khr;A*P9bUQwb@?owXD*{%bCzZzk>$gZFPx|6O zQvSF2@9_VQ|D)RqNYmi8+zh4`uko|RmV1}A=cG&<#YioQBx(73P5L~&?sAeOC24;l zC0!u3!)gN;5Boi-df1sqD~3tiLuJ=x>+bW9KF^>#+miI7_XXS1Txglwhp95zhOguK z^cQW@_UG%xY*O&QVqel7-Pz9_Rm&{RT1qT-X1TSE&}_F~xYd2jzhd99d;3%R8!`8T zH9&Z(*mdtbXwUzbdLQ2Uuh@6$-hNbXiG#KKn3D2{{n7H$g&b-5lFwOm*Y88U+)cXa z_dlP5&iEaL72BI(j-?riuTdcnJ*JS&IR<3hPFWJTLznnmyaqAuHst>4wc_*=W9Cqr z1;p9D1cx$1GW}jXw{L2y+WDJKEbmG%SBwKxHnV~$j^*xR%+ct3b&Q74V z%@e7Ok|#A&{0<+n?)25`{doMfD$ZWegFaZY3T>j5@nCXk(*wV9WL)>7$y!^w?D4N? zbt{lA3Twe*oE2T|B1;c*4cNBBj+QMu%Dwo`h!&MTMAPA_bXA@XPE_%yx<5{)r&hY+ zrOBBnlkqhjW~)n8$175&L_IoN{u+*c8b-GU4rof{oWXmOI_dpkGKnhJfqNI^$ihL! zz@?d!@++2PN$DEcoYcbX-Dn7JYh6h14ZFcR|94mAR5P&a-+=Lc-vYfVN0J$D*Y(Zn zFy@jOefA<2iswax-sx$u^UzM%v&DqG2|LZG-E`uXD6fFcYvZ8)Ts1RihZ^xSTF+$8 zJ_hcW_rU($?&LcC7D9e{3{&Dvh|?-{;!%GE#%npiubbY(erGFEwC6H_XQo(7(gN1Y z!;v4ViTo4k%v2q9=0~&PcW!hwyhDI)z)ORQQqa|EbzsKqAZ;1V*(*hv+=HI9Lc|z zM#Nx8g1q%e@unki0DBV`m`V1Qh@tVUJ)y5Al1V<1#7ucADanx`TOP<6Y0C_T=+yvMMCThVA-Sb zjy_wN+4YOE!c^6!v6XJ%=$9@3akTjB0^b9o2639Mf zCo<`V9@)20nN*+jCj%|d!$=1+qO>I(CZ8>FPMGipo_gdG{yqzGQ?vw6r~HUdv<8#P z;CIk?WeD*#EOlE@lR(4l-SBHm7ra@u3>-Xu=EjHpfVLl=;)jBKJp9}Uwiq|zjN<9E z^0zx~RVLrzp!f@Dm=(eCn~ZVzN>93RT^w#tInNm1=0l$sQKSdm6{h4ss9t^=Rs(YP|p5e9XI{17Gd?z#T3Rq*oY!dOdh8r&OU$+iu@QuS6@VG}f8^ z@RKEJeDDN(RAhL{eRx21cx$U4Wo?Nq^(z?C;LFXe z-NQtPP3f~!#xAY1Jcz?7J$iCnJ-*p?7x#va0yg*ohTC3;jJE^H=zZ2=U2zOkw`?NG zonc26cKA`fg_-oQM-+D})}8V<>eCSiOX0xjUod}vHka=Ho=Y6yL8hHCWWof5?MeU3F3_a}F8ZQ1O{tF{Cd$nq>@2v8Y&eOwklf*8xZgIxoA(aFk6q%1iA54!Nc*CL8k z>L-xo-P6cXy+ZO1%ZYiNGX1Il2QFPOn7*5L2=cO3-4ZU3L@uiaBQ_2qs!N73@f&lv zj3Lpu@#+>RU&RB{1q;ZSRRz#{W+1s=$uOmowXdtXlj-5$Y4n7r2W=6IBELOtfjvWw zaO0i>_;9Ezj#k%f_gC5R%RK(q_g^2EcmLn>AA1$)jN^1|%mnFrzO?-P2)5zre_;e$ zw=X`XMW@DvC&bMNm+l7&jf?Vf*Zty%ws9J2>tyF*XEW5n#?isW*~ZDn$=1o)(caO{ z-pOUCvxCdfp|%drPEwU;uCF>_CQ&FYl6??Tv7dH6bzavskY4C=biWVkb6)4wSi>$X zMzR$B_qI`W>2DrSMpFC*3euf5JWa`uw0=k3{{uXIDZu~$ literal 0 HcmV?d00001 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/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index 990b9e1e..cacf8619 100755 --- a/code/planning/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 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") ]) diff --git a/code/planning/src/behavior_agent/behaviours/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py index f8984b65..e69de29b 100755 --- a/code/planning/src/behavior_agent/behaviours/__init__.py +++ b/code/planning/src/behavior_agent/behaviours/__init__.py @@ -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/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