diff --git a/CMakeLists.txt b/CMakeLists.txt
index 69b371c990bd02ea4040ae38c1b2a842f4aecfc7..4d0ceb9922df5db2921308e4551ab7a3fffd8f0a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,31 +1,32 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(panda_mqtt_connector)
 
-add_compile_options(-std=c++14)
 set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS} -std=c++14")
 
 # 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 controller_manager
-                        effort_controllers
-                        gazebo_ros
-                        genmsg
-                        joint_state_controller
-                        joint_state_publisher
-                        joint_trajectory_controller
-                        robot_state_publisher
-                        roscpp
-                        std_msgs
-                        tf
-                        xacro
-                        moveit_core
-                        moveit_visual_tools
-                        moveit_ros_planning
-                        moveit_ros_planning_interface
-                        controller_interface
-                        hardware_interface)
+find_package(catkin REQUIRED COMPONENTS
+        controller_manager
+        effort_controllers
+        gazebo_ros
+        genmsg
+        joint_state_controller
+        joint_state_publisher
+        joint_trajectory_controller
+        robot_state_publisher
+        roscpp
+        std_msgs
+        tf
+        xacro
+        moveit_core
+        moveit_visual_tools
+        moveit_ros_planning
+        moveit_ros_planning_interface
+        controller_interface
+        hardware_interface
+        simulation_util
+        )
 
 # System dependencies are found with CMake's conventions
 find_package(Boost REQUIRED COMPONENTS filesystem)
@@ -33,14 +34,12 @@ find_package(Boost REQUIRED COMPONENTS filesystem)
 find_package(Protobuf 3 REQUIRED)
 include_directories(${Protobuf_INCLUDE_DIRS})
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
-protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto)
-protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/linkstate.proto)
-protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/robotconfig.proto)
+#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto)
+#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/linkstate.proto)
+#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/robotconfig.proto)
 protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto proto/linkstate.proto proto/robotconfig.proto proto/trajectory.proto)
 
 SET_SOURCE_FILES_PROPERTIES(${PROTO_SRC} ${PROTO_HDRS} PROPERTIES GENERATED TRUE)
-message("psource " ${PROTO_SRC})
-message("pheaders" ${PROTO_HDRS})
 
 find_package(PkgConfig REQUIRED)
 
@@ -52,25 +51,20 @@ set(PAHO_MQTT_C_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/lib/paho.mqtt.c/src")
 
 add_subdirectory(lib/paho.mqtt.cpp)
 
-get_cmake_property(_variableNames VARIABLES)
-list (SORT _variableNames)
-foreach (_variableName ${_variableNames})
-    message(STATUS "${_variableName}=${${_variableName}}")
-endforeach()
-
 # ################################################################################################################################
 # catkin specific configuration ##
 # ################################################################################################################################
 # The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
 catkin_package(CATKIN_DEPENDS
-               moveit_core
-               moveit_visual_tools
-               moveit_ros_planning_interface
-               controller_interface
-               hardware_interface
-               pluginlib
-               DEPENDS
-              # system_lib
+        moveit_core
+        moveit_visual_tools
+        moveit_ros_planning_interface
+        controller_interface
+        hardware_interface
+        pluginlib
+        simulation_util
+        DEPENDS
+        # system_lib
         )
 
 # ################################################################################################################################
@@ -79,7 +73,6 @@ catkin_package(CATKIN_DEPENDS
 
 # Specify additional locations of header files Your package locations should be listed before other locations
 include_directories(
-        include
         lib/paho.mqtt.c/src
         lib/paho.mqtt.cpp/src
         ${catkin_INCLUDE_DIRS}
@@ -90,14 +83,23 @@ add_dependencies(paho-mqttpp3 paho-mqtt3c)
 add_library(TrajectoryUtil src/util/TrajectoryUtil.cpp src/util/TrajectoryUtil.h)
 add_library(MqttUtil src/util/MqttUtil.cpp src/util/MqttUtil.h)
 
-# Specify libraries to link a library or executable target against
-target_link_libraries(TrajectoryUtil ${catkin_LIBRARIES})
 
 add_executable(RobotStateProvider src/RobotStateProvider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(TimedPlanner src/TimedPlanner.cpp)
 add_executable(MqttToRosNode src/MqttToRosNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(MqttRosConnectionTestNode src/MqttRosConnectionTestNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
+add_executable(${PROJECT_NAME}_safety_zone_spawner src/safety_zone_spawner.cpp)
+
+
+set_target_properties(${PROJECT_NAME}_safety_zone_spawner PROPERTIES OUTPUT_NAME safety_zone_spawner PREFIX "")
+
 
+add_dependencies(${PROJECT_NAME}_safety_zone_spawner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+# Specify libraries to link a library or executable target against
+target_link_libraries(TrajectoryUtil
+        ${catkin_LIBRARIES}
+        )
 target_link_libraries(RobotStateProvider LINK_PUBLIC
         MqttUtil
         ${catkin_LIBRARIES}
@@ -117,3 +119,5 @@ target_link_libraries(MqttRosConnectionTestNode LINK_PUBLIC
         ${Protobuf_LIBRARIES}
         )
 target_link_libraries(TimedPlanner ${catkin_LIBRARIES} TrajectoryUtil)
+
+target_link_libraries(${PROJECT_NAME}_safety_zone_spawner ${catkin_LIBRARIES})
diff --git a/LICENSE b/LICENSE
index b59128245e48fe628b1366d4bc607078fc5abff8..a943dd968e959a2a8199e864f634ae3b67836537 100644
--- a/LICENSE
+++ b/LICENSE
@@ -1,4 +1,4 @@
-Copyright 2019 Erdal Pekel
+Copyright 2020 Technische Universität Dresden, Software Technology Group
 
 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
 
diff --git a/config/config.yaml b/config/config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e891d24f37a989336158d6c3bc1c3af0b90a4cc2
--- /dev/null
+++ b/config/config.yaml
@@ -0,0 +1,21 @@
+panda_mqtt_connector:
+  server: "tcp://localhost:1883"
+  topics:
+  - robotConfig: "robotconfig"
+  - dataConfig: "dataconfig"
+  zone_size: 0.5
+  zones:
+    - "1 1"
+    - "-1 -1 1"
+  joints:
+    - panda/Joint0: "panda::panda_link0"
+    - panda/Joint1: "panda::panda_link1"
+    - panda/Joint2: "panda::panda_link2"
+    - panda/Joint3: "panda::panda_link3"
+    - panda/Joint4: "panda::panda_link4"
+    - panda/Joint5: "panda::panda_link5"
+    - panda/Joint6: "panda::panda_link6"
+    - panda/LeftFinger: "panda::panda_leftfinger"
+    - panda/RightFinger: "panda::panda_rightfinger"
+  end_effectors:
+    - panda/EndEffector: "panda::panda_link7"
\ No newline at end of file
diff --git a/config/mqtt.yaml b/config/mqtt.yaml
deleted file mode 100644
index ba53262d460636ff7310aa1b2b29c0dc847c9277..0000000000000000000000000000000000000000
--- a/config/mqtt.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-panda_mqtt_connector:
-  server: "tcp://localhost:1883"
-safety_zone_spawner:
-  size: 0.5
-  zones:
-    - "1 1"
-    - "-1 -1 1"
\ No newline at end of file
diff --git a/launch/simulation_rosrag.launch b/launch/simulation_rosrag.launch
index 65e66c6c729834a1bc95422b9394d133adfc2055..9c55f396d9ef69d4704da5a666225108765a0651 100644
--- a/launch/simulation_rosrag.launch
+++ b/launch/simulation_rosrag.launch
@@ -1,13 +1,13 @@
 <launch>
-    <arg name="mqtt_server" default="" />
+    <arg name="mqtt_server" default=""/>
     <include file="$(find panda_simulation)/launch/simulation.launch"/>
 
     <!-- the launch file parameter mqtt_server overrides the config file parameter -->
-    <rosparam file="$(find panda_mqtt_connector)/config/mqtt.yaml" command="load" />
+    <rosparam file="$(find panda_mqtt_connector)/config/config.yaml" command="load" />
     <rosparam unless="$(eval arg('mqtt_server') == '')" param="panda_mqtt_connector/server" subst_value="True">$(arg mqtt_server)</rosparam>
 
     <node name="TimedPlanner" pkg="panda_mqtt_connector" type="TimedPlanner" respawn="false" output="screen"/>
     <node name="MqttToRosNode" pkg="panda_mqtt_connector" type="MqttToRosNode" respawn="false" output="screen"/>
     <node name="RobotStateProvider" pkg="panda_mqtt_connector" type="RobotStateProvider" respawn="false" output="screen"/>
-    <node name="gazebo_zones" pkg="gazebo_zones" type="gazebo_zones" respawn="false" output="screen"/>
+    <node name="safety_zone_spawner" pkg="panda_mqtt_connector" type="safety_zone_spawner" respawn="false" output="screen"/>
 </launch>
diff --git a/launch/simulation_rosrag_test.launch b/launch/simulation_rosrag_test.launch
index c489c2db372a223e3d4e938978e71a821a40a3b0..70a4b67e3ac44c22fc3f10db6055d2cf38e8c25e 100644
--- a/launch/simulation_rosrag_test.launch
+++ b/launch/simulation_rosrag_test.launch
@@ -1,8 +1,14 @@
 <launch>
+    <arg name="mqtt_server" default=""/>
     <include file="$(find panda_simulation)/launch/simulation.launch"/>
 
+    <!-- the launch file parameter mqtt_server overrides the config file parameter -->
+    <rosparam file="$(find panda_mqtt_connector)/config/config.yaml" command="load" />
+    <rosparam unless="$(eval arg('mqtt_server') == '')" param="panda_mqtt_connector/server" subst_value="True">$(arg mqtt_server)</rosparam>
+
     <node name="TimedPlanner" pkg="panda_mqtt_connector" type="TimedPlanner" respawn="false" output="screen"/>
     <node name="MqttToRosNode" pkg="panda_mqtt_connector" type="MqttToRosNode" respawn="false" output="screen"/>
-    <node name="MqttRosConnectionTestNode" pkg="panda_mqtt_connector" type="MqttRosConnectionTestNode" respawn="false" output="screen"/>
     <node name="RobotStateProvider" pkg="panda_mqtt_connector" type="RobotStateProvider" respawn="false" output="screen"/>
+    <node name="safety_zone_spawner" pkg="panda_mqtt_connector" type="safety_zone_spawner" respawn="false" output="screen"/>
+    <node name="MqttRosConnectionTestNode" pkg="panda_mqtt_connector" type="MqttRosConnectionTestNode" respawn="false" output="screen"/>
 </launch>
diff --git a/mainpage.dox b/mainpage.dox
new file mode 100644
index 0000000000000000000000000000000000000000..072ec4530ba5706ba4efa8165ed09924e4684d4e
--- /dev/null
+++ b/mainpage.dox
@@ -0,0 +1,28 @@
+/**
+\mainpage
+
+\htmlinclude manifest.html
+
+## Provided Nodes
+
+### safety_zone_spawner
+
+* **Author:** Johannes Mey*
+
+This node spawns cubic safety zones in gazebo.
+It expects the ROS parameter `/safety_zone_spawner/zone_size` and `/safety_zone_spawner/zones` to be set.
+
+- `/safety_zone_spawner/zone_size: double`
+
+  the edge size of a safety zone cube to be displayed in *m*.
+- `/safety_zone_spawner/zones: std::vector<std::string>`
+
+  a list of safety zone cube integer coordinates. The coordinates are encoded in a string and separated by spaces
+  and have an `x`, a `y`, and and optionally a `z` coordinate. Examples are `"3 0"` or `"-5 2 4"`. The coordinates
+  are interpreted as follows: the cube at position "0 0 0" is centered at the origin of the coordinate system in
+  `x` and `y` direction, but is located *on top of the x-y-plane*. All other node coordinates are defined such that
+  they completely fill the space. This means that the actual coordinates (in *m*) can be computed using the ones
+  given here and the `zone_size`.
+
+
+*/
diff --git a/package.xml b/package.xml
index 7aa73dd2f1ba748fa553ffeefe129d4452321dbf..d9b8161d3bcf762ff88732d3c05800fff4f19b21 100644
--- a/package.xml
+++ b/package.xml
@@ -42,6 +42,7 @@
   <depend>moveit_visual_tools</depend>
   <depend>controller_interface</depend>
   <depend>hardware_interface</depend>
+  <depend>simulation_util</depend>
 
   <exec_depend>pluginlib</exec_depend>
 
diff --git a/src/safety_zone_spawner.cpp b/src/safety_zone_spawner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa209825e7a99df8479a978612c6325c9dc8bef1
--- /dev/null
+++ b/src/safety_zone_spawner.cpp
@@ -0,0 +1,87 @@
+/*! \file safety_zone_spawner.cpp
+    \brief ROS node main file that spawns safety zones in gazebo.
+
+    \author Johannes Mey
+    \date 28.06.20
+*/
+
+#include <ros/ros.h>
+#include <shape_msgs/SolidPrimitive.h>
+#include <geometry_msgs/Pose.h>
+#include <GazeboZoneSpawner.h>
+
+/**
+ * The main method of the ROS node
+ *
+ * It does not requite command-line parameters, but expects the ROS parameter `/safety_zone_spawner/zone_size` and
+ * `/safety_zone_spawner/zones` to be set.
+ *
+ *  - `/safety_zone_spawner/zone_size: double`
+ *
+ *    the edge size of a safety zone cube to be displayed in *m*.
+ *  - `/safety_zone_spawner/zones: std::vector<std::string>`
+ *
+ *    a list of safety zone cube integer coordinates. The coordinates are encoded in a string and separated by spaces
+ *    and have an `x`, a `y`, and and optionally a `z` coordinate. Examples are `"3 0"` or `"-5 2 4"`. The coordinates
+ *    are interpreted as follows: the cube at position "0 0 0" is centered at the origin of the coordinate system in
+ *    `x` and `y` direction, but is located *on top of the x-y-plane*. All other node coordinates are defined such that
+ *    they completely fill the space. This means that the actual coordinates (in *m*) can be computed using the ones
+ *    given here and the `zone_size`.
+ */
+int main(int argc, char **argv) {
+
+  // setup this ros-node
+  ros::init(argc, argv, "safety_zone_spawner");
+  ros::NodeHandle node_handle("panda_mqtt_connector");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+
+  // wait for gazebo
+  // TODO this should actually wait until gazebo has started
+  ros::Duration(2.0).sleep();
+
+
+  std::vector<std::string> keys;
+  node_handle.getParamNames(keys);
+  for (auto key : keys) {
+    ROS_ERROR_STREAM(key);
+  }
+
+  // get size
+  double size = 0.5;
+  if (!node_handle.getParam("zone_size", size)) {
+    ROS_WARN_STREAM("Could not get string value for " << node_handle.getNamespace() << "/size from param server, defaulting to " << size);
+  }
+
+  shape_msgs::SolidPrimitive box;
+  box.type = shape_msgs::SolidPrimitive::BOX;
+  box.dimensions = {size, size, size};
+
+  // get list of zone voxels
+  std::vector<std::string> voxels;
+  if (!node_handle.getParam("zones", voxels)) {
+    ROS_WARN_STREAM("Could not get string value for " << node_handle.getNamespace() << "/zones from param server. Not adding any zones.");
+  }
+
+  for (const std::string &voxel: voxels) {
+    geometry_msgs::Pose pose;
+
+    std::stringstream stream(voxel);
+    int x, y, z = 0;
+    stream >> x >> y;
+    if (stream) {
+      stream >> z; // the z value is 0 by default
+    }
+
+    pose.position.x = x * size;
+    pose.position.y = y * size;
+    pose.position.z = z * size + size / 2;
+
+    // the orientation is always the default
+    pose.orientation.w = 1;
+
+    GazeboZoneSpawner::spawnCollisionBox(box, pose);
+  }
+
+}
\ No newline at end of file