Add files

2020-09-16 10:05:14 +02:00 · 2020-09-16 10:05:14 +02:00 · a8c94b93eb
commit a8c94b93eb
parent bcfaacd194
851 changed files with 74204 additions and 0 deletions
--- a/Files/mapping_planning_tutorial/control_turtlebot/CMakeLists.txt
+++ b/Files/mapping_planning_tutorial/control_turtlebot/CMakeLists.txt
@ -0,0 +1,223 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(control_turtlebot)
+
+## 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
+  roscpp
+  rospy
+  std_msgs
+  geometry_msgs
+  message_generation
+  laser_geometry
+  tf
+  roslib
+)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system)
+#find_package(Eigen3 REQUIRED)
+#find_package(PCL REQUIRED)
+find_package(Octomap)
+find_package(OMPL REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  GotoWaypoint.srv
+  FindPathToGoal.srv
+)
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs geometry_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES control_turtlebot
+  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+#  DEPENDS Eigen
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+#  ${Eigen_INCLUDE_DIRS}
+  ${OMPL_INCLUDE_DIRS}
+#  ${PCL_INCLUDE_DIRS}
+)
+
+#add_definitions(${EIGEN_DEFINITIONS})
+
+## Declare a C++ library
+# add_library(control_turtlebot
+#   src/${PROJECT_NAME}/control_turtlebot.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(control_turtlebot ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(control_turtlebot_node src/control_turtlebot_node.cpp)
+
+add_executable(laserscan_to_pointcloud src/laserscan_to_pointcloud.cpp)
+target_link_libraries(laserscan_to_pointcloud ${catkin_LIBRARIES} ${Eigen_LIBRARIES} )
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -g")
+
+add_executable(HelloWorld
+src/OMPL_Planning/HelloWorld/Hello_World.cpp
+src/OMPL_Planning/OMPL_Planner/OMPL_Planner.cpp
+src/OMPL_Planning/OMPL_Planner/StateChecker.cpp
+)
+target_link_libraries(HelloWorld ${catkin_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES} ${OMPL_LIBRARIES})
+
+add_executable(Planning_Node
+src/OMPL_Planning/ROS_Implementation/ROS_Planning_Node.cpp
+src/OMPL_Planning/ROS_Implementation/ROS_OMPL_Planner.cpp
+src/OMPL_Planning/OMPL_Planner/OMPL_Planner.cpp
+src/OMPL_Planning/OMPL_Planner/StateChecker.cpp
+)
+target_link_libraries(Planning_Node ${catkin_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES} ${OMPL_LIBRARIES})
+add_dependencies(Planning_Node auv_msgs_generate_messages_cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(control_turtlebot_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(control_turtlebot_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS control_turtlebot control_turtlebot_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_control_turtlebot.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- a/Files/mapping_planning_tutorial/control_turtlebot/launch/config/planner_parameters.yaml
+++ b/Files/mapping_planning_tutorial/control_turtlebot/launch/config/planner_parameters.yaml
@ -0,0 +1,16 @@
+planning_dimension : "2D" # available dimension (2D,3D)
+planner_name : "CForest" # available planners (CForest, RRTstar)
+solving_time : 5.0 # available computation time to attempt to solve the query
+
+#Configuration Space (C-Space) boundaries
+planning_bounds_x : [-15, 15]
+planning_bounds_y : [-15, 15]
+planning_bounds_z : [0, 15]
+
+planning_depth_limit : 1
+
+#Collision Mask
+collision_mask_x : 0.5
+collision_mask_y : 0.5
+collision_mask_z_2D : 0.2
+collision_mask_z_3D : 0.5
--- a/Files/mapping_planning_tutorial/control_turtlebot/launch/config/robot.rviz
+++ b/Files/mapping_planning_tutorial/control_turtlebot/launch/config/robot.rviz
@ -0,0 +1,510 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 81
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /TF1/Tree1
+        - /TF2/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 409
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: odom
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_rgb_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_rgb_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        caster_back_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_front_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        cliff_sensor_front_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_sensor_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_sensor_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        gyro_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        plate_bottom_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        plate_middle_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        plate_top_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_4_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_5_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_kinect_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_kinect_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 0.2972
+        Min Value: 0.2972
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Squares
+      Topic: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Auto Size:
+        Auto Size Factor: 1
+        Value: true
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 3.286
+        Min Value: 0.683
+        Value: true
+      Axis: X
+      Channel Name: intensity
+      Class: rviz/DepthCloud
+      Color: 255; 255; 255
+      Color Image Topic: ""
+      Color Transformer: AxisColor
+      Color Transport Hint: raw
+      Decay Time: 0
+      Depth Map Topic: /camera/depth/image_raw
+      Depth Map Transport Hint: raw
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: DepthCloud
+      Occlusion Compensation:
+        Occlusion Time-Out: 30
+        Value: false
+      Position Transformer: XYZ
+      Queue Size: 5
+      Selectable: true
+      Size (Pixels): 3
+      Style: Points
+      Topic Filter: true
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Auto Size:
+        Auto Size Factor: 1
+        Value: true
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/DepthCloud
+      Color: 255; 255; 255
+      Color Image Topic: /camera/rgb/image_color
+      Color Transformer: RGB8
+      Color Transport Hint: raw
+      Decay Time: 0
+      Depth Map Topic: /camera/depth_registered/image_raw
+      Depth Map Transport Hint: raw
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Registered DepthCloud
+      Occlusion Compensation:
+        Occlusion Time-Out: 30
+        Value: false
+      Position Transformer: XYZ
+      Queue Size: 5
+      Selectable: true
+      Size (Pixels): 3
+      Style: Points
+      Topic Filter: true
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /camera/rgb/image_color
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 3.286
+        Min Value: 0.681
+        Value: true
+      Axis: X
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /camera/depth/points
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 5.632
+        Min Value: 0.65
+        Value: true
+      Axis: X
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Registered PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /camera/depth_registered/points
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: false
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /turtlebot_marker_server/update
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /occupied_cells_vis_array
+      Name: Octomap
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /controller_turtlebot/solution_path
+      Name: Planning Solution
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/MoveCamera
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/Select
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 7.04007
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.102786
+        Y: -0.0889391
+        Z: 0.112978
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.584798
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.2554
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 625
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001950000022bfc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000022b000000dd00fffffffb0000000a0049006d006100670065000000028d000000c60000001600ffffff000000010000010b00000294fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003300000294000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003240000022b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1215
+  X: 58
+  Y: 24
--- a/Files/mapping_planning_tutorial/control_turtlebot/launch/start_turtlebot_mapping_control.launch
+++ b/Files/mapping_planning_tutorial/control_turtlebot/launch/start_turtlebot_mapping_control.launch
@ -0,0 +1,17 @@
+<launch>
+	<arg name="enable_octomap" value="true"/>
+	<arg name="enable_goto_controller" value="false"/>
+    
+	<group if="$(arg enable_octomap)">
+    	<node pkg="octomap_server" type="octomap_server_node" name="octomap_server" args=" " cwd="node" output="screen">
+    		<param name="frame_id"            	  value="/odom" />
+    		<remap from="/cloud_in" to="/camera/depth/points" /> <!-- simulator -->
+    		<!--remap from="/cloud_in" to="/camera/depth_registered/points" /--> <!-- real turtlebot -->
+		</node>
+    </group>
+    
+    <group if="$(arg enable_goto_controller)">
+    	<node pkg="control_turtlebot" type="controller_turtlebot.py" name="controller_turtlebot" respawn="true" output="screen" />
+    </group>
+
+</launch>
--- a/Files/mapping_planning_tutorial/control_turtlebot/launch/start_turtlebot_mapping_planning_control.launch
+++ b/Files/mapping_planning_tutorial/control_turtlebot/launch/start_turtlebot_mapping_planning_control.launch
@ -0,0 +1,35 @@
+<launch>
+	<arg name="enable_octomap" value="true"/>
+	<arg name="enable_controller" value="true"/>
+
+	<arg name="enable_plannerV2" value="true"/>
+    
+	<group if="$(arg enable_octomap)">
+    	<node pkg="octomap_server" type="octomap_server_node" name="octomap_server" args=" " cwd="node" output="screen">
+    		<param name="frame_id"            	  value="/odom" />
+    		<!--remap from="/cloud_in" to="/pc_from_scan" /--> <!-- simulator -->
+    		<param name="resolution"            	  value="0.05" />
+    		<param name="latch"            	  value="True" />
+			<param name="sensor_model/max_range"            	  value="-1" /> <!-- -1 : unlimited -->
+			<param name="sensor_model/hit"            	  value="0.7" /> <!-- default value :0.7 -->
+			<param name="sensor_model/miss"            	  value="0.4" /> <!-- default value :0.4 -->
+
+			<param name="ground_filter/distance"            	  value="0.04" /> <!-- default value :0.04 -->
+			<param name="ground_filter/plane_distance"            	  value="0.07" /> <!-- default value :0.07 -->	
+    		<remap from="/cloud_in" to="/camera/depth/points" /> <!-- real turtlebot -->
+		</node>
+    </group>
+    
+    <node pkg="control_turtlebot" type="laserscan_to_pointcloud" name="laserscan_to_pointcloud" respawn="true" output="screen" />
+    
+    <group if="$(arg enable_controller)">
+    	<node pkg="control_turtlebot" type="controller_turtlebot.py" name="controller_turtlebot" respawn="true" output="screen" />
+    </group>
+
+	<group if="$(arg enable_plannerV2)">
+	    <!-- OMPL planner -->
+		<rosparam command="load" file="$(find control_turtlebot)/launch/config/planner_parameters.yaml" />
+		<node pkg="control_turtlebot" type="Planning_Node" name="Planning_Node" output="screen" />
+	</group>
+
+</launch>
--- a/Files/mapping_planning_tutorial/control_turtlebot/launch/start_turtlebot_modules.launch
+++ b/Files/mapping_planning_tutorial/control_turtlebot/launch/start_turtlebot_modules.launch
@ -0,0 +1,23 @@
+<launch>
+	<arg name="enable_turtlebot_simulation" value="true"/>
+	<arg name="enable_keyboard_teleoperation" value="true"/>
+	<arg name="enable_rviz" value="true"/>
+
+    <group if="$(arg enable_turtlebot_simulation)">
+	    <include file="$(find turtlebot_gazebo)/launch/turtlebot_world.launch"/>  <!-- other ROS distributions -->
+	    <!--include file="$(find turtlebot_gazebo)/launch/turtlebot_playground.launch"/-->  <!-- ROS hydro -->
+
+	    <group if="$(arg enable_rviz)">
+			<include file="$(find control_turtlebot)/launch/view_robot.launch"/>
+		</group>
+    </group>
+    <group unless="$(arg enable_turtlebot_simulation)">
+		<include file="$(find turtlebot_bringup)/launch/minimal.launch"/>
+		<include file="$(find turtlebot_bringup)/launch/3dsensor.launch"/>
+    </group>
+
+    <group if="$(arg enable_keyboard_teleoperation)">
+    	<include file="$(find turtlebot_teleop)/launch/keyboard_teleop.launch">
+    	</include>
+    </group>
+</launch>
--- a/Files/mapping_planning_tutorial/control_turtlebot/launch/start_turtlebot_willowgarage.launch
+++ b/Files/mapping_planning_tutorial/control_turtlebot/launch/start_turtlebot_willowgarage.launch
@ -0,0 +1,7 @@
+<launch>
+    <include file="$(find turtlebot_gazebo)/launch/turtlebot_world.launch">
+        <arg name="world_file" value="$(find control_turtlebot)/worlds/willowgarage.world"/>
+    </include>
+    <include file="$(find turtlebot_teleop)/launch/keyboard_teleop.launch"/>
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find control_turtlebot)/launch/config/robot.rviz"/>
+</launch>
--- a/Files/mapping_planning_tutorial/control_turtlebot/launch/test_planning.launch
+++ b/Files/mapping_planning_tutorial/control_turtlebot/launch/test_planning.launch
@ -0,0 +1,6 @@
+<launch>
+	<!-- Offline planner ompl -->
+	<rosparam command="load" file="$(find control_turtlebot)/launch/config/tests_blocks.yaml" />
+	<node pkg="control_turtlebot" type="tutorial_offline_planner_R2" name="tutorial_offline_planner_R2" output="screen" />
+
+</launch>
--- a/Files/mapping_planning_tutorial/control_turtlebot/launch/view_robot.launch
+++ b/Files/mapping_planning_tutorial/control_turtlebot/launch/view_robot.launch
@ -0,0 +1,8 @@
+<!--
+  Used for visualising turtlebot in action.  
+  
+  It requires minimal.launch and optionally 3dsensor.launch to already be up and running.
+ -->
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find control_turtlebot)/launch/config/robot.rviz" />
+</launch>
--- a/Files/mapping_planning_tutorial/control_turtlebot/missions/mission.yaml
+++ b/Files/mapping_planning_tutorial/control_turtlebot/missions/mission.yaml
@ -0,0 +1,7 @@
+trajectory/north: [8.7958369e-07, 8.7958369e-07, -1.0000000, -1.0000000]
+trajectory/east: [5.8188791e-07, 5.8188791e-07, -1.0000000, -1.0000000]
+trajectory/z: [0.0, 0.25000000, 0.25000000, 0.0]
+trajectory/altitude_mode: [False, False, False, False]
+trajectory/mode: 'los_cte'
+trajectory/time_out: 10
+trajectory/wait: [0.0, 0.0, 0.0, 0.0]
--- a/Files/mapping_planning_tutorial/control_turtlebot/package.xml
+++ b/Files/mapping_planning_tutorial/control_turtlebot/package.xml
@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<package>
+  <name>control_turtlebot</name>
+  <version>0.0.0</version>
+  <description>The control_turtlebot package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="juandhv@todo.todo">juandhv</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>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/control_turtlebot</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <build_depend>message_generation</build_depend>
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <run_depend>message_runtime</run_depend>
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>laser_geometry</build_depend>
+  <build_depend>roslib</build_depend>
+  
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>laser_geometry</run_depend>
+  <run_depend>roslib</run_depend>
+  
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
--- a/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/HelloWorld/Hello_World.cpp
+++ b/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/HelloWorld/Hello_World.cpp
@ -0,0 +1,219 @@
+/**
+ * \file     Hello_World.cpp
+ * \author   Harlé Antoine (antoine.h@flylab.io)
+ * \date     7/2017 - 8/2017
+ * \brief    HelloWorld for OMPL_Planner class.
+ */
+
+#include <iostream>
+
+#include "../OMPL_Planner/OMPL_Planner.hpp"
+
+int main(int argc, char **argv)
+{
+	int planningDimension = 2, solvingTime = 5;
+	std::string plannerName, mapFile;
+	std::vector<double> start, goal, planning_bounds_x_, planning_bounds_y_, planning_bounds_z_;
+
+	planning_bounds_x_.resize(2);
+	planning_bounds_y_.resize(2);
+	planning_bounds_z_.resize(2);
+
+	cout<<"Path planning with OMPL version "<<OMPL_VERSION<<"\n";
+	cout<<"Choose a dimension for planning 2D or 3D (default :"<<planningDimension<<") : ";
+	cin >> planningDimension;
+
+	start.resize(planningDimension);
+	goal.resize(planningDimension);
+
+	cout<<"Choose a planner (available planner : CForest, RRTstar) : ";
+	cin >> plannerName;
+
+	OMPL_Planner planner(planningDimension);
+
+	planning_bounds_x_[0]=-15;
+	planning_bounds_x_[1]=15;
+	planning_bounds_y_[0]=-15;
+	planning_bounds_y_[1]=15;
+	planning_bounds_z_[0]=-15;
+	planning_bounds_z_[1]=15;
+
+	if (planningDimension == 3)
+		planner.setPlanningBounds(planning_bounds_x_, planning_bounds_y_, planning_bounds_z_);
+	else 
+		planner.setPlanningBounds(planning_bounds_x_, planning_bounds_y_);
+
+	planner.setPlanningDepth();
+
+	planner.setPlanner(plannerName);
+
+	cout<<"Enter direction to the Octomap file (.bt file usually) : ";
+	cin>> mapFile;
+	planner.setStateChecker(mapFile,0.5,0.5,0.5);
+
+	cout<<"\n Planner initialization done !\n";
+
+	start[2]=0;
+	goal[2]=0;
+
+	cout<<"Choose a start position (expecting "<< planningDimension <<" values) : ";
+	if(planningDimension==2)
+		cin>> start[0] >> start[1];
+	else
+		cin>> start[0] >> start[1] >> start[2];
+	
+	cout<<"Choose a goal position (expecting "<< planningDimension <<" values) : ";
+	if(planningDimension==2)
+		cin>> goal[0] >> goal[1];
+	else
+		cin>> goal[0] >> goal[1] >> goal[2];
+
+	planner.setStartGoal(start,goal);
+
+	cout<<"Choose solving time (default :"<<solvingTime<<") : ";
+	cin>> solvingTime;
+
+	//cout<<"Searching path from "<<start[0]<<" / "<<start[1]<<" / "<<start[2]<<" to "<<goal[0]<<" / "<<goal[1]<<" / "<<goal[2]<<"\n";
+
+	if(planner.findPath(solvingTime))
+	{
+		og::PathGeometric path = planner.getPath();
+		path.print(cout);
+	}
+	else
+		cout<<"\n No path found\n";
+
+	cout<<"Thanks for using this awsome Hello World !\n";
+
+	return 0;
+}
+
+
+//// DOXYGEN DOC ////
+
+/*! \mainpage Home page
+ *
+ * \section intro_sec Introduction
+ *
+ * This project regroup tools for path planning in 2D or in 3D using OMPL (http://ompl.kavrakilab.org/index.html). 
+ * It also include a ROS implementation for dynamic plannification.
+ *
+ * Work with Ubuntu 14.04 (and ROS Indigo) or with Ubuntu 16.04 (and ROS Kinetic).
+ *
+ * \section install_sec Installation
+ *
+ * \subsection step1 Step 1 : Workspace intialization
+ *
+ * Catkin Workspace : 
+ *		mkdir -p ~/catkin_ws/src
+ *		cd ~/catkin_ws/src
+ *		catkin_init_workspace
+ *		
+ *		cd ~/catkin_ws/
+ *		catkin_make
+ *
+ *		echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
+ *		source ~/.bashrc 
+ *
+ *  \subsection step2 Step 2 : Compilation
+ *
+ *  It's possible to use the Turtlebot or the Hector Quadrotor (ROS Indigo only).
+ *
+ *  \subsubsection ste2a Step 2a : Turtlebot
+ *
+ * Turtlebot dependencies :
+ *
+ *		sudo apt-get install ros-kinetic-turtlebot* ros-kinetic-octomap* ros-kinetic-octovis
+ *		
+ *	    or
+ *
+ *	    sudo apt-get install ros-indigo-turtlebot* ros-indigo-octomap* ros-indigo-octovis
+ *
+ *	Turtlebot install :
+ *
+ *		 copy "mapping_planning_tutorial" folder (in Source Files directory) to "~/catkin_ws/src".
+ *		
+ *		 cd ~/catkin_ws/
+ *
+ *		 catkin_make (mignt need to be performed multiple times if you have error messages with "FindPathToGoal". Warning : it's possible that you won't manage to compile if you don't have enough computation power)
+ *
+ * \subsubsection ste2b Step 2b : Hector Quadrotor (ROS Indigo only)    WARNING : There might need some modification for the package path to make it work
+ *
+ * 		copy "hector_quadrotor_tutorial" folder (in Source Files directory) to "~/catkin_ws/src".
+ *
+ * Differences with Turtlebot
+ *
+ *   		Launch file : start_turtlebot_mapping_planning_control.launch
+ *
+ *     	 	"/odom"  ===> "/world"
+ *
+ *    	  	<node pkg="control_turtlebot" type="controller_turtlebot.py" name="controller_turtlebot" respawn="true" output="screen" />  ===> <node pkg="control_turtlebot" type="controller_quadrotor.py" name="controller_quadrotor" respawn="true" output="screen" />
+ *      
+ *
+ *  ROS_OMPL_Planner.cpp :
+ *
+ *      odom_sub_ = _node_hand.subscribe("/odom", 1, &ROS_OMPL_Planner::odomCallback, this); ==>  odom_sub_ = node_handler_.subscribe("/ground_truth/state", 1, &ROS_OMPL_Planner::odomCallback, this);
+ *
+ *      visual_result_path.header.frame_id = result_path.header.frame_id = q_init_goal.header.frame_id = visual_rrt.header.frame_id =  "/odom"; ===> visual_result_path.header.frame_id = result_path.header.frame_id = q_init_goal.header.frame_id = visual_rrt.header.frame_id =  "/world";
+ *
+ *
+ * \section run Running
+ *
+ * You can run HelloWorld program to understand how works OMPL_Planner alone or you can run the ROS implementation (ROS_OMPL_Planner) which will include the ROS implementation, the visual simulation, the Octomap server, etc.
+ *
+ * \subsection runHello Run HelloWorld
+ *
+ *  	copy the octomap file (.bt) (in Maps directory) you want to use from the Map folder in the project to "~/catkin_ws/devel/lib/control_turtlebot" folder.
+ *
+ *  	cd ~/catkin_ws/devel/lib/control_turtlebot
+ *
+ *  	run HelloWorld (./HelloWorld)
+ *
+ *  Enjoy !
+ * 
+ * \subsection runROS Run ROS implementation
+ *
+ *  	1st terminal (Gazebo / Rviz / Controller) : 
+ *
+ * 		Turtlebot :  roslaunch control_turtlebot start_turtlebot_modules.launch
+ *
+ * 		Quadrotor :  roslaunch hector_quadrotor_demo indoor
+ *       
+ * 	 	2nd terminal (Octomap / Planning): roslaunch control_turtlebot start_turtlebot_mapping_planning_control.launch
+ *      
+ *  	3rd terminal (Planning request) : rosservice call /controller_turtlebot/find_path_to_goal "goal_state_x: 0.0
+ *      	                                                                                       goal_state_y: 0.0
+ *          	                                                                                   goal_state_z: 0.0"
+ *              	                      rosservice call /controller_turtlebot/goto "goal_state_x: 0.0
+ *                  	                                                              goal_state_y: 0.0
+ *                      	                                                          goal_state_z: 0.0" 
+ *
+ * \subsubsection config Configurations files
+ *
+ * See launch folder for runtime configurations.
+ *
+ * In config folder : planner_parameters.yaml
+ *
+ * Contains all the necessary parameters for a basic use of the planner (planning dimension, collision mask, etc.).
+ * 
+ *
+ * See also start_turtlebot_mapping_planning_control.launch file and src folder for a deeper understanding of the ROS implementation.
+ *
+ * \section todo Todo
+ *
+ *	BUG : Compilation sur certain linux : FindPathToGoal.h pb de génération avec cmake
+ *
+ * Frontier exploration
+ *
+ * Clean the dynmacilly allowed memory (Needed for dynamic check)
+ *
+ * Optimisation :
+ * 
+ *   prevent creation of too close state (Ex : start = goal is a problem)
+ *
+ *	 path check : don't check path already done
+ *
+ *	 Improving sending speed of the octomap
+ *
+ *
+ */
--- a/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/OMPL_Planner/OMPL_Planner.cpp
+++ b/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/OMPL_Planner/OMPL_Planner.cpp
@ -0,0 +1,215 @@
+/**
+ * \file      OMPL_Planner.cpp
+ * \author    Harlé Antoine (antoine.h@flylab.io)
+ * \date      7/2017 - 8/2017
+ * \brief     Path planning (without differential constraints) using OMPL with Octomap to represent workspace and to validate if configurations are collision-free.
+ *
+ */
+
+#include "OMPL_Planner.hpp"
+
+OMPL_Planner::OMPL_Planner(int dim) : _planningDimension(dim), _space(new ob::RealVectorStateSpace(dim)), _simple_setup(_space)
+{
+	_si = _simple_setup.getSpaceInformation();
+	/*
+	_start_state.resize(3);
+	_goal_state.resize(3);
+	*/
+}
+
+OMPL_Planner::~OMPL_Planner()
+{
+	/*
+	if(_checker !=nullptr)
+	{
+		delete _checker;
+		_checker = nullptr;
+	}
+	*/
+}
+
+void OMPL_Planner::setPlanningBounds(const std::vector<double> planning_bounds_x, const std::vector<double> planning_bounds_y, const std::vector<double> planning_bounds_z)
+{
+	ob::RealVectorBounds bounds(_planningDimension);
+
+	if(!planning_bounds_z.empty() && _planningDimension ==3)
+	{
+		bounds.setLow(2, planning_bounds_z[0]);
+		bounds.setHigh(2, planning_bounds_z[1]);
+	}
+
+	bounds.setLow(0, planning_bounds_x[0]);
+	bounds.setHigh(0, planning_bounds_x[1]);
+	bounds.setLow(1, planning_bounds_y[0]);
+	bounds.setHigh(1, planning_bounds_y[1]);
+
+	_space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
+}
+
+void OMPL_Planner::setPlanningDepth(double depth_limit)
+{
+	_depth_limit = depth_limit;
+	if(_checker !=nullptr)
+		_checker->setDepthLimit(depth_limit);
+}
+
+void OMPL_Planner::setPlanner(const std::string plannerName)
+{
+	ob::PlannerPtr planner;
+	if(plannerName.compare("CForest")==0)
+		planner = ob::PlannerPtr(new og::CForest(_si)); //Deleted by simple_setup destructor ?
+	else //Default planner
+		planner = ob::PlannerPtr(new og::RRTstar(_si));
+
+	//planner->as<og::RRTstar>()->setRange(0.2);
+	//=======================================================================
+	// Set the setup planner
+	//=======================================================================
+	_simple_setup.setPlanner(planner);
+}
+
+void OMPL_Planner::setStateChecker(const ob::StateValidityCheckerPtr state_checker)
+{
+	if(state_checker != nullptr)
+		_simple_setup.setStateValidityChecker(state_checker);
+	else
+		cout<<"ERROR : invalid pointer on state_checker\n";
+}
+
+void OMPL_Planner::setStateChecker(const std::string mapFile, double collision_mask_x, double collision_mask_y, double collision_mask_z)
+{
+	if(_checker !=nullptr)
+	{
+		delete _checker;
+		_checker = nullptr;
+	}
+	
+	_checker = new StateChecker(_si);
+	_checker->loadMap(mapFile);
+	_checker->setCollisionMask(collision_mask_x,collision_mask_y,collision_mask_z);
+	_checker->setDepthLimit(_depth_limit);
+	_simple_setup.setStateValidityChecker(ob::StateValidityCheckerPtr(_checker));
+}
+
+void OMPL_Planner::setStateChecker(octomap::OcTree* octree, double collision_mask_x, double collision_mask_y, double collision_mask_z)
+{
+	if(_checker !=nullptr)
+	{
+		delete _checker;
+		_checker = nullptr;
+	}
+	
+	_checker = new StateChecker(_si);
+	_checker->loadMap(octree);
+	_checker->setCollisionMask(collision_mask_x,collision_mask_y,collision_mask_z);
+	_checker->setDepthLimit(_depth_limit);
+	_simple_setup.setStateValidityChecker(ob::StateValidityCheckerPtr(_checker));
+}
+
+void OMPL_Planner::updateStateChecker(octomap::OcTree* octree)
+{
+	if(_checker !=nullptr)
+		_checker->loadMap(octree);
+	//No error warned
+}
+
+void OMPL_Planner::setStartGoal(const std::vector<double> start_state, const std::vector<double> goal_state)
+{
+	ob::ScopedState<> start(_space);
+	ob::ScopedState<> goal(_space);
+	start[0] = double(start_state[0]);
+	start[1] = double(start_state[1]);
+
+	goal[0] = double(goal_state[0]);
+	goal[1] = double(goal_state[1]);
+
+	if(_planningDimension==3)
+	{
+		start[2] = double(start_state[2]);
+		goal[2] = double(goal_state[2]);
+
+		//Prevent invalid states because of too low altitude (Octomap issue)
+		if (start[2] < _depth_limit) start[2] = _depth_limit;
+		if (goal[2] < _depth_limit) goal[2] = _depth_limit;
+	}
+
+	_simple_setup.setStartAndGoalStates(start, goal);
+}
+
+bool OMPL_Planner::findPath(double solving_time)
+{
+	//Clear all previous planning data (Planner settings, start/goal states not affected)
+	_simple_setup.clear();
+
+	_simple_setup.setup();
+
+	//_simple_setup.print(cout);
+
+	ob::PlannerStatus solved = _simple_setup.solve( solving_time );
+
+	if (solved && _simple_setup.haveExactSolutionPath())
+	{
+		return true;
+	}
+	else
+		return false;
+}
+
+bool OMPL_Planner::isValid(std::vector<double> position) const
+{
+	if(_checker !=nullptr)
+	{
+		ob::ScopedState<> pos(_space);
+
+		if(_planningDimension==2)
+		{
+			pos[0] = double(position[0]);
+			pos[1] = double(position[1]);
+			return _checker->isValid(pos->as<ob::SE2StateSpace::StateType>());
+		}
+		else
+		{
+			pos[0] = double(position[0]);
+			pos[1] = double(position[1]);
+			
+			//Prevent invalid states because of too low altitude (Octomap issue)
+			if(position[2]<_depth_limit)
+				pos[2]=_depth_limit;
+			else
+				pos[2] = double(position[2]);
+			return _checker->isValid(pos->as<ob::SE3StateSpace::StateType>());
+		}
+	}
+	else
+		return false; //No error warned
+}
+
+bool OMPL_Planner::checkPath() const
+{
+	og::PathGeometric path = getPath();
+
+	return path.check();
+}
+
+bool OMPL_Planner::updatePath(octomap::OcTree* octree, double solving_time, const std::vector<double> start_state, const std::vector<double> goal_state)
+{
+	updateStateChecker(octree);
+
+	if(! checkPath()) //Invalid path
+	{
+		setStartGoal(start_state,goal_state);
+		return findPath(solving_time);
+	}
+	else
+		return true;
+}
+
+og::PathGeometric OMPL_Planner::getPath() const
+{
+	return _simple_setup.getSolutionPath();
+}
+
+og::SimpleSetup OMPL_Planner::getSimpleSetup() const
+{
+	return _simple_setup;
+}
--- a/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/OMPL_Planner/OMPL_Planner.hpp
+++ b/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/OMPL_Planner/OMPL_Planner.hpp
@ -0,0 +1,137 @@
+/**
+ * \file      OMPL_Planner.hpp
+ * \author    Harlé Antoine (antoine.h@flylab.io)
+ * \date      7/2017 - 8/2017
+ * \brief     Path planning (without differential constraints) using OMPL with Octomap to represent workspace and to validate if configurations are collision-free.
+ *
+ */
+
+//#include <string>
+
+//using namespace std;
+
+//OMPL//
+//#include <ompl/config.h>
+//#include <ompl/base/SpaceInformation.h>
+//#include <ompl/base/spaces/RealVectorStateSpace.h>
+#include <ompl/geometric/SimpleSetup.h>
+
+#include <ompl/geometric/planners/cforest/CForest.h>
+#include <ompl/geometric/planners/rrt/RRTstar.h>
+
+#include <ompl/base/spaces/SE3StateSpace.h>
+#include <ompl/base/spaces/SE2StateSpace.h>
+
+//OMPL namespaces
+namespace ob = ompl::base;
+namespace og = ompl::geometric;
+
+#include "StateChecker.hpp"
+
+/**
+ * \class OMPL_Planner
+ * \brief Class for planner for 2D and 3D planning.
+ */
+class OMPL_Planner
+{
+	public :
+
+		//!  Constructor.
+		/*!
+		 * Create space and simple_setup instance for OMPL planning.
+		 * Set planning dimension.
+		*/
+		OMPL_Planner(int dim);
+
+		//! Set the bounds of the planner workspace.
+		/*!
+ 		* Workspace defined by a box with planning_bounds limits.
+ 		* Optional argument : Planning_bounds_z (used only for 3D planning).
+		*/
+		void setPlanningBounds(const std::vector<double> planning_bounds_x, const std::vector<double> planning_bounds_y, const std::vector<double> planning_bounds_z = std::vector<double>());
+		
+		//! Set the planning depth.
+		/*! Set the lowest altitude for states.
+		 * Represent the planning depth (constant) for 2D planning.
+		 * Used to prevent unwanted invalid state (Issue with the ground in Octomap).
+		*/
+		void setPlanningDepth(double depth_limit = 1);
+		
+		//! Set the planning algorithm used to solve the problem.
+		/*!
+		 * Planner avalaible : RRTstar, CForest.
+		 * If no argument provided, RRTstar will be used (reliable planner for most of the situations).
+		 * Implementation of other planners (except control-based planners) is quite simple.
+		*/
+		void setPlanner(const std::string plannerName = "RRTstar");
+
+		//! Set the state checker.
+		/*!
+		 * Check if a given position is collision-free.
+		*/
+		void setStateChecker(const ob::StateValidityCheckerPtr state_checker);
+		void setStateChecker(const std::string mapFile, double collision_mask_x, double collision_mask_y, double collision_mask_z = 0.2);
+		void setStateChecker(octomap::OcTree* octree, double collision_mask_x, double collision_mask_y, double collision_mask_z);
+
+		//! Update the state checker.
+		/*!
+		 * The new map will be used for state checking.
+		 * If the state checker wasn't set before, there's no update and no warning message will be displayed.
+		 */
+		void updateStateChecker(octomap::OcTree* octree);
+
+		//! Set the planning objectives.
+		/*!
+		 * If the state has an altitude lower than the planning depth, it's altitude will be set to the planning depth.
+		 */
+		void setStartGoal(const std::vector<double> start_state, const std::vector<double> goal_state);
+
+		//! Solve the planning problem.
+		/*!
+		 * Find a path from the start state to the goal state in the map given to the state checker.
+		 * The planner will run during solving_time and optimize the solution during this time.
+		 */
+		bool findPath(double solving_time);
+
+		//! Check if position is collision-free.
+		/*!
+		 * Checking is done on the last map given to the state checker.
+		 */
+		bool isValid(std::vector<double> position) const;
+
+		//! Check if the path is valid.
+		bool checkPath() const;
+
+		//! Update the path.
+		/*!
+		 * Search a new path from start_state to goal state if there's a collision with the path and an obstacle from the new map.
+		 * Update the state checker.
+		 * Return True if a new path is found or if the path is collision-free. Return False, if the planner found an obstacle but failed to found a new path.
+		 */
+		bool updatePath(octomap::OcTree* octree, double solving_time, const std::vector<double> start_state, const std::vector<double> goal_state);
+
+		//! Return the path found by the planner.
+		og::PathGeometric getPath() const;
+
+		//! Return the OMPL object containing most of the information of the planning problem.
+		og::SimpleSetup getSimpleSetup() const;
+
+		//!  Destructor.
+		/*!
+		 * Free dynamicly allowed data.
+		*/
+		~OMPL_Planner();
+
+	private :
+
+		int _planningDimension; /*!< 2D or 3D */
+
+		ob::SpaceInformationPtr _si; /*!< Contain all the information on the workspace*/
+		ob::StateSpacePtr _space; /*!< Space in which we're working (2D /3D) */
+		og::SimpleSetup _simple_setup; /*!< OMPL object for planning in simple configurations*/
+
+		StateChecker* _checker = nullptr; /*!< Object for checking if a position is collision-free */
+
+		//std::vector<double> _start_state, _goal_state;
+		double _depth_limit; /*!< lowest altitude for checking (constant depth for 2D checking). Used to prevent unwanted invalid state (Issue with the ground in Octomap). */
+};
--- a/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/OMPL_Planner/StateChecker.cpp
+++ b/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/OMPL_Planner/StateChecker.cpp
@ -0,0 +1,103 @@
+/**
+ * \file      StateChecker.cpp
+ * \author    Harlé Antoine
+ * \date      7/2017 - 8/2017
+ * \brief     Check if a given configuration is collision-free.
+ *  The workspace is represented by an octomap.
+ */
+
+#include "StateChecker.hpp"
+
+StateChecker::StateChecker(const ob::SpaceInformationPtr &si): ob::StateValidityChecker(si)
+{
+	planningDimension = si->getStateDimension();
+}
+
+StateChecker::~StateChecker()
+{
+	if(_map !=nullptr)
+	{
+		delete _map;
+		_map = nullptr;
+	}
+	
+}
+
+void StateChecker::loadMap(const std::string mapFile) //Take a look to memory mangaement for map
+{
+	/*
+	if(_map !=nullptr)
+	{
+		delete _map;
+		_map = nullptr;
+	}
+	*/
+	_map = (new octomap::OcTree(mapFile));
+	_map_res = _map->getResolution();
+}
+
+void StateChecker::loadMap(octomap::OcTree* octree)
+{
+	/*
+	if(_map !=nullptr)
+	{
+		delete _map;
+		_map = nullptr;
+	}
+	*/
+	_map = octree;
+	_map_res = _map->getResolution();
+}
+
+void StateChecker::setCollisionMask(double x, double y, double z)
+{
+	_check_box.x=x;
+	_check_box.y=y;
+	_check_box.z=z;
+}
+
+void StateChecker::setDepthLimit(double depth_limit)
+{
+	_depth_limit = depth_limit;
+}
+
+bool StateChecker::isValid(const ob::State *state) const
+{
+	OcTreeNode* result;
+	point3d query; //Octomap
+	bool collision(false);
+	double node_occupancy;
+
+	// extract the component of the state and cast it to what we expect
+	const ob::RealVectorStateSpace::StateType *pos = state->as<ob::RealVectorStateSpace::StateType>();
+
+	//Constant depth for 2D planning (There might be a better way to implement this)
+	
+	if(planningDimension==2)
+	{
+		pos->values[2]=_depth_limit;
+	}
+	
+	//Recherche dans le collision mask autour de pos
+	for(double xi = pos->values[0]-(_check_box.x/2.0);xi <= pos->values[0]+(_check_box.x/2.0);xi=xi+_map_res)
+		for(double yi = pos->values[1]-(_check_box.y/2.0);yi <= pos->values[1]+(_check_box.y/2.0);yi=yi+_map_res)
+			for(double zi = pos->values[2]-(_check_box.z/2.0);zi <= pos->values[2]+(_check_box.z/2.0);zi=zi+_map_res){ //Trop sensible sur z
+				query.x() = xi;
+				query.y() = yi;
+				query.z() = zi;
+				result = _map->search (query); //Retourne le pointeur de la node à la position query, NULL si elle n'existe pas (terrain inconnu)
+
+				if(result != NULL){ // Node connue
+
+					node_occupancy = result->getOccupancy();
+			
+					if (node_occupancy > 0.4)
+					{
+						collision = true;
+						break;
+					}
+				}
+			}
+
+	return !collision;
+}
--- a/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/OMPL_Planner/StateChecker.hpp
+++ b/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/OMPL_Planner/StateChecker.hpp
@ -0,0 +1,84 @@
+/**
+ * \file      State_Checker.hpp
+ * \author    Harlé Antoine
+ * \date      7/2017 - 8/2017
+ * \brief     Check if a given configuration is collision-free.
+ *  The workspace is represented by an octomap.
+ */
+
+#include <string>
+
+using namespace std;
+
+//OCTOMAP//
+#include <octomap/octomap.h>
+
+using namespace octomap;
+
+//OMPL//
+#include <ompl/config.h>
+#include <ompl/base/SpaceInformation.h>
+#include <ompl/base/spaces/RealVectorStateSpace.h>
+
+namespace ob = ompl::base;
+
+/**
+ * \struct Checking_Box
+ * \brief Collision Mask : virtual representation of the drone as a box with x,y,z length
+ */
+struct Checking_Box
+{
+	double x;
+	double y;
+	double z;
+};
+
+/**
+ * \class StateChecker
+ * \brief Class for state checking for 2D and 3D planning.
+ * Extension of an abstract class used to implement the state validity checker over an octomap (using FCL ?).
+ */
+class StateChecker : public ob::StateValidityChecker
+{
+	public:
+	
+		//! Constructor.
+		/*!
+		 * Call the StateValidityChecker constructor from OMPL library and set checking dimension.
+		*/
+		StateChecker(const ob::SpaceInformationPtr &si);
+
+		//! Load the octomap where the states are checked
+		void loadMap(const std::string mapFile);
+		void loadMap(octomap::OcTree* octree);
+
+		//! Set the area around every state considered for state checking.
+		void setCollisionMask(double x, double y, double z = 0.2);
+
+		//! Set the depth limit (constant for 2D checking)
+		/*! Set the lowest altitude for checking (constant depth for 2D checking).
+		 * Used to prevent unwanted invalid state (Issue with the ground in Octomap)
+		*/
+		void setDepthLimit(double depth_limit);
+
+		//! State validator.
+		/*!
+		 * Function that verifies if the given state is valid (i.e. is free of collision) using FCL.
+		 */
+		virtual bool isValid(const ob::State *state) const;
+
+		//!  Destructor.
+		/*!
+		 * Free dynamicly allowed data.
+		*/
+		~StateChecker();
+
+	private :
+
+		int planningDimension; /*!< 2D or 3D */
+		//Octomap
+		octomap::OcTree* _map = nullptr; /*!< Pointer to the object containing the map (Heavy object ?)*/
+		double _map_res, _depth_limit;
+
+		Checking_Box _check_box; /*!< Collision mask : area around every state considered for state checking. */
+};
--- a/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/ROS_Implementation/ROS_OMPL_Planner.cpp
+++ b/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/ROS_Implementation/ROS_OMPL_Planner.cpp
@ -0,0 +1,467 @@
+/**
+ * \file     ROS_ OMPL_Planner.cpp
+ * \author    Harlé Antoine (antoine.h@flylab.io)
+ * \date      7/2017 - 8/2017
+ * \brief     ROS implementation of OMPL_Planner.
+ */
+
+#include "ROS_OMPL_Planner.hpp"
+
+ROS_OMPL_Planner::ROS_OMPL_Planner(int dimension) : planningDimension(dimension), planner(dimension), _obstacle_flag(true)
+{
+	//=======================================================================
+	// Subscribers.
+	//=======================================================================
+	//Navigation data
+	odom_sub_ = _node_hand.subscribe("/odom", 1, &ROS_OMPL_Planner::odomCallback, this);
+
+	//=======================================================================
+	// Publishers (to visualize the resulting path).
+	//=======================================================================
+	visual_traj_pub_ = _node_hand.advertise<visualization_msgs::Marker> ("/controller_turtlebot/solution_path", 1, true);
+	path_pub_ = _node_hand.advertise<geometry_msgs::PoseArray> ("/Planning_Module/path", 1, true);
+
+	//=======================================================================
+	// Service
+	//=======================================================================
+	service_ = _node_hand.advertiseService("/controller_turtlebot/find_path_to_goal", &ROS_OMPL_Planner::findPathToGoal, this);
+
+	planning_bounds_x_.resize(2);
+	planning_bounds_y_.resize(2);
+	planning_bounds_z_.resize(2);
+	current_position_.resize(3);
+	goal_state.resize(3);
+
+	//=======================================================================
+	// Get parameters from configuration file.
+	//=======================================================================
+	_node_hand.getParam("planning_bounds_x", planning_bounds_x_);
+	_node_hand.getParam("planning_bounds_y", planning_bounds_y_);
+	_node_hand.getParam("planning_bounds_z", planning_bounds_z_);
+
+	_node_hand.getParam("solving_time", solving_time_);
+	_node_hand.getParam("planner_name", planner_name_);
+
+	_node_hand.getParam("collision_mask_x", collision_mask.x);
+	_node_hand.getParam("collision_mask_y", collision_mask.y);
+
+	if(dimension==2)
+		_node_hand.getParam("collision_mask_z_2D", collision_mask.z);
+	else 
+		_node_hand.getParam("collision_mask_z_3D", collision_mask.z);
+
+	_node_hand.getParam("planning_depth_limit", planning_depth_limit);
+
+	//=======================================================================
+	// Planner setup.
+	//=======================================================================
+
+	planner.setPlanningDepth(planning_depth_limit);
+
+	if (dimension == 2)
+		planner.setPlanningBounds(planning_bounds_x_, planning_bounds_y_);
+	else 
+		planner.setPlanningBounds(planning_bounds_x_, planning_bounds_y_, planning_bounds_z_);
+
+	planner.setPlanner(planner_name_);
+
+	updateMap();
+	planner.setStateChecker(octree_, collision_mask.x, collision_mask.y, collision_mask.z);
+
+	ROS_INFO("%s: %dD Planner Init done !", ros::this_node::getName().c_str(), dimension);
+}
+
+bool ROS_OMPL_Planner::updateMap()
+{
+	if(octree_ !=nullptr)
+	{
+	delete octree_;
+	octree_ = nullptr;
+	}
+
+	ROS_INFO("%s: requesting the map to %s...", ros::this_node::getName().c_str(), _node_hand.resolveName(serv_name).c_str());
+
+	while((_node_hand.ok() && !ros::service::call(serv_name, req, resp)) || resp.map.data.size()==0)
+	{
+		ROS_WARN("Request to %s failed; trying again...", _node_hand.resolveName(serv_name).c_str());
+		usleep(1000000);
+	}
+	if (_node_hand.ok()){ // skip when CTRL-C
+		abs_octree_ = octomap_msgs::msgToMap(resp.map);
+
+		if (abs_octree_)
+		{
+			octree_ = dynamic_cast<octomap::OcTree*>(abs_octree_);
+		}
+	
+		if (octree_){
+			ROS_INFO("Octomap received (%zu nodes)", octree_->size());
+			return true;
+			} 
+		else{
+			ROS_ERROR("Error reading OcTree from stream");
+			return false;
+			}
+	}
+}
+
+void ROS_OMPL_Planner::odomCallback(const nav_msgs::OdometryPtr &odom_msg)
+{
+	current_position_[0] = odom_msg->pose.pose.position.x;
+	current_position_[1] = odom_msg->pose.pose.position.y;
+	current_position_[2] = odom_msg->pose.pose.position.z;
+}
+
+bool ROS_OMPL_Planner::findPathToGoal(control_turtlebot::FindPathToGoal::Request& request, control_turtlebot::FindPathToGoal::Response& response)
+{
+	goal_state[0] = request.goal_state_x;
+	goal_state[1] = request.goal_state_y;
+	goal_state[2] = request.goal_state_z;
+
+	//Prevent invalid states because of too low altitude (Octomap issue)
+	if(goal_state[2]<planning_depth_limit) goal_state[2] = planning_depth_limit;
+	if(current_position_[2]<planning_depth_limit) current_position_[2] = planning_depth_limit;
+
+	updateMap();
+	planner.updateStateChecker(octree_);
+
+	// Start and Goal checking
+	if(! planner.isValid(current_position_))
+		ROS_WARN("%s: Start Position %lf/%lf/%lf is invalid !\n",ros::this_node::getName().c_str(), current_position_[0],current_position_[1],current_position_[2]);
+	if(! planner.isValid(goal_state))
+		ROS_WARN("%s: Goal Position %lf/%lf/%lf is invalid !\n",ros::this_node::getName().c_str(), goal_state[0],goal_state[1],goal_state[2]);
+
+	planner.setStartGoal(current_position_, goal_state);
+	
+	ROS_INFO("%s: Searching path from %lf/%lf/%lf to %lf/%lf/%lf ...\n",ros::this_node::getName().c_str(), current_position_[0],current_position_[1],current_position_[2], goal_state[0],goal_state[1],goal_state[2]);
+
+	if(planner.findPath(solving_time_))
+	{
+		_obstacle_flag = false;
+		ROS_INFO("%s: path has been found with simple_setup", ros::this_node::getName().c_str());
+		PublishPath(response);
+	}
+	else
+		ROS_WARN("%s: path has not been found", ros::this_node::getName().c_str());
+
+	return true;
+}
+
+//REALLY Ugly function
+void ROS_OMPL_Planner::visualizeRRT(const og::SimpleSetup simple_setup)
+{
+	og::PathGeometric geopath = simple_setup.getSolutionPath();
+	// %Tag(MARKER_INIT)%
+	visualization_msgs::Marker q_init_goal, visual_rrt, result_path, visual_result_path;
+	visual_result_path.header.frame_id = result_path.header.frame_id = q_init_goal.header.frame_id = visual_rrt.header.frame_id =  "/odom";
+	visual_result_path.header.stamp = result_path.header.stamp = q_init_goal.header.stamp = visual_rrt.header.stamp = ros::Time::now();
+	q_init_goal.ns = "online_planner_points";
+	visual_rrt.ns = "online_planner_rrt";
+	result_path.ns = "online_planner_result";
+	visual_result_path.ns = "online_planner_result_path";
+	visual_result_path.action = result_path.action = q_init_goal.action = visual_rrt.action = visualization_msgs::Marker::ADD;
+
+	visual_result_path.pose.orientation.w = result_path.pose.orientation.w = q_init_goal.pose.orientation.w = visual_rrt.pose.orientation.w = 1.0;
+	// %EndTag(MARKER_INIT)%
+
+	// %Tag(ID)%
+	q_init_goal.id = 0;
+	visual_rrt.id = 1;
+	result_path.id = 2;
+	visual_result_path.id = 3;
+	// %EndTag(ID)%
+
+	// %Tag(TYPE)%
+	result_path.type = q_init_goal.type = visualization_msgs::Marker::POINTS;
+	visual_rrt.type = visual_result_path.type = visualization_msgs::Marker::LINE_LIST;
+	// %EndTag(TYPE)%
+
+	// %Tag(SCALE)%
+	// POINTS markers use x and y scale for width/height respectively
+	result_path.scale.x = q_init_goal.scale.x = 0.5;
+	result_path.scale.y = q_init_goal.scale.y = 0.5;
+	result_path.scale.z = q_init_goal.scale.z = 0.5;
+
+	// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
+	visual_rrt.scale.x = 0.01;
+	visual_result_path.scale.x = 0.02;
+	// %EndTag(SCALE)%
+
+	// %Tag(COLOR)%
+	// Points are green
+	visual_result_path.color.g = 1.0;
+	result_path.color.g = q_init_goal.color.g = 1.0;
+	visual_result_path.color.a = result_path.color.a = q_init_goal.color.a = 1.0;
+
+	// Line strip is blue
+	visual_rrt.color.b = 1.0;
+	visual_rrt.color.a = 1.0;
+
+
+	ob::PlannerData planner_data(simple_setup.getSpaceInformation());
+	simple_setup.getPlannerData(planner_data);
+	std::vector< unsigned int > edgeList;
+	int num_parents;
+	//const ob::SE2StateSpace::StateType *state_se2;
+	const ob::RealVectorStateSpace::StateType *state_r2;
+
+	const ob::RealVectorStateSpace::StateType *state;
+	geometry_msgs::Point p;
+
+	if(planningDimension==3)
+	{
+		for (unsigned int i = 1 ; i < planner_data.numVertices() ; ++i)
+		{
+			if (planner_data.getVertex(i).getState() && planner_data.getIncomingEdges(i,edgeList) > 0)
+			{
+				state_r2 = planner_data.getVertex(i).getState()->as<ob::RealVectorStateSpace::StateType>();
+				p.x = state_r2->values[0];
+				p.y = state_r2->values[1];
+				p.z = state_r2->values[2];//3D change
+				visual_rrt.points.push_back(p);
+
+				state_r2 = planner_data.getVertex(edgeList[0]).getState()->as<ob::RealVectorStateSpace::StateType>();
+				p.x = state_r2->values[0];
+				p.y = state_r2->values[1];
+				p.z = state_r2->values[2];//3D change
+				visual_rrt.points.push_back(p);
+			}
+		}
+		std::vector< ob::State * > states = geopath.getStates();
+
+		for (uint32_t i = 0; i < geopath.getStateCount(); ++i)
+		{
+			// extract the component of the state and cast it to what we expect
+			state = states[i]->as<ob::RealVectorStateSpace::StateType>();
+
+			p.x = state->values[0];
+			p.y = state->values[1];
+			p.z = state->values[2];//3D change
+
+			result_path.points.push_back(p);
+
+			if(i>0)
+			{
+				visual_result_path.points.push_back(p);
+				state = states[i-1]->as<ob::RealVectorStateSpace::StateType>();
+
+				p.x = state->values[0];
+				p.y = state->values[1];
+				p.z = state->values[2];//3D change
+				visual_result_path.points.push_back(p);
+			}
+		}
+	}
+	else
+	{
+		for (unsigned int i = 1 ; i < planner_data.numVertices() ; ++i)
+		{
+			if (planner_data.getVertex(i).getState() && planner_data.getIncomingEdges(i,edgeList) > 0)
+			{
+				state_r2 = planner_data.getVertex(i).getState()->as<ob::RealVectorStateSpace::StateType>();
+				p.x = state_r2->values[0];
+				p.y = state_r2->values[1];
+				p.z = planning_depth_limit; //2D change
+				visual_rrt.points.push_back(p);
+
+				state_r2 = planner_data.getVertex(edgeList[0]).getState()->as<ob::RealVectorStateSpace::StateType>();
+				p.x = state_r2->values[0];
+				p.y = state_r2->values[1];
+				p.z = planning_depth_limit; //2D change
+				visual_rrt.points.push_back(p);
+			}
+		}
+		std::vector< ob::State * > states = geopath.getStates();
+
+		for (uint32_t i = 0; i < geopath.getStateCount(); ++i)
+		{
+			// extract the component of the state and cast it to what we expect
+			state = states[i]->as<ob::RealVectorStateSpace::StateType>();
+
+			p.x = state->values[0];
+			p.y = state->values[1];
+			p.z = planning_depth_limit; //2D change
+
+			result_path.points.push_back(p);
+
+			if(i>0)
+			{
+				visual_result_path.points.push_back(p);
+				state = states[i-1]->as<ob::RealVectorStateSpace::StateType>();
+
+				p.x = state->values[0];
+				p.y = state->values[1];
+				p.z = planning_depth_limit; //2D change
+				visual_result_path.points.push_back(p);
+			}
+		}
+	}
+	//visual_traj_pub_.publish(q_init_goal);
+	visual_traj_pub_.publish(visual_rrt);
+	visual_traj_pub_.publish(visual_result_path);
+	//visual_traj_pub_.publish(result_path);
+}
+
+//Ugly function
+void ROS_OMPL_Planner::PublishPath(control_turtlebot::FindPathToGoal::Response& response)
+{
+	og::PathGeometric path = planner.getPath();
+	//Display path
+	visualizeRRT(planner.getSimpleSetup());
+
+	std::vector< ob::State * > states = path.getStates();
+	const ob::RealVectorStateSpace::StateType *state;
+
+	uint32_t state_nb = path.getStateCount();
+
+	geometry_msgs::PoseArray path_msg;
+
+	//A CHANGER : passage du nombre de state
+	//Passer par stamp dans le header ?
+	geometry_msgs::Pose state_nb_msg;
+	state_nb_msg.position.x=state_nb;
+	path_msg.poses.push_back(state_nb_msg);
+
+	//ROS_INFO("State nb : %d",state_nb);
+	if(planningDimension==3)
+	{
+		for (uint32_t i = 0; i < state_nb; ++i)
+		{
+			geometry_msgs::Pose pose;
+			geometry_msgs::Point point;
+			// extract the component of the state and cast it to what we expect
+			state = states[i]->as<ob::RealVectorStateSpace::StateType>();
+
+			point.x = state->values[0];
+			point.y = state->values[1];
+			point.z = state->values[2];
+				
+			pose.position = point;
+
+			path_msg.poses.push_back(pose);
+			//Send path as response of the planning request
+			response.poses.push_back(pose);
+		}
+	}
+	else
+	{
+		for (uint32_t i = 0; i < state_nb; ++i)
+		{
+			geometry_msgs::Pose pose;
+			geometry_msgs::Point point;
+			// extract the component of the state and cast it to what we expect
+			state = states[i]->as<ob::RealVectorStateSpace::StateType>();
+
+			point.x = state->values[0];
+			point.y = state->values[1];
+			point.z = planning_depth_limit;
+				
+			pose.position = point;
+
+			path_msg.poses.push_back(pose);
+			//Send path as response of the planning request
+			response.poses.push_back(pose);
+		}
+	}
+	//Publish the path on a topic
+	path_pub_.publish(path_msg);
+}
+
+void ROS_OMPL_Planner::PublishPath()
+{
+	og::PathGeometric path = planner.getPath();
+	//Display path
+	visualizeRRT(planner.getSimpleSetup());
+
+	std::vector< ob::State * > states = path.getStates();
+	const ob::RealVectorStateSpace::StateType *state;
+
+	uint32_t state_nb = path.getStateCount();
+
+	geometry_msgs::PoseArray path_msg;
+
+	//A CHANGER : passage du nombre de state
+	//Passer par stamp dans le header ?
+	geometry_msgs::Pose state_nb_msg;
+	state_nb_msg.position.x=state_nb;
+	path_msg.poses.push_back(state_nb_msg);
+
+	//ROS_INFO("State nb : %d",state_nb);
+	if(planningDimension==3)
+	{
+		for (uint32_t i = 0; i < state_nb; ++i)
+		{
+			geometry_msgs::Pose pose;
+			geometry_msgs::Point point;
+			// extract the component of the state and cast it to what we expect
+			state = states[i]->as<ob::RealVectorStateSpace::StateType>();
+
+			point.x = state->values[0];
+			point.y = state->values[1];
+			point.z = state->values[2];
+				
+			pose.position = point;
+
+			path_msg.poses.push_back(pose);
+		}
+	}
+	else
+	{
+		for (uint32_t i = 0; i < state_nb; ++i)
+		{
+			geometry_msgs::Pose pose;
+			geometry_msgs::Point point;
+			// extract the component of the state and cast it to what we expect
+			state = states[i]->as<ob::RealVectorStateSpace::StateType>();
+
+			point.x = state->values[0];
+			point.y = state->values[1];
+			point.z = planning_depth_limit;
+				
+			pose.position = point;
+
+			path_msg.poses.push_back(pose);
+		}
+	}
+	//Publish the path on a topic
+	path_pub_.publish(path_msg);
+}
+
+bool ROS_OMPL_Planner::getObstacleFlag() const
+{
+	return _obstacle_flag;
+}
+
+bool ROS_OMPL_Planner::updatePath()
+{
+	ROS_INFO("%s: Updating path ...\n",ros::this_node::getName().c_str());
+
+	if(current_position_[2]<planning_depth_limit) current_position_[2] = planning_depth_limit;
+
+	updateMap();
+
+	// Start and Goal checking
+	if(! planner.isValid(current_position_))
+		ROS_WARN("%s: Start Position %lf/%lf/%lf is invalid !\n",ros::this_node::getName().c_str(), current_position_[0],current_position_[1],current_position_[2]);
+	if(! planner.isValid(goal_state))
+		ROS_WARN("%s: Goal Position %lf/%lf/%lf is invalid !\n",ros::this_node::getName().c_str(), goal_state[0],goal_state[1],goal_state[2]);
+
+	if(planner.updatePath(octree_, solving_time_, current_position_, goal_state))
+	{
+		_obstacle_flag=false;
+		PublishPath();
+		ROS_INFO("%s: Path update done !\n",ros::this_node::getName().c_str());
+		return true;
+	}
+	else
+	{
+		_obstacle_flag = true;
+		ROS_WARN("%s: Path update failed !\n",ros::this_node::getName().c_str());
+		return false;
+	}
+}
+
+ROS_OMPL_Planner::~ROS_OMPL_Planner()
+{
+
+}
--- a/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/ROS_Implementation/ROS_OMPL_Planner.hpp
+++ b/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/ROS_Implementation/ROS_OMPL_Planner.hpp
@ -0,0 +1,122 @@
+/**
+ * \file      ROS_ OMPL_Planner.hpp
+ * \author    Harlé Antoine (antoine.h@flylab.io)
+ * \date      7/2017 - 8/2017
+ * \brief     ROS implementation of OMPL_Planner.
+ *
+ */
+
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include <nav_msgs/Odometry.h>
+#include <std_msgs/Bool.h>
+#include <geometry_msgs/PoseArray.h>
+#include <ros/package.h>
+#include <std_srvs/Empty.h>
+#include <tf/message_filter.h>
+#include <tf/transform_listener.h>
+#include <control_turtlebot/FindPathToGoal.h>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/Pose.h>
+
+#include <octomap_msgs/conversions.h>
+#include <octomap_msgs/GetOctomap.h>
+
+//ROS-Octomap interface
+using octomap_msgs::GetOctomap;
+
+#include "../OMPL_Planner/OMPL_Planner.hpp"
+
+/**
+ * \class ROS_OMPL_Planner
+ * \brief ROS implementation class for OMPL_Planner.
+ */
+class ROS_OMPL_Planner
+{
+	public:
+		//!  Constructor.
+		/*!
+		 * Load planning parameters from configuration file.
+		 * Create the connection with other ROS nodes.
+		 * Create the OMPL_Planner instance.
+		*/
+		ROS_OMPL_Planner(int dimension);
+
+		//! Request and load the actual map from the Octomap server.
+		bool updateMap();
+
+		//! Callback for getting current vehicle position
+		void odomCallback(const nav_msgs::OdometryPtr &odom_msg);
+
+		//! Find a path
+		/*!
+		 * Find a path from the current position to the goal request.
+		 * Update the state checker, check the validity of the start/goal state.
+		*/
+		bool findPathToGoal(control_turtlebot::FindPathToGoal::Request& request, control_turtlebot::FindPathToGoal::Response& response);
+
+		//! Procedure to visualize the resulting path.
+		/*!
+ 		* Visualize resulting path.
+		*/
+		void visualizeRRT(const og::SimpleSetup simple_setup);
+
+		//! Publish the resulting path.
+		/*!
+		 * Publish the path on a topic and in response of the request.
+		 * Display the path.
+		 * TOPIC : the first state of the path contains the number of states of the path.
+		*/
+		void PublishPath(control_turtlebot::FindPathToGoal::Response& response);
+		
+		//! Publish the resulting path.
+		/*!
+		 * Publish the path on a topic.
+		 * Display the path.
+		 * TOPIC : the first state of the path contains the number of states of the path.
+		*/
+		void PublishPath();
+
+		//! Return the flag representing the validity of the current path.
+		bool getObstacleFlag() const;
+
+		//! Update the path.
+		/*!
+		 * Search a new path from start_state to goal state if there's a collision with the path and an obstacle from the new map.
+		 * Update the state checker and the obstacle_flag.Check the validity of the start/goal state.
+		 * Return True if a new path is found or if the path is collision-free. Return False, if the planner found an obstacle but failed to found a new path.
+		 */
+		bool updatePath();
+
+		//!  Destructor.
+		/*!
+		 * Free dynamicly allowed data.
+		*/
+		~ROS_OMPL_Planner();
+
+	private:
+		OMPL_Planner planner; /*!< Planner for 2D and 3D planning. */
+
+		//Octomap
+		octomap::AbstractOcTree* abs_octree_;
+		octomap::OcTree* octree_ = nullptr; /*!< Pointer to the object containing the map (Heavy object ?).*/
+
+		// *** ROS ***
+		ros::NodeHandle _node_hand;
+		ros::Publisher visual_traj_pub_, path_pub_;
+		ros::Subscriber odom_sub_; 
+		ros::ServiceServer service_; 
+		// Octomap server
+		GetOctomap::Request req;
+		GetOctomap::Response resp;
+		std::string serv_name = "/octomap_full"; /*!< Address of the Octomap server. */
+
+		bool _obstacle_flag; /*!< Flag representing the validity of the current path, whether there is obstacle on the path or not. */
+		int planningDimension;
+		double solving_time_,planning_depth_limit;
+		std::string planner_name_;
+		std::vector<double> current_position_, goal_state;
+		std::vector<double> planning_bounds_x_, planning_bounds_y_, planning_bounds_z_; /*!< Limits of the planning workspace. */
+
+		Checking_Box collision_mask; /*!< Virtual representation of the drone used for collision checking. */
+};
--- a/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/ROS_Implementation/ROS_Planning_Node.cpp
+++ b/Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/ROS_Implementation/ROS_Planning_Node.cpp
@ -0,0 +1,56 @@
+/**
+ * \file     ROS_ Planning_Node.cpp
+ * \author   Harlé Antoine (antoine.h@flylab.io)
+ * \date     7/2017 - 8/2017
+ * \brief    Planning ROS node.
+ *
+ */
+
+#include "ROS_OMPL_Planner.hpp"
+
+int main(int argc, char *argv[])
+{
+	ros::init(argc, argv, "offline_planner_with_services_R2");
+	ros::NodeHandle node_handler_;
+	ROS_INFO("%s: using OMPL version %s", ros::this_node::getName().c_str(), OMPL_VERSION);
+	ompl::msg::setLogLevel(ompl::msg::LOG_NONE);
+
+	std::string planning_dimension;
+	node_handler_.getParam("planning_dimension", planning_dimension);
+
+	
+	if(planning_dimension.compare("2D")==0) //2D planning
+	{
+		//Initialization planner
+		ROS_OMPL_Planner planner(2);
+
+		ros::Rate loop_rate(10);
+		while (ros::ok())
+		{
+			if(! planner.getObstacleFlag()) //Constantly check while the path is valid
+			{
+				planner.updatePath();
+			}
+			ros::spinOnce();
+			loop_rate.sleep();
+		}
+	}
+	else //3D planning
+	{
+		//Initialization planner
+		ROS_OMPL_Planner planner(3);
+
+		ros::Rate loop_rate(10);
+		while (ros::ok())
+		{
+			if(! planner.getObstacleFlag()) //Constantly check while the path is valid
+			{
+				planner.updatePath();
+			}
+			ros::spinOnce();
+			loop_rate.sleep();
+		}
+	}
+
+	return 0;
+}
--- a/Files/mapping_planning_tutorial/control_turtlebot/src/controller_quadrotor.py
+++ b/Files/mapping_planning_tutorial/control_turtlebot/src/controller_quadrotor.py
@ -0,0 +1,149 @@
+#!/usr/bin/env python
+
+# ROS imports
+import roslib; roslib.load_manifest('control_turtlebot')
+import rospy
+import tf
+import math
+
+#ROS messages
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from control_turtlebot.srv import GotoWaypoint, GotoWaypointResponse, FindPathToGoal, FindPathToGoalResponse, FindPathToGoalRequest
+
+#Numpy
+import numpy as np
+
+class Controller(object):
+
+	def __init__(self):
+		self.odometry_sub_ = rospy.Subscriber("/ground_truth/state", Odometry, self.odomCallback, queue_size = 1)
+		self.control_input_pub_ = rospy.Publisher("/cmd_vel", Twist, queue_size = 1)
+		
+		self.serv_ = rospy.Service('/controller_turtlebot/goto', 
+                                  GotoWaypoint, 
+                                  self.calculateControlInput)
+		
+		self.current_position_ = np.zeros(3)
+		self.current_orientation_ = 0.0
+		
+		self.desired_position_ = np.zeros(3)
+		self.desired_orientation_ = 0.0
+		
+		
+		rospy.wait_for_service('/controller_turtlebot/find_path_to_goal')
+		try:
+			self.find_path_to_goal_serv_ = rospy.ServiceProxy('/controller_turtlebot/find_path_to_goal', FindPathToGoal)
+		except rospy.ServiceException, e:
+			print "Service call failed: %s"%e
+		
+		return
+	
+	def calculateControlInput(self, req):
+		
+		planner_request = FindPathToGoalRequest()
+		planner_request.new_goal = True
+		planner_request.goal_state_x = req.goal_state_x
+		planner_request.goal_state_y = req.goal_state_y
+		planner_request.goal_state_z = req.goal_state_z
+		
+		planner_response = self.find_path_to_goal_serv_(planner_request)
+		
+		for pose in planner_response.poses:
+			print pose
+			control_input = Twist()
+			self.desired_position_[0] = pose.position.x
+			self.desired_position_[1] = pose.position.y
+			self.desired_position_[2] = pose.position.z
+	 		
+			loop_rate = rospy.Rate(100) # 10Hz
+			orientation_approach = False
+			while not rospy.is_shutdown():
+				inc_x = self.desired_position_[0] - self.current_position_[0]
+				inc_y = self.desired_position_[1] - self.current_position_[1]
+				inc_z = self.desired_position_[2] - self.current_position_[2]
+	 			
+				self.desired_orientation_ = wrapAngle(math.atan2(inc_y, inc_x))
+				yaw_error = wrapAngle(self.desired_orientation_ - self.current_orientation_)
+				distance_to_goal = math.sqrt(math.pow(inc_x, 2.0) + math.pow(inc_y, 2.0))
+	 			
+				if abs(yaw_error) > 0.04 and not orientation_approach:
+					control_input.angular.x = 0.0
+					control_input.angular.y = 0.0
+					control_input.angular.z = yaw_error * 1.0
+				
+					control_input.linear.x = 0.0
+					control_input.linear.y = 0.0
+					control_input.linear.z = 0.0
+				else:
+					control_input.angular.x = 0.0
+					control_input.angular.y = 0.0
+					control_input.angular.z = 0.0
+					
+					orientation_approach = True
+					liner_speed = abs(distance_to_goal) * 0.5
+					altitude_speed = inc_z * 0.5
+					
+					if liner_speed < 0.1:
+						control_input.linear.x = 0.1
+					elif liner_speed > 0.2:
+						control_input.linear.x = 0.2
+					else :
+						control_input.linear.x = liner_speed
+
+					if abs(altitude_speed) < 0.1:
+						if altitude_speed ==0 :
+							control_input.linear.z = 0
+						elif altitude_speed > 0 :
+							control_input.linear.z = 0.1
+						else:
+							control_input.linear.z = -0.1
+
+					elif abs(altitude_speed) > 0.2:
+						if altitude_speed > 0 :
+							control_input.linear.z = 0.2
+						else:
+							control_input.linear.z = - 0.2
+					else:
+						control_input.linear.z = altitude_speed
+
+					control_input.linear.y = 0.0
+					
+	 				
+				self.control_input_pub_.publish(control_input)
+	 			
+				rospy.logdebug("%s: current position: [%f, %f]\n", rospy.get_name(), self.current_position_[0], self.current_position_[1])
+				rospy.logdebug("%s: desired position: [%f, %f]\n", rospy.get_name(), self.desired_position_[0], self.desired_position_[1])
+				rospy.logdebug("%s: yaw_error: %f\n", rospy.get_name(), yaw_error)
+				rospy.logdebug("%s: current orientation: %f\n", rospy.get_name(), self.current_orientation_)
+				rospy.logdebug("%s: desired orientation: %f\n", rospy.get_name(), self.desired_orientation_)
+				rospy.logdebug("%s: distance_to_goal: %f\n", rospy.get_name(), distance_to_goal)
+				rospy.logdebug("%s: control_input.linear.x %f\n", rospy.get_name(), control_input.linear.x)
+				rospy.logdebug("%s: control_input.angular.z %f\n", rospy.get_name(), control_input.angular.z)
+	 			
+				if distance_to_goal <= 0.2:
+					break
+				loop_rate.sleep()
+		
+		return GotoWaypointResponse()
+
+	def odomCallback(self, odometry_msg):
+		self.current_position_[0] = odometry_msg.pose.pose.position.x
+		self.current_position_[1] = odometry_msg.pose.pose.position.y
+		self.current_position_[2] = odometry_msg.pose.pose.position.z
+		(r, p, y) = tf.transformations.euler_from_quaternion([odometry_msg.pose.pose.orientation.x, odometry_msg.pose.pose.orientation.y, odometry_msg.pose.pose.orientation.z, odometry_msg.pose.pose.orientation.w])
+		self.current_orientation_ = wrapAngle(y)
+		return
+
+def wrapAngle(angle):
+	"""wrapAngle
+	
+	Calculates angles values between 0 and 2pi"""
+	return (angle + ( 2.0 * math.pi * math.floor( ( math.pi - angle ) / ( 2.0 * math.pi ) ) ) )
+
+if __name__ == '__main__':
+    rospy.init_node('control_quadrotor', log_level=rospy.INFO)
+    rospy.loginfo("%s: starting quadrotor controller", rospy.get_name())
+
+    controller = Controller()
+    rospy.spin()
--- a/Files/mapping_planning_tutorial/control_turtlebot/src/controller_turtlebot.py
+++ b/Files/mapping_planning_tutorial/control_turtlebot/src/controller_turtlebot.py
@ -0,0 +1,129 @@
+#!/usr/bin/env python
+
+# ROS imports
+import roslib; roslib.load_manifest('control_turtlebot')
+import rospy
+import tf
+import math
+
+#ROS messages
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from control_turtlebot.srv import GotoWaypoint, GotoWaypointResponse, FindPathToGoal, FindPathToGoalResponse, FindPathToGoalRequest
+
+#Numpy
+import numpy as np
+
+class Controller(object):
+
+	def __init__(self):
+		self.odometry_sub_ = rospy.Subscriber("/odom", Odometry, self.odomCallback, queue_size = 1)
+		self.control_input_pub_ = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size = 1)
+		
+		self.serv_ = rospy.Service('/controller_turtlebot/goto', 
+                                  GotoWaypoint, 
+                                  self.calculateControlInput)
+		
+		self.current_position_ = np.zeros(2)
+		self.current_orientation_ = 0.0
+		
+		self.desired_position_ = np.zeros(2)
+		self.desired_orientation_ = 0.0
+		
+		
+		rospy.wait_for_service('/controller_turtlebot/find_path_to_goal')
+		try:
+			self.find_path_to_goal_serv_ = rospy.ServiceProxy('/controller_turtlebot/find_path_to_goal', FindPathToGoal)
+		except rospy.ServiceException, e:
+			print "Service call failed: %s"%e
+		
+		return
+	
+	def calculateControlInput(self, req):
+		
+		planner_request = FindPathToGoalRequest()
+		planner_request.goal_state_x = req.goal_state_x
+		planner_request.goal_state_y = req.goal_state_y
+		#planner_request.goal_state_z = req.goal_state_z
+		
+		planner_response = self.find_path_to_goal_serv_(planner_request)
+		
+		for pose in planner_response.poses:
+			#print pose
+			control_input = Twist()
+			self.desired_position_[0] = pose.position.x
+			self.desired_position_[1] = pose.position.y
+			#self.desired_position_[2] = pose.position.z
+	 		
+			loop_rate = rospy.Rate(100) # 10Hz
+			orientation_approach = False
+			while not rospy.is_shutdown():
+				inc_x = self.desired_position_[0] - self.current_position_[0]
+				inc_y = self.desired_position_[1] - self.current_position_[1]
+	 			
+				self.desired_orientation_ = wrapAngle(math.atan2(inc_y, inc_x))
+				yaw_error = wrapAngle(self.desired_orientation_ - self.current_orientation_)
+				distance_to_goal = math.sqrt(math.pow(inc_x, 2.0) + math.pow(inc_y, 2.0))
+	 			
+				if abs(yaw_error) > 0.04 and not orientation_approach:
+					control_input.angular.x = 0.0
+					control_input.angular.y = 0.0
+					control_input.angular.z = yaw_error * 1.0
+				
+					control_input.linear.x = 0.08
+					control_input.linear.y = 0.0
+					control_input.linear.z = 0.0
+				else:
+					control_input.angular.x = 0.0
+					control_input.angular.y = 0.0
+					control_input.angular.z = 0.0
+					
+					orientation_approach = True
+					liner_speed = abs(distance_to_goal) * 0.5
+					
+					if liner_speed < 0.1:
+						control_input.linear.x = 0.1
+					elif liner_speed > 0.2:
+						control_input.linear.x = 0.2
+					else:
+						control_input.linear.x = liner_speed
+					
+					control_input.linear.y = 0.0
+					control_input.linear.z = 0.0
+	 				
+				self.control_input_pub_.publish(control_input)
+	 			
+				rospy.logdebug("%s: current position: [%f, %f]\n", rospy.get_name(), self.current_position_[0], self.current_position_[1])
+				rospy.logdebug("%s: desired position: [%f, %f]\n", rospy.get_name(), self.desired_position_[0], self.desired_position_[1])
+				rospy.logdebug("%s: yaw_error: %f\n", rospy.get_name(), yaw_error)
+				rospy.logdebug("%s: current orientation: %f\n", rospy.get_name(), self.current_orientation_)
+				rospy.logdebug("%s: desired orientation: %f\n", rospy.get_name(), self.desired_orientation_)
+				rospy.logdebug("%s: distance_to_goal: %f\n", rospy.get_name(), distance_to_goal)
+				rospy.logdebug("%s: control_input.linear.x %f\n", rospy.get_name(), control_input.linear.x)
+				rospy.logdebug("%s: control_input.angular.z %f\n", rospy.get_name(), control_input.angular.z)
+	 			
+				if distance_to_goal <= 0.2:
+					break
+				loop_rate.sleep()
+		
+		return GotoWaypointResponse()
+
+	def odomCallback(self, odometry_msg):
+		self.current_position_[0] = odometry_msg.pose.pose.position.x
+		self.current_position_[1] = odometry_msg.pose.pose.position.y
+		(r, p, y) = tf.transformations.euler_from_quaternion([odometry_msg.pose.pose.orientation.x, odometry_msg.pose.pose.orientation.y, odometry_msg.pose.pose.orientation.z, odometry_msg.pose.pose.orientation.w])
+		self.current_orientation_ = wrapAngle(y)
+		return
+
+def wrapAngle(angle):
+	"""wrapAngle
+	
+	Calculates angles values between 0 and 2pi"""
+	return (angle + ( 2.0 * math.pi * math.floor( ( math.pi - angle ) / ( 2.0 * math.pi ) ) ) )
+
+if __name__ == '__main__':
+    rospy.init_node('control_turtlebot', log_level=rospy.INFO)
+    rospy.loginfo("%s: starting turtlebot controller", rospy.get_name())
+
+    controller = Controller()
+    rospy.spin()
--- a/Files/mapping_planning_tutorial/control_turtlebot/src/laserscan_to_pointcloud.cpp
+++ b/Files/mapping_planning_tutorial/control_turtlebot/src/laserscan_to_pointcloud.cpp
@ -0,0 +1,136 @@
+// ROS
+#include <ros/ros.h>
+
+// ROS LaserScan tools
+#include <laser_geometry/laser_geometry.h>
+
+// ROS messages
+#include <geometry_msgs/Pose.h>
+#include <message_filters/subscriber.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/PointCloud.h>
+#include <std_msgs/ColorRGBA.h>
+#include <std_msgs/Bool.h>
+#include <visualization_msgs/MarkerArray.h>
+
+// ROS services
+#include <std_srvs/Empty.h>
+
+// ROS tf
+#include <tf/message_filter.h>
+#include <tf/transform_listener.h>
+
+// Octomap
+#include <octomap/octomap.h>
+#include <octomap_msgs/conversions.h>
+//#include <octomap_msgs/OctomapBinary.h>
+#include <octomap_msgs/GetOctomap.h>
+typedef octomap_msgs::GetOctomap OctomapSrv;
+//#include <laser_octomap/BoundingBoxQuery.h>
+//typedef laser_octomap::BoundingBoxQuery BBXSrv;
+#include <octomap_ros/conversions.h>
+
+#include <signal.h>
+
+
+void stopNode(int sig)
+{
+	ros::shutdown();
+	exit(0);
+}
+
+/// CLASS DEFINITION ===========================================================
+class LaserScanToPointCloud
+{
+    public:
+        // Constructor and destructor
+        LaserScanToPointCloud();
+//        virtual ~LaserScanToPointCloud();
+
+        // Callbacks
+        void laserScanCallback(const sensor_msgs::LaserScanConstPtr& scan);
+
+    private:
+
+        // ROS
+        ros::NodeHandle node_hand_;
+        ros::Subscriber laser_scan_sub_;
+        ros::Publisher pc_pub_;
+
+        //TF
+        tf::TransformListener listener_;
+
+        //PC
+        laser_geometry::LaserProjection projector_;
+};
+
+/// Constructor and destructor =================================================
+LaserScanToPointCloud::LaserScanToPointCloud(){
+    //=======================================================================
+    // Subscribers
+    //=======================================================================
+    //Mission Flag (feedback)
+	laser_scan_sub_ = node_hand_.subscribe("/scan", 2, &LaserScanToPointCloud::laserScanCallback, this);
+
+	//=======================================================================
+	// Publishers
+	//=======================================================================
+	pc_pub_ = node_hand_.advertise<sensor_msgs::PointCloud2>("pc_from_scan", 2, true);
+//    // Waiting for mission flag
+//    ros::Rate loop_rate(10);
+//    if(!mission_flag_available_)
+//    	ROS_WARN("Waiting for mission_flag");
+//    while (ros::ok() && !mission_flag_available_)
+//    {
+//    	ros::spinOnce();
+//    	loop_rate.sleep();
+//    }
+//    //Navigation data (feedback)
+//    odomSub_ = node_hand_.subscribe("/pose_ekf_slam/odometry", 1, &LaserScanToPointCloud::odomCallback, this);
+//    nav_sts_available_ = false;
+//    while (ros::ok() && !nav_sts_available_)
+//    {
+//    	ros::spinOnce();
+//    	loop_rate.sleep();
+//    }
+}
+
+void LaserScanToPointCloud::laserScanCallback(const sensor_msgs::LaserScanConstPtr &scan){
+	if(!listener_.waitForTransform(
+			scan->header.frame_id,
+			"/odom",
+			scan->header.stamp + ros::Duration().fromSec(scan->ranges.size()*scan->time_increment),
+			ros::Duration(1.0))){
+		return;
+	}
+
+	sensor_msgs::PointCloud2 cloud;
+	projector_.transformLaserScanToPointCloud("/base_link",*scan,
+			cloud,listener_);
+
+	  // Do something with cloud.
+	pc_pub_.publish(cloud);
+}
+
+/// MAIN NODE FUNCTION =========================================================
+int main(int argc, char** argv){
+
+	//=======================================================================
+	// Override SIGINT handler
+	//=======================================================================
+	signal(SIGINT, stopNode);
+
+	// Init ROS node
+	ros::init(argc, argv, "laserscan_to_pointcloud");
+	ros::NodeHandle private_nh("~");
+
+	// Constructor
+	LaserScanToPointCloud laserscan_to_pc;
+
+	// Spin
+	ros::spin();
+
+	// Exit main function without errors
+	return 0;
+}
--- a/Files/mapping_planning_tutorial/control_turtlebot/srv/FindPathToGoal.srv
+++ b/Files/mapping_planning_tutorial/control_turtlebot/srv/FindPathToGoal.srv
@ -0,0 +1,6 @@
+## Service 
+float32 goal_state_x
+float32 goal_state_y
+float32 goal_state_z
+---
+geometry_msgs/Pose[] poses
--- a/Files/mapping_planning_tutorial/control_turtlebot/srv/GotoWaypoint.srv
+++ b/Files/mapping_planning_tutorial/control_turtlebot/srv/GotoWaypoint.srv
@ -0,0 +1,5 @@
+## Service 
+float32 goal_state_x
+float32 goal_state_y
+float32 goal_state_z
+---
--- a/Files/mapping_planning_tutorial/control_turtlebot/worlds/willowgarage.world
+++ b/Files/mapping_planning_tutorial/control_turtlebot/worlds/willowgarage.world