diff --git a/code/planning/local_planner/CMakeLists.txt b/code/planning/local_planner/CMakeLists.txt
new file mode 100644
index 00000000..32a5947b
--- /dev/null
+++ b/code/planning/local_planner/CMakeLists.txt
@@ -0,0 +1,202 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(local_planner)
+
+## 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/local_planner/__init__.py b/code/planning/local_planner/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch
new file mode 100644
index 00000000..c469c05e
--- /dev/null
+++ b/code/planning/local_planner/launch/local_planner.launch
@@ -0,0 +1,6 @@
+
+
+
+
+
+
diff --git a/code/planning/local_planner/package.xml b/code/planning/local_planner/package.xml
new file mode 100644
index 00000000..6dc4269a
--- /dev/null
+++ b/code/planning/local_planner/package.xml
@@ -0,0 +1,59 @@
+
+
+ local_planner
+ 0.0.0
+ The local planning package
+
+
+
+
+ carla
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+
+
+
+
+
+
+
+
diff --git a/code/planning/local_planner/setup.py b/code/planning/local_planner/setup.py
new file mode 100644
index 00000000..e5a88c1c
--- /dev/null
+++ b/code/planning/local_planner/setup.py
@@ -0,0 +1,7 @@
+#!/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/src/collision_check.py b/code/planning/local_planner/src/collision_check.py
new file mode 100755
index 00000000..34bcdbb4
--- /dev/null
+++ b/code/planning/local_planner/src/collision_check.py
@@ -0,0 +1,107 @@
+#!/usr/bin/env python
+# import rospy
+# import tf.transformations
+import ros_compatibility as roscomp
+from ros_compatibility.node import CompatibleNode
+
+# from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
+# from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo
+# from nav_msgs.msg import Path
+# from std_msgs.msg import String
+# from std_msgs.msg import Float32MultiArray
+
+
+class CollisionCheck(CompatibleNode):
+ """
+ This is currently a test node. In the future this node will be
+ responsible for detecting collisions and reporting them.
+ """
+
+ 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.logdebug("CollisionCheck started")
+
+ def update(self, speed):
+ self.current_speed = speed
+
+ def time_to_collision(self, obstacle_speed, distance):
+ return distance / (self.current_speed - obstacle_speed)
+
+ def meters_to_collision(self, obstacle_speed, distance):
+ return self.time_to_collision(obstacle_speed, distance) * \
+ self.current_speed
+
+ # PAF 22
+ def calculate_safe_dist(self) -> float:
+ """
+ Calculates the distance you have to keep to the vehicle in front to
+ have t_reaction to react to the vehicle suddenly stopping
+ The formula replicates official recommendations for safe distances
+ """
+ t_reaction = 1 # s
+ t_breaking = 1 # s
+ a = 8 # m/s^2
+ v = self.current_speed
+ s = - 0.5 * a * t_breaking ** 2 + v * t_breaking + v * t_reaction
+ return s + 5
+
+ def calculate_rule_of_thumb(self, emergency):
+ reaction_distance = self.current_speed
+ braking_distance = (self.current_speed * 0.36)**2
+ if emergency:
+ return reaction_distance + braking_distance / 2
+ else:
+ return reaction_distance + braking_distance
+
+ def check_crash(self, obstacle):
+ distance, obstacle_speed = obstacle
+
+ collision_time = self.time_to_collision(obstacle_speed, distance)
+ collision_meter = self.meters_to_collision(obstacle_speed, distance)
+
+ safe_distance = self.calculate_safe_dist()
+ safe_distance2 = self.calculate_rule_of_thumb(False)
+ emergency_distance2 = self.calculate_rule_of_thumb(True)
+
+ # TODO: Convert to Publishers
+ if collision_time > 0:
+ if distance < emergency_distance2:
+ print(f"Emergency Brake needed, {emergency_distance2:.2f}")
+ print(f"Ego reaches obstacle after {collision_time:.2f} seconds.")
+ print(f"Ego reaches obstacle after {collision_meter:.2f} meters.")
+ print(f"Safe Distance PAF 22: {safe_distance:.2f}")
+ print(f"Safe Distance Thumb: {safe_distance2:.2f}")
+ else:
+ print("Ego slower then car in front")
+
+ def run(self):
+ """
+ Control loop
+ :return:
+ """
+
+ def loop(timer_event=None):
+ pass
+
+ self.new_timer(self.control_loop_rate, loop)
+ self.spin()
+
+
+if __name__ == "__main__":
+ """
+ main function starts the CollisionCheck node
+ :param args:
+ """
+ roscomp.init('CollisionCheck')
+
+ try:
+ node = CollisionCheck()
+ node.run()
+ except KeyboardInterrupt:
+ pass
+ finally:
+ roscomp.shutdown()
diff --git a/code/planning/planning_runner/launch/planning_runner.launch b/code/planning/planning_runner/launch/planning_runner.launch
index 30f01411..b5597b71 100755
--- a/code/planning/planning_runner/launch/planning_runner.launch
+++ b/code/planning/planning_runner/launch/planning_runner.launch
@@ -2,4 +2,6 @@
+
+