diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2ee1526a4fdebbc9dcc8014a6db8f992e3950655..c1ec4c8f41d7acd1f91bb1058a0da2d445b77c49 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,10 +13,6 @@ find_package(catkin REQUIRED COMPONENTS
   moveit_core
   moveit_visual_tools
   moveit_ros_planning_interface
-        roscpp
-        rospy
-        std_msgs
-        genmsg
 )
 
 ## System dependencies are found with CMake's conventions
@@ -65,8 +61,6 @@ find_package(catkin REQUIRED COMPONENTS
 #   Service1.srv
 #   Service2.srv
 # )
-add_message_files(DIRECTORY msg FILES Num.msg)
-add_service_files(DIRECTORY srv FILES AddTwoInts.srv)
 ## Generate actions in the 'action' folder
 # add_action_files(
 #   FILES
@@ -79,7 +73,7 @@ add_service_files(DIRECTORY srv FILES AddTwoInts.srv)
 #   DEPENDENCIES
 #   std_msgs  # Or other packages containing msgs
 # )
-generate_messages(DEPENDENCIES std_msgs)
+
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
 ################################################
@@ -200,9 +194,7 @@ add_executable(worldsafety
         src/datalink/worldobject.h
         src/datalink/quaternion.h
         )
-add_executable(talker
-        src/talker.cpp
-        )
+
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -217,7 +209,7 @@ add_executable(talker
 ## Specify libraries to link a library or executable target against
 target_link_libraries(robot_models_node ${catkin_LIBRARIES} )
 target_link_libraries(worldsafety ${catkin_LIBRARIES} )
-target_link_libraries(talker ${catkin_LIBRARIES} )
+
 ##target_link_libraries(safetytest ${catkin_LIBRARIES} )
 #add_dependencies(robot_models_node ${catkin_LIBRARIES})
 
diff --git a/cmake-build-debug/CMakeFiles/Makefile.cmake b/cmake-build-debug/CMakeFiles/Makefile.cmake
index c4159cf89842278ec8a6de893e948b29a13d45c3..80fa80744fd0b6cf03d436df2afd91b52f235837 100644
--- a/cmake-build-debug/CMakeFiles/Makefile.cmake
+++ b/cmake-build-debug/CMakeFiles/Makefile.cmake
@@ -325,7 +325,6 @@ set(CMAKE_MAKEFILE_PRODUCTS
 # Dependency information for all targets:
 set(CMAKE_DEPEND_INFO_FILES
   "CMakeFiles/worldsafety.dir/DependInfo.cmake"
-  "CMakeFiles/safetytest.dir/DependInfo.cmake"
   "CMakeFiles/moveit_ros_manipulation_gencfg.dir/DependInfo.cmake"
   "CMakeFiles/moveit_ros_planning_gencfg.dir/DependInfo.cmake"
   "CMakeFiles/graph_msgs_generate_messages_nodejs.dir/DependInfo.cmake"
diff --git a/cmake-build-debug/CMakeFiles/Makefile2 b/cmake-build-debug/CMakeFiles/Makefile2
index f048ac584ffd09b748422cd280ec939497a524e2..e29cb9071208968784705261f974d08104ec4aa5 100644
--- a/cmake-build-debug/CMakeFiles/Makefile2
+++ b/cmake-build-debug/CMakeFiles/Makefile2
@@ -54,7 +54,6 @@ CMAKE_BINARY_DIR = /home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debu
 
 # The main recursive "all" target.
 all: CMakeFiles/worldsafety.dir/all
-all: CMakeFiles/safetytest.dir/all
 all: CMakeFiles/robot_models_node.dir/all
 all: gtest/all
 
@@ -62,7 +61,6 @@ all: gtest/all
 
 # The main recursive "clean" target.
 clean: CMakeFiles/worldsafety.dir/clean
-clean: CMakeFiles/safetytest.dir/clean
 clean: CMakeFiles/moveit_ros_manipulation_gencfg.dir/clean
 clean: CMakeFiles/moveit_ros_planning_gencfg.dir/clean
 clean: CMakeFiles/graph_msgs_generate_messages_nodejs.dir/clean
@@ -185,7 +183,7 @@ preinstall: gtest/preinstall
 CMakeFiles/worldsafety.dir/all:
 	$(MAKE) -f CMakeFiles/worldsafety.dir/build.make CMakeFiles/worldsafety.dir/depend
 	$(MAKE) -f CMakeFiles/worldsafety.dir/build.make CMakeFiles/worldsafety.dir/build
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles --progress-num=16,17 "Built target worldsafety"
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles --progress-num=14,15 "Built target worldsafety"
 .PHONY : CMakeFiles/worldsafety.dir/all
 
 # Build rule for subdir invocation for target.
@@ -205,33 +203,6 @@ CMakeFiles/worldsafety.dir/clean:
 	$(MAKE) -f CMakeFiles/worldsafety.dir/build.make CMakeFiles/worldsafety.dir/clean
 .PHONY : CMakeFiles/worldsafety.dir/clean
 
-#=============================================================================
-# Target rules for target CMakeFiles/safetytest.dir
-
-# All Build rule for target.
-CMakeFiles/safetytest.dir/all:
-	$(MAKE) -f CMakeFiles/safetytest.dir/build.make CMakeFiles/safetytest.dir/depend
-	$(MAKE) -f CMakeFiles/safetytest.dir/build.make CMakeFiles/safetytest.dir/build
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles --progress-num=14,15 "Built target safetytest"
-.PHONY : CMakeFiles/safetytest.dir/all
-
-# Build rule for subdir invocation for target.
-CMakeFiles/safetytest.dir/rule: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles 2
-	$(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/safetytest.dir/all
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles 0
-.PHONY : CMakeFiles/safetytest.dir/rule
-
-# Convenience name for target.
-safetytest: CMakeFiles/safetytest.dir/rule
-
-.PHONY : safetytest
-
-# clean rule for target.
-CMakeFiles/safetytest.dir/clean:
-	$(MAKE) -f CMakeFiles/safetytest.dir/build.make CMakeFiles/safetytest.dir/clean
-.PHONY : CMakeFiles/safetytest.dir/clean
-
 #=============================================================================
 # Target rules for target CMakeFiles/moveit_ros_manipulation_gencfg.dir
 
diff --git a/cmake-build-debug/CMakeFiles/Progress/12 b/cmake-build-debug/CMakeFiles/Progress/12
new file mode 100644
index 0000000000000000000000000000000000000000..7b4d68d70fcae134d5348f5e118f5e9c9d3f05f6
--- /dev/null
+++ b/cmake-build-debug/CMakeFiles/Progress/12
@@ -0,0 +1 @@
+empty
\ No newline at end of file
diff --git a/cmake-build-debug/CMakeFiles/Progress/count.txt b/cmake-build-debug/CMakeFiles/Progress/count.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0cfbf08886fca9a91cb753ec8734c84fcbe52c9f
--- /dev/null
+++ b/cmake-build-debug/CMakeFiles/Progress/count.txt
@@ -0,0 +1 @@
+2
diff --git a/cmake-build-debug/CMakeFiles/TargetDirectories.txt b/cmake-build-debug/CMakeFiles/TargetDirectories.txt
index 117e574795f9de43d02c0e4d7e599e0dda59398e..7b65098e78e0d6ad2aa3b5738ebb707022426d33 100644
--- a/cmake-build-debug/CMakeFiles/TargetDirectories.txt
+++ b/cmake-build-debug/CMakeFiles/TargetDirectories.txt
@@ -4,7 +4,6 @@
 /home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles/list_install_components.dir
 /home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles/test.dir
 /home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles/worldsafety.dir
-/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles/safetytest.dir
 /home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles/moveit_ros_manipulation_gencfg.dir
 /home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles/moveit_ros_planning_gencfg.dir
 /home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles/graph_msgs_generate_messages_nodejs.dir
diff --git a/cmake-build-debug/CMakeFiles/clion-log.txt b/cmake-build-debug/CMakeFiles/clion-log.txt
index 91fd45c4bb2045a6c1e68243f1fc0dbc6dc1d363..9a9658744a880d2a248bb7ec658a03acaf3b4559 100644
--- a/cmake-build-debug/CMakeFiles/clion-log.txt
+++ b/cmake-build-debug/CMakeFiles/clion-log.txt
@@ -16,13 +16,6 @@
 -- catkin 0.7.28
 -- BUILD_SHARED_LIBS is on
 -- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-CMake Error at /opt/ros/melodic/share/genmsg/cmake/genmsg-extras.cmake:94 (message):
-  add_message_files() directory not found:
-  /home/nikhil/panda_gazebo_workspace/src/zero/msg
-Call Stack (most recent call first):
-  CMakeLists.txt:68 (add_message_files)
-
-
--- Configuring incomplete, errors occurred!
-See also "/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles/CMakeOutput.log".
-See also "/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles/CMakeError.log".
+-- Configuring done
+-- Generating done
+-- Build files have been written to: /home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug
diff --git a/cmake-build-debug/CMakeFiles/progress.marks b/cmake-build-debug/CMakeFiles/progress.marks
index 1e8b314962144c26d5e0e50fd29d2ca327864913..b8626c4cff2849624fb67f87cd0ad72b163671ad 100644
--- a/cmake-build-debug/CMakeFiles/progress.marks
+++ b/cmake-build-debug/CMakeFiles/progress.marks
@@ -1 +1 @@
-6
+4
diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/CXX.includecache b/cmake-build-debug/CMakeFiles/robot_models_node.dir/CXX.includecache
index cc63722908f23ebfb6be68f4cd112eef99f93794..a35af57e673b44fa48495652d02fc391e7011147 100644
--- a/cmake-build-debug/CMakeFiles/robot_models_node.dir/CXX.includecache
+++ b/cmake-build-debug/CMakeFiles/robot_models_node.dir/CXX.includecache
@@ -75,8 +75,6 @@ datalink/hand.h
 /home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/hand.h
 datalink/humanspace.h
 /home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/humanspace.h
-datalink/joint.h
-/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h
 datalink/obstacle.h
 /home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/obstacle.h
 datalink/robot.h
@@ -113,6 +111,8 @@ tf2_geometry_msgs/tf2_geometry_msgs.h
 /home/nikhil/panda_gazebo_workspace/src/zero/src/tf2_geometry_msgs/tf2_geometry_msgs.h
 geometry_msgs/PointStamped.h
 /home/nikhil/panda_gazebo_workspace/src/zero/src/geometry_msgs/PointStamped.h
+datalink/joint.h
+/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h
 tf2_ros/transform_broadcaster.h
 -
 tf2/LinearMath/Quaternion.h
@@ -121,6 +121,12 @@ moveit/move_group_interface/move_group_interface.h
 -
 moveit/planning_scene_interface/planning_scene_interface.h
 -
+std_msgs/UInt32.h
+-
+std_msgs/String.h
+/home/nikhil/panda_gazebo_workspace/src/zero/src/std_msgs/String.h
+sstream
+-
 
 /opt/ros/melodic/include/actionlib/action_definition.h
 
@@ -2602,6 +2608,38 @@ ros/builtin_message_traits.h
 ros/message_operations.h
 -
 
+/opt/ros/melodic/include/std_msgs/String.h
+string
+-
+vector
+-
+map
+-
+ros/types.h
+-
+ros/serialization.h
+-
+ros/builtin_message_traits.h
+-
+ros/message_operations.h
+-
+
+/opt/ros/melodic/include/std_msgs/UInt32.h
+string
+-
+vector
+-
+map
+-
+ros/types.h
+-
+ros/serialization.h
+-
+ros/builtin_message_traits.h
+-
+ros/message_operations.h
+-
+
 /opt/ros/melodic/include/tf2/LinearMath/Matrix3x3.h
 Vector3.h
 /opt/ros/melodic/include/tf2/LinearMath/Vector3.h
diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make b/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make
index d37e2bdb8d9fcd97a2960011879443b69e3e3eab..fe0e1d7c6e17f8d858ed5472a9a836b04db16370 100644
--- a/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make
+++ b/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make
@@ -79,6 +79,106 @@ robot_models_node_EXTERNAL_OBJECTS =
 
 devel/lib/robot_models/robot_models_node: CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o
 devel/lib/robot_models/robot_models_node: CMakeFiles/robot_models_node.dir/build.make
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_visual_tools.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools_gui.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libeigen_conversions.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf_conversions.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libkdl_conversions.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_move_group_interface.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_cpp.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_warehouse.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libwarehouse_ros.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_pick_place_planner.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_rdf_loader.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_model_loader.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_pipeline.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_plan_execution.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_exceptions.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_background_processing.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematics_base.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_model.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_transforms.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_state.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_trajectory.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_interface.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_detection.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_scene.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_constraint_samplers.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_profiler.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_trajectory_processing.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_distance_field.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_distance_field.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_dynamics_solver.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_utils.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_test_utils.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_iostreams.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libfcl.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libkdl_parser.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liburdf.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_sensor.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_model_state.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_model.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_world.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole_bridge.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libsrdfdom.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libtinyxml.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libgeometric_shapes.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liboctomap.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liboctomath.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librandom_numbers.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libclass_loader.so
+devel/lib/robot_models/robot_models_node: /usr/lib/libPocoFoundation.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libdl.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libroslib.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librospack.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libpython2.7.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_program_options.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libtinyxml2.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liborocos-kdl.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf2_ros.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libactionlib.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmessage_filters.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libroscpp.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_filesystem.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole_log4cxx.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole_backend_interface.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liblog4cxx.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_regex.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libxmlrpcpp.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf2.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libroscpp_serialization.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librostime.so
+devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libcpp_common.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_system.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_thread.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_chrono.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_date_time.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_atomic.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libpthread.so
+devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4
 devel/lib/robot_models/robot_models_node: CMakeFiles/robot_models_node.dir/link.txt
 	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Linking CXX executable devel/lib/robot_models/robot_models_node"
 	$(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/robot_models_node.dir/link.txt --verbose=$(VERBOSE)
diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/depend.internal b/cmake-build-debug/CMakeFiles/robot_models_node.dir/depend.internal
index 7d5756594124df3268756b7c2879a0e12ff89c5b..8eaaddffb49202218e2eade8bcb258ae77ea8970 100644
--- a/cmake-build-debug/CMakeFiles/robot_models_node.dir/depend.internal
+++ b/cmake-build-debug/CMakeFiles/robot_models_node.dir/depend.internal
@@ -187,6 +187,8 @@ CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o
  /opt/ros/melodic/include/std_msgs/ColorRGBA.h
  /opt/ros/melodic/include/std_msgs/Empty.h
  /opt/ros/melodic/include/std_msgs/Header.h
+ /opt/ros/melodic/include/std_msgs/String.h
+ /opt/ros/melodic/include/std_msgs/UInt32.h
  /opt/ros/melodic/include/tf2/LinearMath/Matrix3x3.h
  /opt/ros/melodic/include/tf2/LinearMath/MinMax.h
  /opt/ros/melodic/include/tf2/LinearMath/QuadWord.h
diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/depend.make b/cmake-build-debug/CMakeFiles/robot_models_node.dir/depend.make
index 3dcf92078d1ea79d00ac3197ba425b46462a52aa..c15c072c80bb10f0fa703d5dfa9f7b73f436c2b9 100644
--- a/cmake-build-debug/CMakeFiles/robot_models_node.dir/depend.make
+++ b/cmake-build-debug/CMakeFiles/robot_models_node.dir/depend.make
@@ -186,6 +186,8 @@ CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o: /opt/ros/melodic/i
 CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o: /opt/ros/melodic/include/std_msgs/ColorRGBA.h
 CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o: /opt/ros/melodic/include/std_msgs/Empty.h
 CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o: /opt/ros/melodic/include/std_msgs/Header.h
+CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o: /opt/ros/melodic/include/std_msgs/String.h
+CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o: /opt/ros/melodic/include/std_msgs/UInt32.h
 CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o: /opt/ros/melodic/include/tf2/LinearMath/Matrix3x3.h
 CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o: /opt/ros/melodic/include/tf2/LinearMath/MinMax.h
 CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o: /opt/ros/melodic/include/tf2/LinearMath/QuadWord.h
diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt b/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt
index 12ee203c2f4cbced72583ffbd879597e95df4b30..ec80df68feb54f8e140ee7c70a87d69f8fd0c1fa 100644
--- a/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt
+++ b/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt
@@ -1 +1 @@
-/usr/bin/c++  -g  -rdynamic CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o  -o devel/lib/robot_models/robot_models_node 
+/usr/bin/c++  -g  -rdynamic CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o  -o devel/lib/robot_models/robot_models_node -Wl,-rpath,/opt/ros/melodic/lib /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so /opt/ros/melodic/lib/libmoveit_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools_gui.so /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so /opt/ros/melodic/lib/libeigen_conversions.so /opt/ros/melodic/lib/libtf_conversions.so /opt/ros/melodic/lib/libkdl_conversions.so /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so /opt/ros/melodic/lib/libmoveit_move_group_interface.so /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so /opt/ros/melodic/lib/libmoveit_cpp.so /opt/ros/melodic/lib/libmoveit_warehouse.so /opt/ros/melodic/lib/libwarehouse_ros.so /opt/ros/melodic/lib/libtf.so /opt/ros/melodic/lib/libmoveit_pick_place_planner.so /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so /opt/ros/melodic/lib/libmoveit_rdf_loader.so /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so /opt/ros/melodic/lib/libmoveit_robot_model_loader.so /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so /opt/ros/melodic/lib/libmoveit_planning_pipeline.so /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so /opt/ros/melodic/lib/libmoveit_plan_execution.so /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so /opt/ros/melodic/lib/libmoveit_exceptions.so /opt/ros/melodic/lib/libmoveit_background_processing.so /opt/ros/melodic/lib/libmoveit_kinematics_base.so /opt/ros/melodic/lib/libmoveit_robot_model.so /opt/ros/melodic/lib/libmoveit_transforms.so /opt/ros/melodic/lib/libmoveit_robot_state.so /opt/ros/melodic/lib/libmoveit_robot_trajectory.so /opt/ros/melodic/lib/libmoveit_planning_interface.so /opt/ros/melodic/lib/libmoveit_collision_detection.so /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so /opt/ros/melodic/lib/libmoveit_planning_scene.so /opt/ros/melodic/lib/libmoveit_constraint_samplers.so /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so /opt/ros/melodic/lib/libmoveit_profiler.so /opt/ros/melodic/lib/libmoveit_trajectory_processing.so /opt/ros/melodic/lib/libmoveit_distance_field.so /opt/ros/melodic/lib/libmoveit_collision_distance_field.so /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so /opt/ros/melodic/lib/libmoveit_dynamics_solver.so /opt/ros/melodic/lib/libmoveit_utils.so /opt/ros/melodic/lib/libmoveit_test_utils.so -lboost_iostreams -lfcl /opt/ros/melodic/lib/libkdl_parser.so /opt/ros/melodic/lib/liburdf.so -lurdfdom_sensor -lurdfdom_model_state -lurdfdom_model -lurdfdom_world /opt/ros/melodic/lib/librosconsole_bridge.so /opt/ros/melodic/lib/libsrdfdom.so -ltinyxml /opt/ros/melodic/lib/libgeometric_shapes.so /opt/ros/melodic/lib/liboctomap.so /opt/ros/melodic/lib/liboctomath.so /opt/ros/melodic/lib/librandom_numbers.so /opt/ros/melodic/lib/libclass_loader.so -lPocoFoundation -ldl /opt/ros/melodic/lib/libroslib.so /opt/ros/melodic/lib/librospack.so -lpython2.7 -lboost_program_options -ltinyxml2 /opt/ros/melodic/lib/liborocos-kdl.so /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0 /opt/ros/melodic/lib/libtf2_ros.so /opt/ros/melodic/lib/libactionlib.so /opt/ros/melodic/lib/libmessage_filters.so /opt/ros/melodic/lib/libroscpp.so -lboost_filesystem /opt/ros/melodic/lib/librosconsole.so /opt/ros/melodic/lib/librosconsole_log4cxx.so /opt/ros/melodic/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex /opt/ros/melodic/lib/libxmlrpcpp.so /opt/ros/melodic/lib/libtf2.so /opt/ros/melodic/lib/libroscpp_serialization.so /opt/ros/melodic/lib/librostime.so /opt/ros/melodic/lib/libcpp_common.so -lboost_system -lboost_thread -lboost_chrono -lboost_date_time -lboost_atomic -lpthread /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4 
diff --git a/cmake-build-debug/CMakeFiles/worldsafety.dir/build.make b/cmake-build-debug/CMakeFiles/worldsafety.dir/build.make
index 736017e5fa2899e5241787b22f97b214b8a5e02c..00e9b40e23c9dc0ea693a661bcb40364de5c0264 100644
--- a/cmake-build-debug/CMakeFiles/worldsafety.dir/build.make
+++ b/cmake-build-debug/CMakeFiles/worldsafety.dir/build.make
@@ -160,7 +160,6 @@ devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0
 devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libtf2_ros.so
 devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libactionlib.so
 devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libmessage_filters.so
-devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libtf2.so
 devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libroscpp.so
 devel/lib/robot_models/worldsafety: /usr/lib/x86_64-linux-gnu/libboost_filesystem.so
 devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/librosconsole.so
@@ -169,6 +168,7 @@ devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/librosconsole_backend_i
 devel/lib/robot_models/worldsafety: /usr/lib/x86_64-linux-gnu/liblog4cxx.so
 devel/lib/robot_models/worldsafety: /usr/lib/x86_64-linux-gnu/libboost_regex.so
 devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libxmlrpcpp.so
+devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libtf2.so
 devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libroscpp_serialization.so
 devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/librostime.so
 devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libcpp_common.so
diff --git a/cmake-build-debug/CMakeFiles/worldsafety.dir/link.txt b/cmake-build-debug/CMakeFiles/worldsafety.dir/link.txt
index 304ee30ceb768ca4038272679cbc8d2eedee1808..e5d21ee5c4c0583e85f4520e15e07a9c31eb7dcb 100644
--- a/cmake-build-debug/CMakeFiles/worldsafety.dir/link.txt
+++ b/cmake-build-debug/CMakeFiles/worldsafety.dir/link.txt
@@ -1 +1 @@
-/usr/bin/c++  -g  -rdynamic CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o  -o devel/lib/robot_models/worldsafety -Wl,-rpath,/opt/ros/melodic/lib /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so /opt/ros/melodic/lib/libmoveit_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools_gui.so /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so /opt/ros/melodic/lib/libeigen_conversions.so /opt/ros/melodic/lib/libtf_conversions.so /opt/ros/melodic/lib/libkdl_conversions.so /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so /opt/ros/melodic/lib/libmoveit_move_group_interface.so /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so /opt/ros/melodic/lib/libmoveit_cpp.so /opt/ros/melodic/lib/libmoveit_warehouse.so /opt/ros/melodic/lib/libwarehouse_ros.so /opt/ros/melodic/lib/libtf.so /opt/ros/melodic/lib/libmoveit_pick_place_planner.so /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so /opt/ros/melodic/lib/libmoveit_rdf_loader.so /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so /opt/ros/melodic/lib/libmoveit_robot_model_loader.so /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so /opt/ros/melodic/lib/libmoveit_planning_pipeline.so /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so /opt/ros/melodic/lib/libmoveit_plan_execution.so /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so /opt/ros/melodic/lib/libmoveit_exceptions.so /opt/ros/melodic/lib/libmoveit_background_processing.so /opt/ros/melodic/lib/libmoveit_kinematics_base.so /opt/ros/melodic/lib/libmoveit_robot_model.so /opt/ros/melodic/lib/libmoveit_transforms.so /opt/ros/melodic/lib/libmoveit_robot_state.so /opt/ros/melodic/lib/libmoveit_robot_trajectory.so /opt/ros/melodic/lib/libmoveit_planning_interface.so /opt/ros/melodic/lib/libmoveit_collision_detection.so /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so /opt/ros/melodic/lib/libmoveit_planning_scene.so /opt/ros/melodic/lib/libmoveit_constraint_samplers.so /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so /opt/ros/melodic/lib/libmoveit_profiler.so /opt/ros/melodic/lib/libmoveit_trajectory_processing.so /opt/ros/melodic/lib/libmoveit_distance_field.so /opt/ros/melodic/lib/libmoveit_collision_distance_field.so /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so /opt/ros/melodic/lib/libmoveit_dynamics_solver.so /opt/ros/melodic/lib/libmoveit_utils.so /opt/ros/melodic/lib/libmoveit_test_utils.so -lboost_iostreams -lfcl /opt/ros/melodic/lib/libkdl_parser.so /opt/ros/melodic/lib/liburdf.so -lurdfdom_sensor -lurdfdom_model_state -lurdfdom_model -lurdfdom_world /opt/ros/melodic/lib/librosconsole_bridge.so /opt/ros/melodic/lib/libsrdfdom.so -ltinyxml /opt/ros/melodic/lib/libgeometric_shapes.so /opt/ros/melodic/lib/liboctomap.so /opt/ros/melodic/lib/liboctomath.so /opt/ros/melodic/lib/librandom_numbers.so /opt/ros/melodic/lib/libclass_loader.so -lPocoFoundation -ldl /opt/ros/melodic/lib/libroslib.so /opt/ros/melodic/lib/librospack.so -lpython2.7 -lboost_program_options -ltinyxml2 /opt/ros/melodic/lib/liborocos-kdl.so /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0 /opt/ros/melodic/lib/libtf2_ros.so /opt/ros/melodic/lib/libactionlib.so /opt/ros/melodic/lib/libmessage_filters.so /opt/ros/melodic/lib/libtf2.so /opt/ros/melodic/lib/libroscpp.so -lboost_filesystem /opt/ros/melodic/lib/librosconsole.so /opt/ros/melodic/lib/librosconsole_log4cxx.so /opt/ros/melodic/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex /opt/ros/melodic/lib/libxmlrpcpp.so /opt/ros/melodic/lib/libroscpp_serialization.so /opt/ros/melodic/lib/librostime.so /opt/ros/melodic/lib/libcpp_common.so -lboost_system -lboost_thread -lboost_chrono -lboost_date_time -lboost_atomic -lpthread /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4 
+/usr/bin/c++  -g  -rdynamic CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o  -o devel/lib/robot_models/worldsafety -Wl,-rpath,/opt/ros/melodic/lib /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so /opt/ros/melodic/lib/libmoveit_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools_gui.so /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so /opt/ros/melodic/lib/libeigen_conversions.so /opt/ros/melodic/lib/libtf_conversions.so /opt/ros/melodic/lib/libkdl_conversions.so /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so /opt/ros/melodic/lib/libmoveit_move_group_interface.so /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so /opt/ros/melodic/lib/libmoveit_cpp.so /opt/ros/melodic/lib/libmoveit_warehouse.so /opt/ros/melodic/lib/libwarehouse_ros.so /opt/ros/melodic/lib/libtf.so /opt/ros/melodic/lib/libmoveit_pick_place_planner.so /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so /opt/ros/melodic/lib/libmoveit_rdf_loader.so /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so /opt/ros/melodic/lib/libmoveit_robot_model_loader.so /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so /opt/ros/melodic/lib/libmoveit_planning_pipeline.so /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so /opt/ros/melodic/lib/libmoveit_plan_execution.so /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so /opt/ros/melodic/lib/libmoveit_exceptions.so /opt/ros/melodic/lib/libmoveit_background_processing.so /opt/ros/melodic/lib/libmoveit_kinematics_base.so /opt/ros/melodic/lib/libmoveit_robot_model.so /opt/ros/melodic/lib/libmoveit_transforms.so /opt/ros/melodic/lib/libmoveit_robot_state.so /opt/ros/melodic/lib/libmoveit_robot_trajectory.so /opt/ros/melodic/lib/libmoveit_planning_interface.so /opt/ros/melodic/lib/libmoveit_collision_detection.so /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so /opt/ros/melodic/lib/libmoveit_planning_scene.so /opt/ros/melodic/lib/libmoveit_constraint_samplers.so /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so /opt/ros/melodic/lib/libmoveit_profiler.so /opt/ros/melodic/lib/libmoveit_trajectory_processing.so /opt/ros/melodic/lib/libmoveit_distance_field.so /opt/ros/melodic/lib/libmoveit_collision_distance_field.so /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so /opt/ros/melodic/lib/libmoveit_dynamics_solver.so /opt/ros/melodic/lib/libmoveit_utils.so /opt/ros/melodic/lib/libmoveit_test_utils.so -lboost_iostreams -lfcl /opt/ros/melodic/lib/libkdl_parser.so /opt/ros/melodic/lib/liburdf.so -lurdfdom_sensor -lurdfdom_model_state -lurdfdom_model -lurdfdom_world /opt/ros/melodic/lib/librosconsole_bridge.so /opt/ros/melodic/lib/libsrdfdom.so -ltinyxml /opt/ros/melodic/lib/libgeometric_shapes.so /opt/ros/melodic/lib/liboctomap.so /opt/ros/melodic/lib/liboctomath.so /opt/ros/melodic/lib/librandom_numbers.so /opt/ros/melodic/lib/libclass_loader.so -lPocoFoundation -ldl /opt/ros/melodic/lib/libroslib.so /opt/ros/melodic/lib/librospack.so -lpython2.7 -lboost_program_options -ltinyxml2 /opt/ros/melodic/lib/liborocos-kdl.so /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0 /opt/ros/melodic/lib/libtf2_ros.so /opt/ros/melodic/lib/libactionlib.so /opt/ros/melodic/lib/libmessage_filters.so /opt/ros/melodic/lib/libroscpp.so -lboost_filesystem /opt/ros/melodic/lib/librosconsole.so /opt/ros/melodic/lib/librosconsole_log4cxx.so /opt/ros/melodic/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex /opt/ros/melodic/lib/libxmlrpcpp.so /opt/ros/melodic/lib/libtf2.so /opt/ros/melodic/lib/libroscpp_serialization.so /opt/ros/melodic/lib/librostime.so /opt/ros/melodic/lib/libcpp_common.so -lboost_system -lboost_thread -lboost_chrono -lboost_date_time -lboost_atomic -lpthread /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4 
diff --git a/cmake-build-debug/CMakeFiles/worldsafety.dir/progress.make b/cmake-build-debug/CMakeFiles/worldsafety.dir/progress.make
index d7d1be2766f1c3c6a07eb5e96cb9c025648e2fa8..8063b3b41f4abfb7adb2f51e1685368a6a3ad674 100644
--- a/cmake-build-debug/CMakeFiles/worldsafety.dir/progress.make
+++ b/cmake-build-debug/CMakeFiles/worldsafety.dir/progress.make
@@ -1,3 +1,3 @@
-CMAKE_PROGRESS_1 = 16
-CMAKE_PROGRESS_2 = 17
+CMAKE_PROGRESS_1 = 14
+CMAKE_PROGRESS_2 = 15
 
diff --git a/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o b/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o
index 049f7aaf7f1b21f581d07e8c1bd41d0e65751ed6..9b7a9e72089f31c1f85853edfdfd80923b8e1cfc 100644
Binary files a/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o and b/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o differ
diff --git a/cmake-build-debug/Makefile b/cmake-build-debug/Makefile
index 8ae493eedd4f09b1f71c5b496b3dc087063ed57a..07df3ca26d2475061ebc0af1520e26c9795fc8ab 100644
--- a/cmake-build-debug/Makefile
+++ b/cmake-build-debug/Makefile
@@ -180,19 +180,6 @@ worldsafety/fast:
 	$(MAKE) -f CMakeFiles/worldsafety.dir/build.make CMakeFiles/worldsafety.dir/build
 .PHONY : worldsafety/fast
 
-#=============================================================================
-# Target rules for targets named safetytest
-
-# Build rule for target.
-safetytest: cmake_check_build_system
-	$(MAKE) -f CMakeFiles/Makefile2 safetytest
-.PHONY : safetytest
-
-# fast build rule for target.
-safetytest/fast:
-	$(MAKE) -f CMakeFiles/safetytest.dir/build.make CMakeFiles/safetytest.dir/build
-.PHONY : safetytest/fast
-
 #=============================================================================
 # Target rules for targets named moveit_ros_manipulation_gencfg
 
@@ -1650,33 +1637,6 @@ src/robot_models_node.cpp.s:
 	$(MAKE) -f CMakeFiles/robot_models_node.dir/build.make CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.s
 .PHONY : src/robot_models_node.cpp.s
 
-src/safetytest.o: src/safetytest.cpp.o
-
-.PHONY : src/safetytest.o
-
-# target to build an object file
-src/safetytest.cpp.o:
-	$(MAKE) -f CMakeFiles/safetytest.dir/build.make CMakeFiles/safetytest.dir/src/safetytest.cpp.o
-.PHONY : src/safetytest.cpp.o
-
-src/safetytest.i: src/safetytest.cpp.i
-
-.PHONY : src/safetytest.i
-
-# target to preprocess a source file
-src/safetytest.cpp.i:
-	$(MAKE) -f CMakeFiles/safetytest.dir/build.make CMakeFiles/safetytest.dir/src/safetytest.cpp.i
-.PHONY : src/safetytest.cpp.i
-
-src/safetytest.s: src/safetytest.cpp.s
-
-.PHONY : src/safetytest.s
-
-# target to generate assembly for a file
-src/safetytest.cpp.s:
-	$(MAKE) -f CMakeFiles/safetytest.dir/build.make CMakeFiles/safetytest.dir/src/safetytest.cpp.s
-.PHONY : src/safetytest.cpp.s
-
 src/worldsafety.o: src/worldsafety.cpp.o
 
 .PHONY : src/worldsafety.o
@@ -1716,7 +1676,6 @@ help:
 	@echo "... list_install_components"
 	@echo "... test"
 	@echo "... worldsafety"
-	@echo "... safetytest"
 	@echo "... moveit_ros_manipulation_gencfg"
 	@echo "... moveit_ros_planning_gencfg"
 	@echo "... graph_msgs_generate_messages_nodejs"
@@ -1832,9 +1791,6 @@ help:
 	@echo "... src/robot_models_node.o"
 	@echo "... src/robot_models_node.i"
 	@echo "... src/robot_models_node.s"
-	@echo "... src/safetytest.o"
-	@echo "... src/safetytest.i"
-	@echo "... src/safetytest.s"
 	@echo "... src/worldsafety.o"
 	@echo "... src/worldsafety.i"
 	@echo "... src/worldsafety.s"
diff --git a/cmake-build-debug/catkin_generated/stamps/robot_models/package.xml.stamp b/cmake-build-debug/catkin_generated/stamps/robot_models/package.xml.stamp
index bfd7934054c76a7c19e41cf4e697b284417f1abb..2396dbb9305dfd06cd818441d187c1a18976a0ee 100644
--- a/cmake-build-debug/catkin_generated/stamps/robot_models/package.xml.stamp
+++ b/cmake-build-debug/catkin_generated/stamps/robot_models/package.xml.stamp
@@ -1,48 +1,64 @@
 <?xml version="1.0"?>
-<?xml-model
-  href="http://download.ros.org/schema/package_format3.xsd"
-  schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>catkin</name>
-  <version>0.7.28</version>
-  <description>Low-level build system macros and infrastructure for ROS.</description>
-  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
-  <license>BSD</license>
-
-  <url type="website">http://wiki.ros.org/catkin</url>
-  <url type="bugtracker">https://github.com/ros/catkin/issues</url>
-  <url type="repository">https://github.com/ros/catkin</url>
-
-  <author>Troy Straszheim</author>
-  <author>Morten Kjaergaard</author>
-  <author>Brian Gerkey</author>
-  <author>Dirk Thomas</author>
-
-  <depend condition="$ROS_PYTHON_VERSION == 2">python-argparse</depend>
-  <depend condition="$ROS_PYTHON_VERSION == 2" version_gt="0.4.3">python-catkin-pkg</depend>
-  <depend condition="$ROS_PYTHON_VERSION == 3" version_gt="0.4.3">python3-catkin-pkg</depend>
-  <depend condition="$ROS_PYTHON_VERSION == 2">python-empy</depend>
-  <depend condition="$ROS_PYTHON_VERSION == 3">python3-empy</depend>
-
-  <buildtool_depend>cmake</buildtool_depend>
-  <buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-setuptools</buildtool_depend>
-  <buildtool_depend condition="$ROS_PYTHON_VERSION == 3">python3-setuptools</buildtool_depend>
-
-  <buildtool_export_depend>cmake</buildtool_export_depend>
-  <buildtool_export_depend condition="$ROS_PYTHON_VERSION == 3">python3-setuptools</buildtool_export_depend>
-
-  <build_export_depend>google-mock</build_export_depend>
-  <build_export_depend>gtest</build_export_depend>
-  <build_export_depend condition="$ROS_PYTHON_VERSION == 2">python-nose</build_export_depend>
-  <build_export_depend condition="$ROS_PYTHON_VERSION == 3">python3-nose</build_export_depend>
-
-  <test_depend condition="$ROS_PYTHON_VERSION == 2">python-mock</test_depend>
-  <test_depend condition="$ROS_PYTHON_VERSION == 3">python3-mock</test_depend>
-  <test_depend condition="$ROS_PYTHON_VERSION == 2">python-nose</test_depend>
-  <test_depend condition="$ROS_PYTHON_VERSION == 3">python3-nose</test_depend>
+<package format="2">
+  <name>robot_models</name>
+  <version>0.1.0</version>
+  <description>The robot_models package</description>
 
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="nikhil.ambardar@tu-dresden.de">Nikhil Ambardar</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>MIT</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/robot_models</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+  <author email="nikhil.ambardar@tu-dresden.de">Nikhil Ambardar</author>
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>gazebo_ros</depend>
+  <depend>moveit_core</depend>
+  <depend>moveit_visual_tools</depend>
+  <depend>moveit_ros_planning_interface</depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <rosdoc config="rosdoc.yaml"/>
-    <architecture_independent/>
+    <!-- Other tools can request additional information be placed here -->
+
   </export>
 </package>
diff --git a/cmake-build-debug/devel/lib/robot_models/worldsafety b/cmake-build-debug/devel/lib/robot_models/worldsafety
index 1488315a236788ccbc46adef3812467a1024fa93..3f69f7da1fd20e265a7d86ebd18de323a9a197ed 100755
Binary files a/cmake-build-debug/devel/lib/robot_models/worldsafety and b/cmake-build-debug/devel/lib/robot_models/worldsafety differ
diff --git a/cmake-build-debug/robot_models.cbp b/cmake-build-debug/robot_models.cbp
index ca5d9be45955a2baa4c4525131d04fadcd3a4705..52f602d6a1b8ec562d4e5389318c2f2452dbc692 100644
--- a/cmake-build-debug/robot_models.cbp
+++ b/cmake-build-debug/robot_models.cbp
@@ -129,68 +129,6 @@
 					<DistClean command="/usr/bin/make -j4 -f &quot;/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/Makefile&quot;  VERBOSE=1 clean"/>
 				</MakeCommands>
 			</Target>
-			<Target title="safetytest">
-				<Option output="/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/devel/lib/robot_models/safetytest" prefix_auto="0" extension_auto="0"/>
-				<Option working_dir="/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/devel/lib/robot_models"/>
-				<Option object_output="./"/>
-				<Option type="1"/>
-				<Option compiler="gcc"/>
-				<Compiler>
-					<Add option="-DROS_BUILD_SHARED_LIBS=1"/>
-					<Add option="-DROS_PACKAGE_NAME=&quot;robot_models&quot;"/>
-					<Add option="-DROSCONSOLE_BACKEND_LOG4CXX"/>
-					<Add directory="/opt/ros/melodic/include"/>
-					<Add directory="/opt/ros/melodic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp"/>
-					<Add directory="/opt/ros/melodic/share/orocos_kdl/cmake/../../../include"/>
-					<Add directory="/usr/include/eigen3"/>
-					<Add directory="/usr/include/OGRE"/>
-					<Add directory="/usr/include/c++/7"/>
-					<Add directory="/usr/include/x86_64-linux-gnu/c++/7"/>
-					<Add directory="/usr/include/c++/7/backward"/>
-					<Add directory="/usr/lib/gcc/x86_64-linux-gnu/7/include"/>
-					<Add directory="/usr/local/include"/>
-					<Add directory="/usr/lib/gcc/x86_64-linux-gnu/7/include-fixed"/>
-					<Add directory="/usr/include/x86_64-linux-gnu"/>
-					<Add directory="/usr/include"/>
-				</Compiler>
-				<MakeCommands>
-					<Build command="/usr/bin/make -j4 -f &quot;/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/Makefile&quot;  VERBOSE=1 safetytest"/>
-					<CompileFile command="/usr/bin/make -j4 -f &quot;/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/Makefile&quot;  VERBOSE=1 &quot;$file&quot;"/>
-					<Clean command="/usr/bin/make -j4 -f &quot;/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/Makefile&quot;  VERBOSE=1 clean"/>
-					<DistClean command="/usr/bin/make -j4 -f &quot;/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/Makefile&quot;  VERBOSE=1 clean"/>
-				</MakeCommands>
-			</Target>
-			<Target title="safetytest/fast">
-				<Option output="/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/devel/lib/robot_models/safetytest" prefix_auto="0" extension_auto="0"/>
-				<Option working_dir="/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/devel/lib/robot_models"/>
-				<Option object_output="./"/>
-				<Option type="1"/>
-				<Option compiler="gcc"/>
-				<Compiler>
-					<Add option="-DROS_BUILD_SHARED_LIBS=1"/>
-					<Add option="-DROS_PACKAGE_NAME=&quot;robot_models&quot;"/>
-					<Add option="-DROSCONSOLE_BACKEND_LOG4CXX"/>
-					<Add directory="/opt/ros/melodic/include"/>
-					<Add directory="/opt/ros/melodic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp"/>
-					<Add directory="/opt/ros/melodic/share/orocos_kdl/cmake/../../../include"/>
-					<Add directory="/usr/include/eigen3"/>
-					<Add directory="/usr/include/OGRE"/>
-					<Add directory="/usr/include/c++/7"/>
-					<Add directory="/usr/include/x86_64-linux-gnu/c++/7"/>
-					<Add directory="/usr/include/c++/7/backward"/>
-					<Add directory="/usr/lib/gcc/x86_64-linux-gnu/7/include"/>
-					<Add directory="/usr/local/include"/>
-					<Add directory="/usr/lib/gcc/x86_64-linux-gnu/7/include-fixed"/>
-					<Add directory="/usr/include/x86_64-linux-gnu"/>
-					<Add directory="/usr/include"/>
-				</Compiler>
-				<MakeCommands>
-					<Build command="/usr/bin/make -j4 -f &quot;/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/Makefile&quot;  VERBOSE=1 safetytest/fast"/>
-					<CompileFile command="/usr/bin/make -j4 -f &quot;/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/Makefile&quot;  VERBOSE=1 &quot;$file&quot;"/>
-					<Clean command="/usr/bin/make -j4 -f &quot;/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/Makefile&quot;  VERBOSE=1 clean"/>
-					<DistClean command="/usr/bin/make -j4 -f &quot;/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/Makefile&quot;  VERBOSE=1 clean"/>
-				</MakeCommands>
-			</Target>
 			<Target title="moveit_ros_manipulation_gencfg">
 				<Option working_dir="/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug"/>
 				<Option type="4"/>
@@ -1542,65 +1480,51 @@
 		</Build>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/abstractobject.h">
 			<Option target="worldsafety"/>
-			<Option target="safetytest"/>
 			<Option target="robot_models_node"/>
 		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/graspobject.h">
 			<Option target="worldsafety"/>
-			<Option target="safetytest"/>
 			<Option target="robot_models_node"/>
 		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/hand.h">
 			<Option target="worldsafety"/>
-			<Option target="safetytest"/>
 			<Option target="robot_models_node"/>
 		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/humanspace.h">
 			<Option target="worldsafety"/>
-			<Option target="safetytest"/>
 			<Option target="robot_models_node"/>
 		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h">
 			<Option target="worldsafety"/>
-			<Option target="safetytest"/>
 			<Option target="robot_models_node"/>
 		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/obstacle.h">
 			<Option target="worldsafety"/>
-			<Option target="safetytest"/>
 			<Option target="robot_models_node"/>
 		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/quaternion.h">
 			<Option target="worldsafety"/>
-			<Option target="safetytest"/>
 			<Option target="robot_models_node"/>
 		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robot.h">
 			<Option target="worldsafety"/>
-			<Option target="safetytest"/>
 			<Option target="robot_models_node"/>
 		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robotfpe.h">
 			<Option target="worldsafety"/>
-			<Option target="safetytest"/>
 			<Option target="robot_models_node"/>
 		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/world.h">
 			<Option target="worldsafety"/>
-			<Option target="safetytest"/>
 			<Option target="robot_models_node"/>
 		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h">
 			<Option target="worldsafety"/>
-			<Option target="safetytest"/>
 			<Option target="robot_models_node"/>
 		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/robot_models_node.cpp">
 			<Option target="robot_models_node"/>
 		</Unit>
-		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/safetytest.cpp">
-			<Option target="safetytest"/>
-		</Unit>
 		<Unit filename="/home/nikhil/panda_gazebo_workspace/src/zero/src/worldsafety.cpp">
 			<Option target="worldsafety"/>
 		</Unit>
diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp
index 54d1273b4a9ba18aef62d5a32e4a17d967f4692b..be41ced1eb599112bd376b30119707571ed25491 100644
--- a/src/robot_models_node.cpp
+++ b/src/robot_models_node.cpp
@@ -29,10 +29,12 @@
 #include <std_msgs/UInt32.h>
 #include "std_msgs/String.h"
 #include <sstream>
-
-void pub(const std_msgs::String::ConstPtr& msg)
+//use global var gvar
+uint32_t gvar;
+void pub(const std_msgs::UInt32::ConstPtr& msg)
 {
-    ROS_INFO("I heard: [%s]", msg->data.c_str());
+    ROS_INFO("I heard: [%i]", msg->data);
+    gvar= msg->data;
 }
 
     int main(int argc, char **argv) {
@@ -117,14 +119,8 @@ void pub(const std_msgs::String::ConstPtr& msg)
     o9.b2->z = 1.6;
     o9.b2->w = 1.9;
 */
-    ros::Subscriber sub = namespace_name.subscribe("updateDectectedHumans", 1000, pub);
-        o9.humans->humannum = sub;
-        //o9.humans->humannum = 1;
+    ros::Subscriber sub = node_handle.subscribe("updateDectectedHumans", 1000, pub);
     ros::spin();
-    if(o9.humans->humannum>0)
-        ROS_INFO_STREAM("THE ROBOT IS IN AN UNSAFE STATE");
-    if(o9.humans->humannum==0)
-        ROS_INFO_STREAM("THE ROBOT IS IN A SAFE STATE");
 
     tf2_ros::Buffer tfBuffer;
     tf2_ros::TransformListener tfListener(tfBuffer);
@@ -149,6 +145,7 @@ void pub(const std_msgs::String::ConstPtr& msg)
                 transformStamped5 = tfBuffer.lookupTransform("world", robot1.jo[5].name, ros::Time(0));
                 transformStamped6 = tfBuffer.lookupTransform("world", robot1.jo[6].name, ros::Time(0));
                 transformStamped7 = tfBuffer.lookupTransform("world", robot1.jo[7].name, ros::Time(0));
+                o9.humans->humannum =gvar;
             } catch (tf2::TransformException &ex) {
                 ROS_WARN("%s", ex.what());
                 ros::Duration(0.1).sleep();
diff --git a/src/talker.cpp b/src/talker.cpp
deleted file mode 100644
index 280fa3e7ca5cb2690311e83736012ce2f3a46b76..0000000000000000000000000000000000000000
--- a/src/talker.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#include <ros/ros.h>
-#include <tf2_ros/transform_listener.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <tf2_ros/static_transform_broadcaster.h>
-#include "tf2_ros/message_filter.h"
-#include "message_filters/subscriber.h"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
-#include "geometry_msgs/PointStamped.h"
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2/LinearMath/Quaternion.h>
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <std_msgs/UInt32.h>
-#include "std_msgs/String.h"
-#include <sstream>
-
-int talker(int argc, char **argv) {
-    ros::init(argc, argv, "talker");
-    ros::NodeHandle n;
-    ros::Publisher chatter_pub = n.advertise<std_msgs::UInt32>("updateDectectedHumans", 1000);
-    ros::Rate loop_rate(10);
-    int count = 0;
-    while (ros::ok()) {
-        std_msgs::UInt32 msg;
-        std::stringstream ss;
-        ss << "hello world " << count;
-        msg.data = ss.str();
-        ROS_INFO("%s", msg.data);
-        chatter_pub.publish(msg);
-        ros::spinOnce();
-        loop_rate.sleep();
-        ++count;
-    }return 0;
-}
-
diff --git a/src/worldsafety.cpp b/src/worldsafety.cpp
index df71e194afa41967943fbc65205bd850bcf5157a..d8e9aef15c154e6b92125a6d83b2489e2b2dee70 100644
--- a/src/worldsafety.cpp
+++ b/src/worldsafety.cpp
@@ -145,9 +145,10 @@ int main(int argc, char **argv) {
         double epsilon = 0.001; // 1mm deviation is allowed
         double dev_x = 0; // TODO compute dev_x/y using std::abs, the position in
         double dev_y = 0; //      the model and another_pose.position.x/y
-        std::abs(dev_x/dev_y);
-        std::abs(another_pose.position.x/another_pose.position.y);
-        if (false) { // TODO add correct condition
+        dev_x=std::abs(robot1->jo[6].p1[0]-another_pose.position.x);
+        dev_y=std::abs(robot1->jo[6].p1[1]-another_pose.position.y);
+
+        if (dev_x<=epsilon&&dev_y<=epsilon) { // TODO add correct condition
             ROS_INFO_STREAM(" ROBOT MOVED TO INTENDED POSITION. x/y deviations are " << dev_x << "/" << dev_y);
             break; // the test succeeded, we can exit the loop
         } else {