git fix

KW-BBIBBI · Sep 13, 2023 · 140b9f8 · 140b9f8
1 parent 581b973
commit 140b9f8
Show file tree

Hide file tree

Showing 4,759 changed files with 1,196 additions and 472,334 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -1,3 +1,8 @@
 {
-    "cmake.sourceDirectory": "/home/pet/Food-Delivery/catkin_ws/src/ira_laser_tools"
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
 }
diff --git a/catkin_ws/src/.vscode/c_cpp_properties.json b/catkin_ws/src/.vscode/c_cpp_properties.json
diff --git a/catkin_ws/src/.vscode/settings.json b/catkin_ws/src/.vscode/settings.json
diff --git a/catkin_ws/src/CMakeLists.txt b/catkin_ws/src/CMakeLists.txt
diff --git a/catkin_ws/src/CMakeLists.txt b/catkin_ws/src/CMakeLists.txt
@@ -0,0 +1,69 @@
+# toplevel CMakeLists.txt for a catkin workspace
+# catkin/cmake/toplevel.cmake
+
+cmake_minimum_required(VERSION 3.0.2)
+
+project(Project)
+
+set(CATKIN_TOPLEVEL TRUE)
+
+# search for catkin within the workspace
+set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
+execute_process(COMMAND ${_cmd}
+  RESULT_VARIABLE _res
+  OUTPUT_VARIABLE _out
+  ERROR_VARIABLE _err
+  OUTPUT_STRIP_TRAILING_WHITESPACE
+  ERROR_STRIP_TRAILING_WHITESPACE
+)
+if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
+  # searching fot catkin resulted in an error
+  string(REPLACE ";" " " _cmd_str "${_cmd}")
+  message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
+endif()
+
+# include catkin from workspace or via find_package()
+if(_res EQUAL 0)
+  set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
+  # include all.cmake without add_subdirectory to let it operate in same scope
+  include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
+  add_subdirectory("${_out}")
+
+else()
+  # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
+  # or CMAKE_PREFIX_PATH from the environment
+  if(NOT DEFINED CMAKE_PREFIX_PATH)
+    if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
+      if(NOT WIN32)
+        string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      else()
+        set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      endif()
+    endif()
+  endif()
+
+  # list of catkin workspaces
+  set(catkin_search_path "")
+  foreach(path ${CMAKE_PREFIX_PATH})
+    if(EXISTS "${path}/.catkin")
+      list(FIND catkin_search_path ${path} _index)
+      if(_index EQUAL -1)
+        list(APPEND catkin_search_path ${path})
+      endif()
+    endif()
+  endforeach()
+
+  # search for catkin in all workspaces
+  set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
+  find_package(catkin QUIET
+    NO_POLICY_SCOPE
+    PATHS ${catkin_search_path}
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+  unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
+
+  if(NOT catkin_FOUND)
+    message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
+  endif()
+endif()
+
+catkin_workspace()
diff --git a/catkin_ws/src/ira_laser_tools/.gitignore b/catkin_ws/src/ira_laser_tools/.gitignore
diff --git a/catkin_ws/src/ira_laser_tools/CHANGELOG.rst b/catkin_ws/src/ira_laser_tools/CHANGELOG.rst
diff --git a/catkin_ws/src/ira_laser_tools/CMakeLists.txt b/catkin_ws/src/ira_laser_tools/CMakeLists.txt
diff --git a/catkin_ws/src/ira_laser_tools/LICENSE b/catkin_ws/src/ira_laser_tools/LICENSE
diff --git a/catkin_ws/src/ira_laser_tools/README.md b/catkin_ws/src/ira_laser_tools/README.md
diff --git a/catkin_ws/src/ira_laser_tools/launch/laserscan_multi_merger.launch b/catkin_ws/src/ira_laser_tools/launch/laserscan_multi_merger.launch
diff --git a/catkin_ws/src/ira_laser_tools/launch/laserscan_virtualizer.launch b/catkin_ws/src/ira_laser_tools/launch/laserscan_virtualizer.launch
diff --git a/catkin_ws/src/ira_laser_tools/package.xml b/catkin_ws/src/ira_laser_tools/package.xml
diff --git a/catkin_ws/src/ira_laser_tools/src/laserscan_multi_merger.cpp b/catkin_ws/src/ira_laser_tools/src/laserscan_multi_merger.cpp
diff --git a/catkin_ws/src/ira_laser_tools/src/laserscan_virtualizer.cpp b/catkin_ws/src/ira_laser_tools/src/laserscan_virtualizer.cpp
diff --git a/catkin_ws/src/kw/CMakeLists.txt b/catkin_ws/src/kw/CMakeLists.txt
@@ -158,10 +158,10 @@ include_directories(
 
 ## 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}
-# )
+catkin_install_python(PROGRAMS
+  scripts/manager.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark executables for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html

diff --git a/catkin_ws/src/kw/launch/application.launch b/catkin_ws/src/kw/launch/application.launch
@@ -0,0 +1,4 @@
+<launch>
+    <node name="kw_manager" pkg="kw" type="manager.py"/>
+    <node name="kw_save_n_follow" pkg="multiwaypoints" type="save_n_following"/> 
+</launch>
diff --git a/catkin_ws/src/kw/launch/final_.launch b/catkin_ws/src/kw/launch/final_.launch
@@ -0,0 +1,5 @@
+<launch>
+    <include file="$(find kw)/launch/robot_pointcloud.launch" />
+    <include file="$(find kw)/launch/application.launch" />
+    <include file="$(find kw)/launch/navigation.launch" />
+</launch>
diff --git a/catkin_ws/src/kw/launch/lidar_bringup.launch b/catkin_ws/src/kw/launch/lidar_bringup.launch
diff --git a/catkin_ws/src/kw/launch/navigation.launch b/catkin_ws/src/kw/launch/navigation.launch
diff --git a/catkin_ws/src/kw/launch/robot_bringup.launch b/catkin_ws/src/kw/launch/robot_bringup.launch
diff --git a/catkin_ws/src/kw/launch/robot_pointcloud.launch b/catkin_ws/src/kw/launch/robot_pointcloud.launch
@@ -1,13 +1,8 @@
 <launch>
-    <include file="$(find kw)/launch/lidar_bringup.launch" />
-    <include file="$(find scout_bringup)/launch/scout_mini_robot_base.launch" />
-    <include file="$(find witmotion_ros)/launch/witmotion.launch" />
-    <include file="$(find robot_localization)/launch/dual_ekf_navsat_example.launch" />
-    <include file="$(find kw)/launch/navigation.launch" />
+    <include file="$(find kw)/launch/robot_bringup.launch" />
 
     <node pkg="tf" type="static_transform_publisher" name="base_link_to_camera_link" args="0.12 0.0 0.29 0 0 0 base_link camera_link 50"/>
     <include file="$(find realsense2_camera)/launch/demo_pointcloud.launch" />
     <include file="$(find pointcloud_to_laserscan)/launch/sample_node.launch" />
-    <include file="$(find obstacle_detector)/launch/nodes.launch" />
-
+    <include file="$(find obstacle_detector)/launch/nodes.launch" />\    
 </launch>
diff --git a/catkin_ws/src/kw/map.pgm b/catkin_ws/src/kw/map.pgm
diff --git a/catkin_ws/src/kw/maps/map.pgm b/catkin_ws/src/kw/maps/map.pgm
diff --git a/catkin_ws/src/kw/maps/map.yaml b/catkin_ws/src/kw/maps/map.yaml
diff --git a/catkin_ws/src/kw/package.xml b/catkin_ws/src/kw/package.xml
@@ -54,7 +54,10 @@
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <exec_depend>roscpp</exec_depend>
-  <exec_depend>rospy</exec_depend>
+  <exec_depend>rospy</exec_depend>  
+  <build_depend>kw_msgs</build_depend>
+  <exec_depend>kw_msgs</exec_depend>
+  <exec_depend>actionlib</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

diff --git a/gui_ws/src/application/kw/scripts/manager.py → catkin_ws/src/kw/scripts/manager.py b/gui_ws/src/application/kw/scripts/manager.py → catkin_ws/src/kw/scripts/manager.py
@@ -98,6 +98,7 @@ def save(self):
             self.robot_status = RobotMode.SavingThree
 
         self.carrot_client.send_goal(self.action_goal)
+        # self.carrot_client.wait_for_result()
 
 
     def follow(self):

diff --git a/catkin_ws/src/kw/src/rviz/rviz_config.rviz b/catkin_ws/src/kw/src/rviz/rviz_config.rviz
diff --git a/...owing/limo_navigation/wego/CMakeLists.txt → catkin_ws/src/kw_msgs/CMakeLists.txt b/...owing/limo_navigation/wego/CMakeLists.txt → catkin_ws/src/kw_msgs/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(wego)
+project(kw_msgs)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
@@ -10,6 +10,9 @@ project(wego)
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
+  std_msgs
+  message_generation
+  actionlib_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -53,24 +56,23 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  Button.srv
+)
 
 ## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
+add_action_files(
+  FILES
+  Carrot.action
+)
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs  # Or other packages containing msgs
+  actionlib_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -103,8 +105,8 @@ find_package(catkin REQUIRED COMPONENTS
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-#  LIBRARIES wego
-#  CATKIN_DEPENDS roscpp rospy
+#  LIBRARIES kw_msgs
+ CATKIN_DEPENDS roscpp rospy message_runtime actionlib_msgs std_msgs
 #  DEPENDS system_lib
 )
 
@@ -121,7 +123,7 @@ include_directories(
 
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/wego.cpp
+#   src/${PROJECT_NAME}/kw_msgs.cpp
 # )
 
 ## Add cmake target dependencies of the library
@@ -132,7 +134,7 @@ include_directories(
 ## 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/wego_node.cpp)
+# add_executable(${PROJECT_NAME}_node src/kw_msgs_node.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -191,15 +193,12 @@ include_directories(
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
 
-add_executable(receive_esignal src/receive_esignal.cpp)
-target_link_libraries(receive_esignal ${catkin_LIBRARIES})
-
 #############
 ## Testing ##
 #############
 
 ## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_wego.cpp)
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_kw_msgs.cpp)
 # if(TARGET ${PROJECT_NAME}-test)
 #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 # endif()

diff --git a/.../application/kw_msgs/action/Carrot.action → catkin_ws/src/kw_msgs/action/Carrot.action b/.../application/kw_msgs/action/Carrot.action → catkin_ws/src/kw_msgs/action/Carrot.action
diff --git a/gui_ws/src/application/kw_msgs/package.xml → catkin_ws/src/kw_msgs/package.xml b/gui_ws/src/application/kw_msgs/package.xml → catkin_ws/src/kw_msgs/package.xml
diff --git a/...ws/src/application/kw_msgs/srv/Button.srv → catkin_ws/src/kw_msgs/srv/Button.srv b/...ws/src/application/kw_msgs/srv/Button.srv → catkin_ws/src/kw_msgs/srv/Button.srv
diff --git a/catkin_ws/src/laser_filters/.github/workflows/ros1_ci.yml b/catkin_ws/src/laser_filters/.github/workflows/ros1_ci.yml
diff --git a/catkin_ws/src/laser_filters/.gitignore b/catkin_ws/src/laser_filters/.gitignore
diff --git a/catkin_ws/src/laser_filters/CHANGELOG.rst b/catkin_ws/src/laser_filters/CHANGELOG.rst
diff --git a/catkin_ws/src/laser_filters/CMakeLists.txt b/catkin_ws/src/laser_filters/CMakeLists.txt
diff --git a/catkin_ws/src/laser_filters/cfg/BoxFilter.cfg b/catkin_ws/src/laser_filters/cfg/BoxFilter.cfg
diff --git a/catkin_ws/src/laser_filters/doc/node_organization.svg b/catkin_ws/src/laser_filters/doc/node_organization.svg
diff --git a/catkin_ws/src/laser_filters/examples/angle_filter_example.launch b/catkin_ws/src/laser_filters/examples/angle_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/angle_filter_example.yaml b/catkin_ws/src/laser_filters/examples/angle_filter_example.yaml
diff --git a/catkin_ws/src/laser_filters/examples/box_filter.yaml b/catkin_ws/src/laser_filters/examples/box_filter.yaml
diff --git a/catkin_ws/src/laser_filters/examples/box_filter_example.launch b/catkin_ws/src/laser_filters/examples/box_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/footprint_filter_example.launch b/catkin_ws/src/laser_filters/examples/footprint_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/footprint_filter_example.yaml b/catkin_ws/src/laser_filters/examples/footprint_filter_example.yaml
diff --git a/catkin_ws/src/laser_filters/examples/intensity_filter_example.launch b/catkin_ws/src/laser_filters/examples/intensity_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/intensity_filter_example.yaml b/catkin_ws/src/laser_filters/examples/intensity_filter_example.yaml
diff --git a/catkin_ws/src/laser_filters/examples/interpolation_filter_example.launch b/catkin_ws/src/laser_filters/examples/interpolation_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/interpolation_filter_example.yaml b/catkin_ws/src/laser_filters/examples/interpolation_filter_example.yaml
diff --git a/catkin_ws/src/laser_filters/examples/mask_filter_example.launch b/catkin_ws/src/laser_filters/examples/mask_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/mask_filter_example.yaml b/catkin_ws/src/laser_filters/examples/mask_filter_example.yaml
diff --git a/catkin_ws/src/laser_filters/examples/median_filter_5_example.launch b/catkin_ws/src/laser_filters/examples/median_filter_5_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/median_filter_5_example.yaml b/catkin_ws/src/laser_filters/examples/median_filter_5_example.yaml
diff --git a/catkin_ws/src/laser_filters/examples/multiple_filters_example.launch b/catkin_ws/src/laser_filters/examples/multiple_filters_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/multiple_filters_example.yaml b/catkin_ws/src/laser_filters/examples/multiple_filters_example.yaml
diff --git a/catkin_ws/src/laser_filters/examples/pass_through_example.launch b/catkin_ws/src/laser_filters/examples/pass_through_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/pass_through_example.xml b/catkin_ws/src/laser_filters/examples/pass_through_example.xml
diff --git a/catkin_ws/src/laser_filters/examples/point_cloud_footprint_filter_example.yaml b/catkin_ws/src/laser_filters/examples/point_cloud_footprint_filter_example.yaml
diff --git a/catkin_ws/src/laser_filters/examples/polygon_filter.yaml b/catkin_ws/src/laser_filters/examples/polygon_filter.yaml
diff --git a/catkin_ws/src/laser_filters/examples/polygon_filter_example.launch b/catkin_ws/src/laser_filters/examples/polygon_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/range_filter.yaml b/catkin_ws/src/laser_filters/examples/range_filter.yaml
diff --git a/catkin_ws/src/laser_filters/examples/range_filter_example.launch b/catkin_ws/src/laser_filters/examples/range_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/scan_blob_filter_example.launch b/catkin_ws/src/laser_filters/examples/scan_blob_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/scan_blob_filter_example.yaml b/catkin_ws/src/laser_filters/examples/scan_blob_filter_example.yaml
diff --git a/catkin_ws/src/laser_filters/examples/sector_filter.yaml b/catkin_ws/src/laser_filters/examples/sector_filter.yaml
diff --git a/catkin_ws/src/laser_filters/examples/sector_filter_example.launch b/catkin_ws/src/laser_filters/examples/sector_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/shadow_filter_example.launch b/catkin_ws/src/laser_filters/examples/shadow_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/shadow_filter_example.yaml b/catkin_ws/src/laser_filters/examples/shadow_filter_example.yaml
diff --git a/catkin_ws/src/laser_filters/examples/speckle_filter_example.launch b/catkin_ws/src/laser_filters/examples/speckle_filter_example.launch
diff --git a/catkin_ws/src/laser_filters/examples/speckle_filter_example.yaml b/catkin_ws/src/laser_filters/examples/speckle_filter_example.yaml
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/angular_bounds_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/angular_bounds_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/angular_bounds_filter_in_place.h b/catkin_ws/src/laser_filters/include/laser_filters/angular_bounds_filter_in_place.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/array_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/array_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/box_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/box_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/footprint_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/footprint_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/intensity_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/intensity_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/interpolation_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/interpolation_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/median_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/median_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/point_cloud_footprint_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/point_cloud_footprint_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/polygon_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/polygon_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/range_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/range_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/scan_blob_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/scan_blob_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/scan_mask_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/scan_mask_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/scan_shadow_detector.h b/catkin_ws/src/laser_filters/include/laser_filters/scan_shadow_detector.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/scan_shadows_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/scan_shadows_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/sector_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/sector_filter.h
diff --git a/catkin_ws/src/laser_filters/include/laser_filters/speckle_filter.h b/catkin_ws/src/laser_filters/include/laser_filters/speckle_filter.h
diff --git a/catkin_ws/src/laser_filters/laser_filters_plugins.xml b/catkin_ws/src/laser_filters/laser_filters_plugins.xml
diff --git a/catkin_ws/src/laser_filters/mainpage.dox b/catkin_ws/src/laser_filters/mainpage.dox
diff --git a/catkin_ws/src/laser_filters/nodelets.xml b/catkin_ws/src/laser_filters/nodelets.xml
diff --git a/catkin_ws/src/laser_filters/package.xml b/catkin_ws/src/laser_filters/package.xml
diff --git a/catkin_ws/src/laser_filters/src/array_filter.cpp b/catkin_ws/src/laser_filters/src/array_filter.cpp
diff --git a/catkin_ws/src/laser_filters/src/box_filter.cpp b/catkin_ws/src/laser_filters/src/box_filter.cpp
diff --git a/catkin_ws/src/laser_filters/src/generic_laser_filter_node.cpp b/catkin_ws/src/laser_filters/src/generic_laser_filter_node.cpp
diff --git a/catkin_ws/src/laser_filters/src/intensity_filter.cpp b/catkin_ws/src/laser_filters/src/intensity_filter.cpp
diff --git a/catkin_ws/src/laser_filters/src/laser_scan_filters.cpp b/catkin_ws/src/laser_filters/src/laser_scan_filters.cpp
diff --git a/catkin_ws/src/laser_filters/src/median_filter.cpp b/catkin_ws/src/laser_filters/src/median_filter.cpp
diff --git a/catkin_ws/src/laser_filters/src/pointcloud_filters.cpp b/catkin_ws/src/laser_filters/src/pointcloud_filters.cpp
diff --git a/catkin_ws/src/laser_filters/src/polygon_filter.cpp b/catkin_ws/src/laser_filters/src/polygon_filter.cpp
diff --git a/catkin_ws/src/laser_filters/src/scan_shadow_detector.cpp b/catkin_ws/src/laser_filters/src/scan_shadow_detector.cpp
diff --git a/catkin_ws/src/laser_filters/src/scan_shadows_filter.cpp b/catkin_ws/src/laser_filters/src/scan_shadows_filter.cpp
diff --git a/catkin_ws/src/laser_filters/src/scan_to_cloud_filter_chain.cpp b/catkin_ws/src/laser_filters/src/scan_to_cloud_filter_chain.cpp
diff --git a/catkin_ws/src/laser_filters/src/scan_to_scan_filter_chain.cpp b/catkin_ws/src/laser_filters/src/scan_to_scan_filter_chain.cpp
diff --git a/catkin_ws/src/laser_filters/src/sector_filter.cpp b/catkin_ws/src/laser_filters/src/sector_filter.cpp
diff --git a/catkin_ws/src/laser_filters/src/speckle_filter.cpp b/catkin_ws/src/laser_filters/src/speckle_filter.cpp
diff --git a/catkin_ws/src/laser_filters/test/test_polygon_filter.launch b/catkin_ws/src/laser_filters/test/test_polygon_filter.launch
diff --git a/catkin_ws/src/laser_filters/test/test_polygon_filter.yaml b/catkin_ws/src/laser_filters/test/test_polygon_filter.yaml
diff --git a/catkin_ws/src/laser_filters/test/test_scan_filter_chain.cpp b/catkin_ws/src/laser_filters/test/test_scan_filter_chain.cpp
diff --git a/catkin_ws/src/laser_filters/test/test_scan_filter_chain.launch b/catkin_ws/src/laser_filters/test/test_scan_filter_chain.launch
diff --git a/catkin_ws/src/laser_filters/test/test_scan_filter_chain.yaml b/catkin_ws/src/laser_filters/test/test_scan_filter_chain.yaml
diff --git a/catkin_ws/src/laser_filters/test/test_scan_shadows_filter.cpp b/catkin_ws/src/laser_filters/test/test_scan_shadows_filter.cpp
diff --git a/catkin_ws/src/laser_filters/test/test_shadow_detector.cpp b/catkin_ws/src/laser_filters/test/test_shadow_detector.cpp
diff --git a/catkin_ws/src/laser_filters/test/test_speckle_filter.cpp b/catkin_ws/src/laser_filters/test/test_speckle_filter.cpp
diff --git a/catkin_ws/src/laser_filters/test/test_speckle_filter.launch b/catkin_ws/src/laser_filters/test/test_speckle_filter.launch
diff --git a/catkin_ws/src/laser_filters/test/test_speckle_filter_distance.yaml b/catkin_ws/src/laser_filters/test/test_speckle_filter_distance.yaml
diff --git a/catkin_ws/src/laser_filters/test/test_speckle_filter_euclidean.yaml b/catkin_ws/src/laser_filters/test/test_speckle_filter_euclidean.yaml
diff --git a/catkin_ws/src/multiwaypoints/CMakeLists.txt b/catkin_ws/src/multiwaypoints/CMakeLists.txt
@@ -145,8 +145,10 @@ add_executable(save_waypoint src/save_waypoint.cpp)
 target_link_libraries(save_waypoint ${catkin_LIBRARIES})
 add_executable(follow_waypoint src/follow_waypoint.cpp)
 target_link_libraries(follow_waypoint ${catkin_LIBRARIES})
-add_executable(safty src/safty.cpp)
-target_link_libraries(safty ${catkin_LIBRARIES})
+# add_executable(safty src/safty.cpp)
+# target_link_libraries(safty ${catkin_LIBRARIES})
+add_executable(gazebo_safety src/gazebo_safety.cpp)
+target_link_libraries(gazebo_safety ${catkin_LIBRARIES})
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the

diff --git a/catkin_ws/src/multiwaypoints/data/waypoint_one.csv b/catkin_ws/src/multiwaypoints/data/waypoint_one.csv
@@ -0,0 +1 @@
+0.00598418,0.111692,0,0,0.00331074,0.999995
diff --git a/catkin_ws/src/multiwaypoints/data/waypoint_three.csv b/catkin_ws/src/multiwaypoints/data/waypoint_three.csv
@@ -0,0 +1,14 @@
+0.0810586,-0.533927,0,0,0.999888,-0.0149843
+-0.0690323,-0.538389,0,0,0.999886,-0.0151147
+-0.172111,-0.541502,0,0,0.999884,-0.0152127
+-0.356678,-0.547538,0,0,0.999892,-0.0147281
+-0.697704,-0.558062,0,0,0.999864,-0.0164634
+-0.938238,-0.5667,0,0,0.999811,-0.0194179
+-1.07367,-0.572344,0,0,0.999853,-0.0171675
+-1.37693,-0.581084,0,0,0.999877,-0.0156881
+-1.52088,-0.584881,0,0,0.999889,-0.0149302
+-1.7874,-0.595032,0,0,0.999875,-0.0158009
+-1.96491,-0.599778,0,0,0.999875,-0.0157825
+-2.1242,-0.603707,0,0,0.999877,-0.0157143
+-2.2669,-0.607909,0,0,0.999895,-0.0144878
+-2.38574,-0.607434,0,0,0.999739,-0.0228575
diff --git a/catkin_ws/src/multiwaypoints/data/waypoint_two.csv b/catkin_ws/src/multiwaypoints/data/waypoint_two.csv
@@ -0,0 +1,23 @@
+-5.24148,-0.721478,0,0,0.99955,-0.0299879
+-5.11299,-0.869542,0,0,0.173702,-0.984798
+-4.74981,-0.969921,0,0,0.100324,-0.994955
+-4.41811,-1.01884,0,0,0.0570586,-0.998371
+-4.2377,-1.03066,0,0,0.0325065,-0.999472
+-3.99617,-1.03603,0,0,0.01194,-0.999929
+-3.60456,-0.981149,0,0,-0.018575,-0.999827
+-3.44473,-0.954856,0,0,-0.0259181,-0.999664
+-3.28498,-0.927623,0,0,-0.0306013,-0.999532
+-3.18641,-0.910758,0,0,-0.0326171,-0.999468
+-3.08806,-0.892621,0,0,-0.0328397,-0.999461
+-2.99396,-0.858787,0,0,-0.0359679,-0.999353
+-2.81213,-0.824033,0,0,-0.0332585,-0.999447
+-2.71364,-0.806715,0,0,-0.0278407,-0.999612
+-2.61427,-0.79548,0,0,-0.0237454,-0.999718
+-2.51496,-0.783751,0,0,-0.0030382,-0.999995
+-2.41353,-0.821412,0,0,0.243453,-0.969913
+-2.31784,-0.873568,0,0,0.244591,-0.969626
+-2.2285,-0.918484,0,0,0.202973,-0.979184
+-2.13461,-0.952926,0,0,0.125785,-0.992058
+-2.01725,-0.969165,0,0,0.0425579,-0.999094
+-1.91397,-0.970215,0,0,-0.0334379,-0.999441
+-1.81421,-0.963284,0,0,-0.0602776,-0.998182