From 8f16333880f829e3a481df794f9b8427c6b682eb Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 21 Dec 2023 16:35:32 +0100 Subject: [PATCH 1/4] 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/4] 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/4] 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/4] 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 @@ - - - - - - - - -