Initial commit

2022-03-14 20:24:35 +08:00 · 2022-03-14 20:24:35 +08:00 · 11fff5f14a
commit 11fff5f14a
57 changed files with 5806 additions and 0 deletions
--- a/.gitignore
+++ b/.gitignore
@ -0,0 +1,4 @@
+/CMakeLists.txt
+*.pyc
+*.o
+.vscode
--- a/README.md
+++ b/README.md
@ -0,0 +1,105 @@
+# 自主清洁机器人仿真
+适用于**Noetic**，版权属于原作者，此处引用供学生毕业设计参考。
+
+## 效果展示视频
+
+[【bilibili】](https://www.bilibili.com/video/BV17z4y1D7Wj)
+
+全覆盖路径规划
+[![result](img/result.png)](https://www.bilibili.com/video/BV17z4y1D7Wj)
+
+自主探索建图
+[![exploration](img/exploration.png)](https://www.bilibili.com/video/BV17z4y1D7Wj)
+
+## 安装依赖
+```bash
+sudo apt install ros-${ROS_DISTRO}-turtlebot3 ros-${ROS_DISTRO}-navigation ros-${ROS_DISTRO}-dwa-local-planner ros-${ROS_DISTRO}-slam-karto
+```
+
+noetic:
+
+```
+sudo apt install ros-noetic-turtlebot3 ros-noetic-navigation ros-noetic-dwa-local-planner ros-noetic-slam-karto
+```
+
+
+
+## 使用方法
+
+### 自主探索
+**Ubuntu 18.04+melodic 上测试通过。Ubuntu 16.04请参看[master](https://github.com/mywisdomfly/Clean-robot-turtlebot3/tree/master)分支**
+
+补充：Ubuntu 20.04+noetic上完全可以。
+
+explore中为自动探索的包，这里用的karto slam，可以搭配其他的slam算法
+
+自主探索建图示例启动方法 
+
+```bash
+export TURTLEBOT3_MODEL=burger 
+roslaunch clean_robot auto_slam.launch
+```
+### 自主探索
+
+清扫启动方法 
+
+```bash
+export TURTLEBOT3_MODEL=burger 
+roslaunch clean_robot clean_work.launch
+```
+### 手动导航建图
+
+```bash
+export TURTLEBOT3_MODEL=burger 
+roslaunch clean_robot nav_slam.launch
+```
+
+## 系统结构
+![structure](img/structure.png)
+
+## 文件结构
+`clean_robot`包中
+
+`CleaningPathPlanner.cpp/h`为路径规划核心程序，  
+`next_goal.cpp`为发布下一个目标点的源程序，  
+`PathPlanningNode.cpp`为对路径规划的封装。  
+
+整个包会形成两个节点`path_planning_node`与`next_goal`分表示全覆盖路径规划器与目标点发送程序。
+
+
+Launch文件中
+
+`clean_work.launch`表示启动清洁工作，  
+`auto_slam.launch `表示全自主探索建图（全自动不需要人干预）  
+`nav_slam.launch`表示导航建图（也可以使用键盘控制建图），
+`gazebo.launch`表示启动仿真环境，  
+`move_base.launch`表示机器人的导航栈配置，  
+`amcl.launch`表示粒子滤波定位系统，  
+`turtlebot3_navigation.launch`，表示机器人导航系统  
+`Config` 为路径规划所使用参数的yaml文件，  
+`param`为move_base所采用的参数文件，  
+`maps`表示地图文件，worlds为仿真环境。
+
+## 算法思想
+### 全覆盖路径规划
+将原有的栅格地图无障碍物区域按照机器人大小进行分割，分割后直接对所有分割区域进行遍历即可。（按照一定规则直接进行遍历，不能保证路径最优，可以实现相对较优，完成基本功能）。
+1. 如果机器人前方已经被覆盖过了或者是障碍,那么左转或者右转旋转180度接着走，实现S型走，如图5所示。 
+2. 如果机器人的周围都被覆盖过了，以当前机器人的位置为起点，未清洁区域作为目标点，用A*算法找出路径。重复1，2
+3. 如果没有未清洁区域，算法结束
+   
+
+![fcpp](img/fcpp.png)
+
+### exploration
+考虑到真正的清洁机器人不可能让用户操作建图，因此尝试使用自主探索建图。采用开源的`explore_lite`包实现自主探索建图，其思路为找到可以达到的地图中的位置区域的边界中点作为导航的目标点。该方法在封闭环境中十分迅速！
+
+## 参考资源
+
+[CleaningRobot](https://github.com/peterWon/CleaningRobot)
+
+[SLAM-Clean-Robot-Path-Coverage-in-ROS](https://github.com/hjr553199215/SLAM-Clean-Robot-Path-Coverage-in-ROS)
+
+[polygon_coverage_planning](https://github.com/ethz-asl/polygon_coverage_planning)
+
+[full_coverage_path_planner](https://github.com/nobleo/full_coverage_path_planner)
+
--- a/clean_robot/CMakeLists.txt
+++ b/clean_robot/CMakeLists.txt
@ -0,0 +1,228 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(clean_robot)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  costmap_2d
+  geometry_msgs
+  move_base_msgs
+  nav_msgs
+  roscpp
+  tf
+)
+find_package(OpenCV REQUIRED)
+include_directories(${OpenCV_INCLUDE_DIRS})
+find_package(catkin REQUIRED
+  COMPONENTS
+    angles
+    costmap_2d
+    geometry_msgs
+    nav_msgs
+    roscpp
+    tf
+)
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   geometry_msgs#   move_base_msgs#   nav_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES clean_robot
+ CATKIN_DEPENDS actionlib costmap_2d geometry_msgs move_base_msgs nav_msgs roscpp tf
+ DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/clean_robot.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/clean_robot_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_clean_robot.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)
+
+add_executable(path_planning_node src/PathPlanningNode.cpp src/CleaningPathPlanner.cpp )
+target_link_libraries(path_planning_node ${catkin_LIBRARIES} ${OpenCV_LIBS})
+
+add_executable(next_goal src/next_goal.cpp)
+target_link_libraries(next_goal
+	${catkin_LIBRARIES}
+)
--- a/clean_robot/config/cleaning_costmap_params.yaml
+++ b/clean_robot/config/cleaning_costmap_params.yaml
@ -0,0 +1,15 @@
+cleaning_costmap:
+   global_frame: map
+   robot_base_frame: base_footprint
+   update_frequency: 1.0
+   publish_frequency: 1.0
+   static_map: true
+   rolling_window: false
+   resolution: 0.05
+   transform_tolerance: 1.0
+   inflation_radius: 0.15
+   map_type: costmap
+
+cleaning_plan_nodehandle:   
+   size_of_cell: 3
+   grid_covered_value: 200
--- a/clean_robot/config/costmap_common_params.yaml
+++ b/clean_robot/config/costmap_common_params.yaml
@ -0,0 +1,13 @@
+obstacle_range: 2.5   #2.5
+raytrace_range: 3.0   #3.0
+#footprint: [[0.175, 0.175], [0.175, -0.175], [-0.175, -0.175], [-0.175, 0.175]]
+#footprint_inflation: 0.01
+robot_radius: 0.175  #0.175
+max_obstacle_height: 0.6
+min_obstacle_height: 0.0
+observation_sources: scan
+scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
+
+# if set to false,then obstacle layer take unknow space to be free.
+obstacle_layer:
+   track_unknown_space: true
--- a/clean_robot/include/CleaningPathPlanner.h
+++ b/clean_robot/include/CleaningPathPlanner.h
@ -0,0 +1,98 @@
+/***
+ * @brief: cleaning robot path planning
+ * @author: Wang
+ * @date: 20170702
+***/
+
+#ifndef CLEANINGPATHPLANNING_H
+#define CLEANINGPATHPLANNING_H
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <vector>
+#include <string>
+
+#include "tf/tf.h"
+#include "tf/transform_listener.h"
+#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Path.h>
+
+using namespace cv;
+using namespace std;
+
+constexpr double PI =3.14159;
+
+struct cellIndex
+{
+    int row;
+    int col;
+    double theta; //{0, 45,90,135,180,225,270,315}     角度信息   hjr 注
+};
+
+/*************************************************
+ *
+ * 读取栅格地图并根据占据信息获取其对应的空闲（可行走）空间，
+ * 按照遍历算法规划行走路线。
+ *
+ * **********************************************/
+class CleaningPathPlanning
+{
+public:
+    //CleaningPathPlanning() = delete;
+    CleaningPathPlanning(costmap_2d::Costmap2DROS *costmap2d_ros);
+
+    vector<geometry_msgs::PoseStamped> GetPathInROS(); 
+    vector<geometry_msgs::PoseStamped> GetBorderTrackingPathInROS();
+
+    void SetCoveredGrid(double wx,double wy);
+    int GetSizeOfCell(){return this->SIZE_OF_CELL;}
+
+    //for visualization
+    void PublishCoveragePath();
+    void PublishGrid();
+private:
+    //helper functions.
+    bool initializeMats();
+    bool initializeCoveredGrid();
+    void getCellMatAndFreeSpace(Mat srcImg, Mat &cellMat,vector<cellIndex> &freeSpaceVec);
+    void initializeNeuralMat(Mat cellMat, Mat neuralizedMat);
+    void writeResult(Mat resultmat,vector<cellIndex> pathVec);
+    void writeResult(cv::Mat resultmat,std::vector<cv::Point2i> pathVec);
+    void mainPlanningLoop();
+    double distance(Point2i pta,Point2i ptb);
+    bool findElement(vector<cv::Point2i> pointsVec,cv::Point2i pt, int&index);
+    void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
+    bool cellContainsPoint(cv::Point2i pt,cellIndex cell);
+
+    void GetBorderTrackingPathInCV(vector<cv::Point2i>&resultVec);
+    vector<cellIndex> GetPathInCV();
+
+
+    bool initialized_;
+    Mat srcMap_;
+    Mat cellMat_;
+    Mat neuralizedMat_;
+    vector<cellIndex> freeSpaceVec_;
+    vector<cellIndex> pathVec_;
+    vector<geometry_msgs::PoseStamped> pathVecInROS_;
+
+    double resolution_;
+    ros::Publisher plan_pub_;
+    ros::Publisher grid_pub_;
+    nav_msgs::OccupancyGrid covered_path_grid_;
+
+    //tf::TransformListener &tf_;
+    tf::Stamped<tf::Pose> initPose_;
+
+    costmap_2d::Costmap2D* costmap2d_;
+    costmap_2d::Costmap2DROS* costmap2d_ros_;
+
+    int SIZE_OF_CELL; //must be odd number.
+    int GRID_COVERED_VALUE;
+};
+
+#endif // CLEANINGPATHPLANNING_H
--- a/clean_robot/launch/amcl.launch
+++ b/clean_robot/launch/amcl.launch
@ -0,0 +1,46 @@
+<launch>
+  <!-- Arguments -->
+  <arg name="scan_topic"     default="scan"/>
+  <arg name="initial_pose_x" default="0.0"/>
+  <arg name="initial_pose_y" default="0.0"/>
+  <arg name="initial_pose_a" default="0.0"/>
+
+  <!-- AMCL -->
+  <node pkg="amcl" type="amcl" name="amcl">
+
+    <param name="min_particles"             value="500"/>
+    <param name="max_particles"             value="3000"/>
+    <param name="kld_err"                   value="0.02"/>
+    <param name="update_min_d"              value="0.20"/>
+    <param name="update_min_a"              value="0.20"/>
+    <param name="resample_interval"         value="1"/>
+    <param name="transform_tolerance"       value="1"/>
+    <param name="recovery_alpha_slow"       value="0.00"/>
+    <param name="recovery_alpha_fast"       value="0.00"/>
+    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
+    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
+    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
+    <param name="gui_publish_rate"          value="50.0"/>
+
+    <remap from="scan"                      to="$(arg scan_topic)"/>
+    <param name="laser_max_range"           value="3.5"/>
+    <param name="laser_max_beams"           value="180"/>
+    <param name="laser_z_hit"               value="0.5"/>
+    <param name="laser_z_short"             value="0.05"/>
+    <param name="laser_z_max"               value="0.05"/>
+    <param name="laser_z_rand"              value="0.5"/>
+    <param name="laser_sigma_hit"           value="0.2"/>
+    <param name="laser_lambda_short"        value="0.1"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="laser_model_type"          value="likelihood_field"/>
+
+    <param name="odom_model_type"           value="diff"/>
+    <param name="odom_alpha1"               value="0.1"/>
+    <param name="odom_alpha2"               value="0.1"/>
+    <param name="odom_alpha3"               value="0.1"/>
+    <param name="odom_alpha4"               value="0.1"/>
+    <param name="odom_frame_id"             value="odom"/>
+    <param name="base_frame_id"             value="base_footprint"/>
+
+  </node>
+</launch>
--- a/clean_robot/launch/auto_slam.launch
+++ b/clean_robot/launch/auto_slam.launch
@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- <arg name="slam_methods" default="karto" doc="slam type [gmapping, cartographer, hector, karto, frontier_exploration]"/> -->
+  <arg name="model" value="burger" doc="model type [burger, waffle, waffle_pi]"/>
+  <arg name="configuration_basename" default="turtlebot3_lds_2d.lua"/>
+  <arg name="open_rviz" default="true"/>
+  <include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
+    <arg name="model" value="$(arg model)" />
+  </include>
+  <!-- 启动仿真环境 -->
+  <include file="$(find clean_robot)/launch/gazebo.launch"/>
+
+  <!-- SLAM: Gmapping, Cartographer, Hector, Karto, Frontier_exploration, RTAB-Map -->
+  <include file="$(find turtlebot3_slam)/launch/turtlebot3_karto.launch">
+    <arg name="model" value="$(arg model)"/>
+    <arg name="configuration_basename" value="$(arg configuration_basename)"/>
+  </include>
+
+  <!-- 启动 rviz 的标签 -->
+  <group if="$(arg open_rviz)">
+    <node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find clean_robot)/rviz/turtlebot3_karto.rviz"/>
+  </group>
+  <!-- 通过导航来实现自动建图 -->
+  <include file="$(find clean_robot)/launch/move_base.launch">
+  <arg name="model" value="$(arg model)" />
+  </include>
+    <!-- 自动探索 -->
+  <node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
+  <param name="robot_base_frame" value="base_link"/>
+  <param name="costmap_topic" value="map"/>
+  <param name="costmap_updates_topic" value="map_updates"/>
+  <param name="visualize" value="true"/>
+  <param name="planner_frequency" value="0.33"/>
+  <param name="progress_timeout" value="50.0"/>
+  <param name="potential_scale" value="3.0"/>
+  <param name="orientation_scale" value="0.0"/>
+  <param name="gain_scale" value="1.0"/>
+  <param name="transform_tolerance" value="0.3"/>
+  <param name="min_frontier_size" value="0.6"/>
+</node>
+<!-- 键盘操作 -->
+  <!-- <node pkg="turtlebot3_teleop" type="turtlebot3_teleop_key" name="turtlebot3_teleop_key" launch-prefix="xterm -e" output="screen"/> -->
+
+</launch>
--- a/clean_robot/launch/clean_work.launch
+++ b/clean_robot/launch/clean_work.launch
@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="model" value="burger" doc="model type [burger, waffle, waffle_pi]"/>
+    <arg name="configuration_basename" default="turtlebot3_lds_2d.lua"/>
+    <arg name="open_rviz" default="true"/>
+    <!-- 启动仿真环境 -->
+    <include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
+        <arg name="model" value="$(arg model)" />
+    </include>
+    <include file="$(find clean_robot)/launch/gazebo.launch"/>
+    <!-- 启动导航系统，用作目标点路径规划，实现躲避小障碍物-->
+    <include file="$(find clean_robot)/launch/turtlebot3_navigation.launch">
+        <arg name="model" value="$(arg model)" />
+    </include>
+    <!-- 清扫路径规划器 -->
+    <node pkg="clean_robot" type="path_planning_node" respawn="false" name="path_planning_node" output="screen" clear_params="true">
+        <rosparam file="$(find clean_robot)/config/costmap_common_params.yaml" command="load" ns="cleaning_costmap" />
+        <rosparam file="$(find clean_robot)/config/cleaning_costmap_params.yaml" command="load" />
+    </node>
+    <!-- 根据清扫的路径向导航系统发送目标点位 -->
+    <!-- 设定距离当前目标点多进时候发布下一个目标点 -->
+    <param name="/NextGoal/tolerance_goal" value="0.25" />
+    <node pkg="clean_robot" type="next_goal" respawn="true" name="next_goal" output="screen" />
+</launch>
--- a/clean_robot/launch/gazebo.launch
+++ b/clean_robot/launch/gazebo.launch
@ -0,0 +1,19 @@
+<launch>
+  <arg name="model" default="burger" doc="model type [burger, waffle, waffle_pi]"/>
+  <arg name="x_pos" default="0.0"/>
+  <arg name="y_pos" default="0.0"/>
+  <arg name="z_pos" default="0.0"/>
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find clean_robot)/worlds/clean_room.world"/>
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
+
+  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
+</launch>
--- a/clean_robot/launch/move_base.launch
+++ b/clean_robot/launch/move_base.launch
@ -0,0 +1,21 @@
+<launch>
+  <!-- Arguments -->
+  <arg name="model" default="burger" doc="model type [burger, waffle, waffle_pi]"/>
+  <arg name="cmd_vel_topic" default="/cmd_vel" />
+  <arg name="odom_topic" default="odom" />
+  <arg name="move_forward_only" default="false"/>
+
+  <!-- move_base -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
+    <rosparam file="$(find clean_robot)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find clean_robot)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find clean_robot)/param/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find clean_robot)/param/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find clean_robot)/param/move_base_params.yaml" command="load" />
+    <rosparam file="$(find clean_robot)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
+    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
+    <remap from="odom" to="$(arg odom_topic)"/>
+    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
+  </node>
+</launch>
--- a/clean_robot/launch/nav_slam.launch
+++ b/clean_robot/launch/nav_slam.launch
@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- <arg name="slam_methods" default="karto" doc="slam type [gmapping, cartographer, hector, karto, frontier_exploration]"/> -->
+  <arg name="model" value="burger" doc="model type [burger, waffle, waffle_pi]"/>
+  <arg name="configuration_basename" default="turtlebot3_lds_2d.lua"/>
+  <arg name="open_rviz" default="true"/>
+  <include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
+    <arg name="model" value="$(arg model)" />
+  </include>
+  <!-- 启动仿真环境 -->
+  <include file="$(find clean_robot)/launch/gazebo.launch"/>
+
+  <!-- SLAM: Gmapping, Cartographer, Hector, Karto, Frontier_exploration, RTAB-Map -->
+  <include file="$(find turtlebot3_slam)/launch/turtlebot3_karto.launch">
+    <arg name="model" value="$(arg model)"/>
+    <arg name="configuration_basename" value="$(arg configuration_basename)"/>
+  </include>
+
+  <!-- 启动 rviz 的标签 -->
+  <group if="$(arg open_rviz)">
+    <node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find clean_robot)/rviz/turtlebot3_karto.rviz"/>
+  </group>
+  <!-- 通过导航来实现自动建图 -->
+  <include file="$(find clean_robot)/launch/move_base.launch">
+  <arg name="model" value="$(arg model)" />
+  </include>
+  <node pkg="turtlebot3_teleop" type="turtlebot3_teleop_key" name="turtlebot3_teleop_key" launch-prefix="xterm -e" output="screen"/>
+
+</launch>
--- a/clean_robot/launch/turtlebot3_navigation.launch
+++ b/clean_robot/launch/turtlebot3_navigation.launch
@ -0,0 +1,26 @@
+<launch>
+  <!-- Arguments -->
+  <arg name="model" default="burger" doc="model type [burger, waffle, waffle_pi]"/>
+  <arg name="map_file" default="$(find clean_robot)/maps/clean_room.yaml"/>
+  <arg name="open_rviz" default="true"/>
+  <arg name="move_forward_only" default="false"/>
+
+
+  <!-- Map server -->
+  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
+
+  <!-- AMCL -->
+  <include file="$(find clean_robot)/launch/amcl.launch"/>
+
+  <!-- move_base -->
+  <include file="$(find clean_robot)/launch/move_base.launch">
+    <arg name="model" value="$(arg model)" />
+    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
+  </include>
+
+  <!-- rviz -->
+  <group if="$(arg open_rviz)"> 
+    <node pkg="rviz" type="rviz" name="rviz" required="true"
+          args="-d $(find clean_robot)/rviz/turtlebot3_navigation.rviz"/>
+  </group>
+</launch>
--- a/clean_robot/maps/clean_room.pgm
+++ b/clean_robot/maps/clean_room.pgm
--- a/clean_robot/maps/clean_room.yaml
+++ b/clean_robot/maps/clean_room.yaml
@ -0,0 +1,7 @@
+image: clean_room.pgm
+resolution: 0.050000
+origin: [-5.159059, -3.144864, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
--- a/clean_robot/maps/clean_room_new.pgm
+++ b/clean_robot/maps/clean_room_new.pgm
--- a/clean_robot/maps/clean_room_new.yaml
+++ b/clean_robot/maps/clean_room_new.yaml
@ -0,0 +1,7 @@
+image: clean_room_new.pgm
+resolution: 0.050000
+origin: [-5.169793, -3.148376, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
--- a/clean_robot/package.xml
+++ b/clean_robot/package.xml
@ -0,0 +1,80 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>clean_robot</name>
+  <version>0.0.0</version>
+  <description>The clean_robot package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="derek@todo.todo">derek</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 multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/clean_robot</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>costmap_2d</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>move_base_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>tf</build_depend>
+  <build_export_depend>actionlib</build_export_depend>
+  <build_export_depend>costmap_2d</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>move_base_msgs</build_export_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>tf</build_export_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>costmap_2d</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>move_base_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>tf</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
--- a/clean_robot/param/base_local_planner_params.yaml
+++ b/clean_robot/param/base_local_planner_params.yaml
@ -0,0 +1,26 @@
+TrajectoryPlannerROS:
+
+# Robot Configuration Parameters
+  max_vel_x: 0.18
+  min_vel_x: 0.08
+
+  max_vel_theta:  1.0
+  min_vel_theta: -1.0
+  min_in_place_vel_theta: 1.0
+
+  acc_lim_x: 1.0
+  acc_lim_y: 0.0
+  acc_lim_theta: 0.6
+
+# Goal Tolerance Parameters
+  xy_goal_tolerance: 0.10
+  yaw_goal_tolerance: 0.05
+
+# Differential-drive robot configuration
+  holonomic_robot: false
+
+# Forward Simulation Parameters
+  sim_time: 0.8
+  vx_samples: 18
+  vtheta_samples: 20
+  sim_granularity: 0.05
--- a/clean_robot/param/costmap_common_params_burger.yaml
+++ b/clean_robot/param/costmap_common_params_burger.yaml
@ -0,0 +1,12 @@
+obstacle_range: 3.0
+raytrace_range: 3.5
+
+footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]]
+#robot_radius: 0.105
+
+inflation_radius: 1.0
+cost_scaling_factor: 4.0
+
+map_type: costmap
+observation_sources: scan
+scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}
--- a/clean_robot/param/costmap_common_params_waffle.yaml
+++ b/clean_robot/param/costmap_common_params_waffle.yaml
@ -0,0 +1,12 @@
+obstacle_range: 3.0
+raytrace_range: 3.5
+
+footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]]
+#robot_radius: 0.17
+
+inflation_radius: 1.0
+cost_scaling_factor: 3.0
+
+map_type: costmap
+observation_sources: scan
+scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}
--- a/clean_robot/param/costmap_common_params_waffle_pi.yaml
+++ b/clean_robot/param/costmap_common_params_waffle_pi.yaml
@ -0,0 +1,12 @@
+obstacle_range: 3.0
+raytrace_range: 3.5
+
+footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]]
+#robot_radius: 0.17
+
+inflation_radius: 1.0
+cost_scaling_factor: 3.0
+
+map_type: costmap
+observation_sources: scan
+scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}
--- a/clean_robot/param/dwa_local_planner_params_burger.yaml
+++ b/clean_robot/param/dwa_local_planner_params_burger.yaml
@ -0,0 +1,47 @@
+DWAPlannerROS:
+
+# Robot Configuration Parameters
+  max_vel_x: 0.22
+  min_vel_x: -0.22
+
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+
+# The velocity when robot is moving in a straight line
+  max_vel_trans:  0.22
+  min_vel_trans:  0.11
+
+  max_vel_theta: 2.75
+  min_vel_theta: 1.37
+
+  acc_lim_x: 2.5
+  acc_lim_y: 0.0
+  acc_lim_theta: 3.2 
+
+# Goal Tolerance Parametes
+  xy_goal_tolerance: 0.05
+  yaw_goal_tolerance: 0.17
+  latch_xy_goal_tolerance: false
+
+# Forward Simulation Parameters
+  sim_time: 1.5
+  vx_samples: 20
+  vy_samples: 0
+  vth_samples: 40
+  controller_frequency: 10.0
+
+# Trajectory Scoring Parameters
+  path_distance_bias: 32.0
+  goal_distance_bias: 20.0
+  occdist_scale: 0.02
+  forward_point_distance: 0.325
+  stop_time_buffer: 0.2
+  scaling_speed: 0.25
+  max_scaling_factor: 0.2
+
+# Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05
+
+# Debugging
+  publish_traj_pc : true
+  publish_cost_grid_pc: true
--- a/clean_robot/param/dwa_local_planner_params_waffle.yaml
+++ b/clean_robot/param/dwa_local_planner_params_waffle.yaml
@ -0,0 +1,47 @@
+DWAPlannerROS:
+
+# Robot Configuration Parameters
+  max_vel_x: 0.26
+  min_vel_x: -0.26
+
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+
+# The velocity when robot is moving in a straight line
+  max_vel_trans:  0.26
+  min_vel_trans:  0.13
+
+  max_vel_theta: 1.82
+  min_vel_theta: 0.9
+
+  acc_lim_x: 2.5
+  acc_lim_y: 0.0
+  acc_lim_theta: 3.2 
+
+# Goal Tolerance Parametes
+  xy_goal_tolerance: 0.05
+  yaw_goal_tolerance: 0.17
+  latch_xy_goal_tolerance: false
+
+# Forward Simulation Parameters
+  sim_time: 2.0
+  vx_samples: 20
+  vy_samples: 0
+  vth_samples: 40
+  controller_frequency: 10.0
+
+# Trajectory Scoring Parameters
+  path_distance_bias: 32.0
+  goal_distance_bias: 20.0
+  occdist_scale: 0.02
+  forward_point_distance: 0.325
+  stop_time_buffer: 0.2
+  scaling_speed: 0.25
+  max_scaling_factor: 0.2
+
+# Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05
+
+# Debugging
+  publish_traj_pc : true
+  publish_cost_grid_pc: true
--- a/clean_robot/param/dwa_local_planner_params_waffle_pi.yaml
+++ b/clean_robot/param/dwa_local_planner_params_waffle_pi.yaml
@ -0,0 +1,47 @@
+DWAPlannerROS:
+
+# Robot Configuration Parameters
+  max_vel_x: 0.26
+  min_vel_x: -0.26
+
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+
+# The velocity when robot is moving in a straight line
+  max_vel_trans:  0.26
+  min_vel_trans:  0.13
+
+  max_vel_theta: 1.82
+  min_vel_theta: 0.9
+
+  acc_lim_x: 2.5
+  acc_lim_y: 0.0
+  acc_lim_theta: 3.2 
+
+# Goal Tolerance Parametes
+  xy_goal_tolerance: 0.05
+  yaw_goal_tolerance: 0.17
+  latch_xy_goal_tolerance: false
+
+# Forward Simulation Parameters
+  sim_time: 2.0
+  vx_samples: 20
+  vy_samples: 0
+  vth_samples: 40
+  controller_frequency: 10.0
+
+# Trajectory Scoring Parameters
+  path_distance_bias: 32.0
+  goal_distance_bias: 20.0
+  occdist_scale: 0.02
+  forward_point_distance: 0.325
+  stop_time_buffer: 0.2
+  scaling_speed: 0.25
+  max_scaling_factor: 0.2
+
+# Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05
+
+# Debugging
+  publish_traj_pc : true
+  publish_cost_grid_pc: true
--- a/clean_robot/param/global_costmap_params.yaml
+++ b/clean_robot/param/global_costmap_params.yaml
@ -0,0 +1,10 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: base_footprint
+
+  update_frequency: 10.0
+  publish_frequency: 10.0
+  transform_tolerance: 0.5
+
+  static_map: true
+ 
--- a/clean_robot/param/local_costmap_params.yaml
+++ b/clean_robot/param/local_costmap_params.yaml
@ -0,0 +1,14 @@
+local_costmap:
+  global_frame: odom
+  robot_base_frame: base_footprint
+
+  update_frequency: 10.0
+  publish_frequency: 10.0
+  transform_tolerance: 0.5  
+
+  static_map: false  
+  rolling_window: true
+  width: 3
+  height: 3
+  resolution: 0.05
+  
--- a/clean_robot/param/move_base_params.yaml
+++ b/clean_robot/param/move_base_params.yaml
@ -0,0 +1,9 @@
+shutdown_costmaps: false
+controller_frequency: 10.0
+planner_patience: 5.0
+controller_patience: 15.0
+conservative_reset_dist: 3.0
+planner_frequency: 5.0
+oscillation_timeout: 10.0
+oscillation_distance: 0.2
+
--- a/clean_robot/rviz/turtlebot3_karto.rviz
+++ b/clean_robot/rviz/turtlebot3_karto.rviz
@ -0,0 +1,295 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /Path2
+        - /Map2
+        - /Map3
+      Splitter Ratio: 0.594406009
+    Tree Height: 776
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: map
+      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
+        base_scan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_back_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        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: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 0; 255; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 11799
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.0300000012
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /raspicam_node/image
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: compressed
+      Unreliable: false
+      Value: false
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Billboards
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /move_base/NavfnROS/plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 85; 0; 255
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Billboards
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /move_base/DWAPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: false
+      Name: Map
+      Topic: /move_base/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: false
+      Name: Map
+      Topic: /move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Angle: -1.57079637
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Scale: 136.475739
+      Target Frame: map
+      Value: TopDownOrtho (rviz)
+      X: -0.542091846
+      Y: 0.0558035932
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: true
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000020f000001900000001600ffffff000000010000010f0000035cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000430000035c000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1855
+  X: 65
+  Y: 24
--- a/clean_robot/rviz/turtlebot3_navigation.rviz
+++ b/clean_robot/rviz/turtlebot3_navigation.rviz
@ -0,0 +1,432 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /TF1/Tree1
+        - /Global Map1/Costmap1
+        - /Local Map1/Polygon1
+        - /Local Map1/Costmap1
+        - /Local Map1/Planner1
+        - /Path3
+      Splitter Ratio: 0.5
+    Tree Height: 844
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 20
+      Reference Frame: <Fixed Frame>
+      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
+        base_scan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_back_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        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: false
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 0; 255; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 13069
+      Min Color: 0; 0; 0
+      Min Intensity: 28
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.0300000012
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /raspicam_node/image
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: compressed
+      Unreliable: false
+      Value: false
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 0; 0; 0
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Planner Plan
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /move_base/NavfnROS/plan
+      Unreliable: false
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.699999988
+          Class: rviz/Map
+          Color Scheme: costmap
+          Draw Behind: true
+          Enabled: true
+          Name: Costmap
+          Topic: /move_base/global_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 255; 0; 0
+          Enabled: false
+          Head Diameter: 0.300000012
+          Head Length: 0.200000003
+          Length: 0.300000012
+          Line Style: Lines
+          Line Width: 0.0299999993
+          Name: Planner
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.0299999993
+          Shaft Diameter: 0.100000001
+          Shaft Length: 0.100000001
+          Topic: /move_base/NavfnROS/plan
+          Unreliable: false
+          Value: false
+      Enabled: true
+      Name: Global Map
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 1
+          Class: rviz/Polygon
+          Color: 0; 0; 0
+          Enabled: true
+          Name: Polygon
+          Topic: /move_base/local_costmap/footprint
+          Unreliable: false
+          Value: true
+        - Alpha: 0.699999988
+          Class: rviz/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Costmap
+          Topic: /move_base/local_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 255; 255; 0
+          Enabled: true
+          Head Diameter: 0.300000012
+          Head Length: 0.200000003
+          Length: 0.300000012
+          Line Style: Lines
+          Line Width: 0.0299999993
+          Name: Planner
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.0299999993
+          Shaft Diameter: 0.100000001
+          Shaft Length: 0.100000001
+          Topic: /move_base/DWAPlannerROS/local_plan
+          Unreliable: false
+          Value: true
+      Enabled: false
+      Name: Local Map
+    - Alpha: 1
+      Arrow Length: 0.0500000007
+      Axes Length: 0.300000012
+      Axes Radius: 0.00999999978
+      Class: rviz/PoseArray
+      Color: 0; 192; 0
+      Enabled: false
+      Head Length: 0.0700000003
+      Head Radius: 0.0299999993
+      Name: Amcl Particles
+      Shaft Length: 0.230000004
+      Shaft Radius: 0.00999999978
+      Shape: Arrow (Flat)
+      Topic: /particlecloud
+      Unreliable: false
+      Value: false
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.100000001
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.0500000007
+      Head Radius: 0.100000001
+      Name: Goal
+      Shaft Length: 0.0500000007
+      Shaft Radius: 0.0500000007
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Billboards
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /path_planning_node/cleaning_plan_nodehandle/cleaning_path
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 170; 0; 0
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Billboards
+      Line Width: 0.349999994
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /clean_robot/passed_path
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 255; 127
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /move_base/DWAPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    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
+    - Class: rviz/Measure
+  Value: true
+  Views:
+    Current:
+      Angle: -1.57079637
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Scale: 156.50177
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz)
+      X: -2.20532393
+      Y: -1.97824216
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: true
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000003dafc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003da000000d600fffffffb0000000a0049006d0061006700650000000317000000cc0000001600fffffffb0000000a0049006d0061006700650000000330000000ce0000000000000000000000010000010f000003a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003a0000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf000003da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1855
+  X: 65
+  Y: 24
--- a/clean_robot/src/CleaningPathPlanner.cpp
+++ b/clean_robot/src/CleaningPathPlanner.cpp
@ -0,0 +1,724 @@
+#include "CleaningPathPlanner.h"
+#include <costmap_2d/cost_values.h>
+
+CleaningPathPlanning::CleaningPathPlanning(costmap_2d::Costmap2DROS *costmap2d_ros)
+{
+    //temp solution.
+    costmap2d_ros_ = costmap2d_ros;
+    //costmap2d_ros_->updateMap();
+    costmap2d_ = costmap2d_ros->getCostmap();
+
+    ros::NodeHandle private_nh("~/cleaning_plan_nodehandle");
+    plan_pub_ = private_nh.advertise<nav_msgs::Path>("cleaning_path", 1);
+    grid_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("covered_grid", 1);
+
+    string sizeOfCellString, coveredValueStr;
+
+    SIZE_OF_CELL = 3;
+    if (private_nh.searchParam("size_of_cell", sizeOfCellString)) //搜索参数,根据名称"size of cell"搜索参数，将对应名称下的参数值赋给sizeOfCellString.
+        private_nh.param("size_of_cell", SIZE_OF_CELL, 3);   //设置机器人占据n*n的栅格，决定规划的稀疏   
+
+    GRID_COVERED_VALUE = 0;
+    if (private_nh.searchParam("grid_covered_value", coveredValueStr))
+        private_nh.param("grid_covered_value", GRID_COVERED_VALUE, 0);
+
+    int sizex = costmap2d_->getSizeInCellsX(); //获取地图尺寸
+    int sizey = costmap2d_->getSizeInCellsY();
+    cout << "The size of map is " << sizex << "  " << sizey << endl;
+    resolution_ = costmap2d_->getResolution(); //分辨率
+
+    srcMap_ = Mat(sizey, sizex, CV_8U);
+    for (int r = 0; r < sizey; r++)
+    {
+        for (int c = 0; c < sizex; c++)
+        {
+            srcMap_.at<uchar>(r, c) = costmap2d_->getCost(c, sizey - r - 1); //??sizey-r-1 caution: costmap's origin is at left bottom ,while opencv's pic's origin is at left-top.
+            //getCost（）:获取代价值
+        }
+    }
+
+    initializeMats();
+    initializeCoveredGrid();
+
+    //imshow("debugMapImage",srcMap_);
+    //imshow("debugCellMatImage",cellMat_);
+    //waitKey(0);
+    //imwrite("debug_srcmap.jpg",srcMap_);
+
+    if (!srcMap_.empty())
+        initialized_ = true; //这句话説明srcMap_里面得有东西才能说明初始化成功。
+    else
+        initialized_ = false;
+}
+//规划路径
+vector<geometry_msgs::PoseStamped> CleaningPathPlanning::GetPathInROS()
+{
+    //    vector<geometry_msgs::PoseStamped> resultVec;
+    if (!pathVecInROS_.empty())
+        pathVecInROS_.clear(); //清空操作
+    geometry_msgs::PoseStamped posestamped;
+    geometry_msgs::Pose pose;
+    vector<cellIndex> cellvec;
+    cellvec = GetPathInCV();
+    /*trasnsform*/
+    vector<cellIndex>::iterator iter; //cellIndex里面存放的是行，列以及角度信息。
+    int sizey = cellMat_.rows;
+
+    for (iter = cellvec.begin(); iter != cellvec.end(); iter++)
+    {
+        costmap2d_->mapToWorld((*iter).col * SIZE_OF_CELL + SIZE_OF_CELL / 2, (sizey - (*iter).row - 1) * SIZE_OF_CELL + SIZE_OF_CELL / 2, pose.position.x, pose.position.y);
+        pose.orientation.w = cos((*iter).theta * PI / 180 / 2); //(sizey-(*iter).row-1)
+        pose.orientation.x = 0;
+        pose.orientation.y = 0;
+        pose.orientation.z = sin((*iter).theta * PI / 180 / 2);
+        posestamped.header.stamp = ros::Time::now();
+        posestamped.header.frame_id = "map";
+        posestamped.pose = pose;
+
+        pathVecInROS_.push_back(posestamped);
+    }
+    publishPlan(pathVecInROS_);
+    cout << "The path size is " << pathVecInROS_.size() << endl;
+    return pathVecInROS_;
+}
+
+vector<geometry_msgs::PoseStamped> CleaningPathPlanning::GetBorderTrackingPathInROS()
+{
+    //vector<geometry_msgs::PoseStamped> resultPathInROS;
+    if (!pathVecInROS_.empty())
+        pathVecInROS_.clear();
+    geometry_msgs::PoseStamped posestamped;
+    geometry_msgs::Pose pose;
+    vector<cv::Point2i> resultCV;
+    GetBorderTrackingPathInCV(resultCV);
+    vector<bool> visitedVec;
+
+    cv::Point2i lastPoint = resultCV[0], curPoint;
+    double theta = 0.0;
+    visitedVec.assign(resultCV.size(), false);
+    int i, j, k, index;
+    for (i = 0; i < resultCV.size(); i++)
+    {
+        if (!visitedVec[i])
+        {
+            for (k = -SIZE_OF_CELL / 2; k < SIZE_OF_CELL / 2; k++)
+            {
+                for (j = -SIZE_OF_CELL / 2; j < SIZE_OF_CELL / 2; j++)
+                {
+                    if (findElement(resultCV, Point2i(resultCV[i].x + j, resultCV[i].y + k), index))
+                    {
+                        visitedVec[index] = true;
+
+                        curPoint = resultCV[index];
+                        if (curPoint.x == lastPoint.x)
+                        {
+                            if (curPoint.y > lastPoint.y)
+                                theta = PI / 2;
+                            else
+                                theta = -PI / 2;
+                        }
+                        else
+                            theta = atan((curPoint.y - lastPoint.x) / (curPoint.x - lastPoint.x));
+
+                        //srcMap_'s shape is same with costmap2d_,ie.. they have same resolution and size.
+                        costmap2d_->mapToWorld(resultCV[index].x, srcMap_.rows - resultCV[index].y - 1, pose.position.x, pose.position.y);
+                        pose.orientation.w = cos(theta * PI / 180 / 2);
+                        pose.orientation.x = 0;
+                        pose.orientation.y = 0;
+                        pose.orientation.z = sin(theta * PI / 180 / 2);
+                        posestamped.header.stamp = ros::Time::now();
+                        posestamped.header.frame_id = "map";
+                        posestamped.pose = pose;
+
+                        pathVecInROS_.push_back(posestamped);
+                    }
+                }
+            }
+        }
+    }
+    publishPlan(pathVecInROS_);
+    return pathVecInROS_;
+}
+
+//First make border path planning in original resolution using opencv tools.Then transform the result
+//in ROS format and take the robot's shape into account.
+void CleaningPathPlanning::GetBorderTrackingPathInCV(vector<cv::Point2i> &resultVec)
+{
+    std::vector<cv::Point2i> borderPointsIndexVec; //todo:make point2i corrresponding to cellindex.
+    resultVec.clear();
+    if (srcMap_.empty())
+        return; //srcMap 是个啥？
+
+    cv::Point2i temppoint;
+    int r, c, i, j;
+    for (r = 0; r < srcMap_.rows; r++)
+    {
+        for (c = 0; c < srcMap_.cols; c++)
+        {
+            //ROS_INFO("The value at row %d, col %d is %d",r,c,srcMap_.at<uchar>(r,c));
+            if (srcMap_.at<uchar>(r, c) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+            {
+                ROS_INFO("entered once!");
+                if (srcMap_.at<uchar>(r, c - 1) != srcMap_.at<uchar>(r, c + 1) && (srcMap_.at<uchar>(r, c - 1) == costmap_2d::FREE_SPACE || srcMap_.at<uchar>(r, c + 1) == costmap_2d::FREE_SPACE))
+                {
+                    temppoint.x = c;
+                    temppoint.y = r;
+                    borderPointsIndexVec.push_back(temppoint);
+                }
+                else if (srcMap_.at<uchar>(r - 1, c) != srcMap_.at<uchar>(r + 1, c) && (srcMap_.at<uchar>(r - 1, c) == costmap_2d::FREE_SPACE || srcMap_.at<uchar>(r + 1, c) == costmap_2d::FREE_SPACE))
+                {
+                    temppoint.x = c;
+                    temppoint.y = r;
+                    borderPointsIndexVec.push_back(temppoint);
+                }
+            }
+        }
+    }
+
+    std::cout << "The border path length is:" << borderPointsIndexVec.size() << std::endl;
+    std::vector<bool> visitedVec;
+    if (borderPointsIndexVec.empty())
+        return;
+
+    int minDistIndex = -1, loopCounter = 0;
+    double dist, minDist;
+    visitedVec.assign(borderPointsIndexVec.size(), false); //vector assign是vector容器下的赋值函数。
+
+    Point2i curPoint = borderPointsIndexVec[0];
+    visitedVec[0] = true;
+
+    while (loopCounter < borderPointsIndexVec.size())
+    {
+        minDist = 1e6;
+        resultVec.push_back(curPoint);
+        for (j = 1; j < borderPointsIndexVec.size(); j++)
+        {
+            if (!visitedVec[j])
+            {
+                dist = distance(curPoint, borderPointsIndexVec[j]);
+                if (dist < minDist)
+                {
+                    minDist = dist;
+                    minDistIndex = j;
+                }
+            }
+        }
+        curPoint = borderPointsIndexVec[minDistIndex];
+        visitedVec[minDistIndex] = true;
+        loopCounter++;
+    }
+
+    //Mat resultmat=Mat(srcMap_.rows,srcMap_.cols, CV_8UC3);;
+    //writeResult(resultmat,resultVec);
+}
+
+void CleaningPathPlanning::SetCoveredGrid(double wx, double wy)
+{
+    unsigned int mx, my, index;
+    bool isok = costmap2d_->worldToMap(wx, wy, mx, my);
+    if (!isok)
+    {
+        return;
+    }
+
+    for (int dx = -SIZE_OF_CELL / 2; dx < SIZE_OF_CELL / 2 + 1; dx++)
+    {
+        for (int dy = -SIZE_OF_CELL / 2; dy < SIZE_OF_CELL / 2 + 1; dy++)
+        {
+            index = costmap2d_->getIndex(mx + dx, my + dy);
+            covered_path_grid_.data[index] = GRID_COVERED_VALUE;
+        }
+    }
+}
+
+void CleaningPathPlanning::PublishGrid()
+{
+    if (!initialized_)
+        initializeCoveredGrid();
+    grid_pub_.publish(covered_path_grid_);
+}
+
+vector<cellIndex> CleaningPathPlanning::GetPathInCV()
+{
+    mainPlanningLoop();
+    return this->pathVec_;
+}
+
+void CleaningPathPlanning::PublishCoveragePath()
+{
+    publishPlan(this->pathVecInROS_);
+}
+
+void CleaningPathPlanning::publishPlan(const std::vector<geometry_msgs::PoseStamped> &path)
+{
+    if (!initialized_)
+    {
+        ROS_ERROR(
+            "This planner has not been initialized yet, but it is being used, please call initialize() before use");
+        return;
+    }
+
+    //create a message for the plan
+    nav_msgs::Path gui_path;
+    gui_path.poses.resize(path.size());
+
+    gui_path.header.frame_id = "map";
+    gui_path.header.stamp = ros::Time::now();
+
+    // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+    for (unsigned int i = 0; i < path.size(); i++)
+    {
+        gui_path.poses[i] = path[i];
+    }
+
+    plan_pub_.publish(gui_path);
+}
+
+bool CleaningPathPlanning::cellContainsPoint(Point2i pt, cellIndex cell)
+{
+    return pt.y > cell.row * SIZE_OF_CELL && pt.y < (cell.row + 1) * SIZE_OF_CELL && pt.x > cell.col * SIZE_OF_CELL && pt.x < (cell.col + 1) * SIZE_OF_CELL;
+}
+
+bool CleaningPathPlanning::initializeMats()
+{
+    //initialize the member variables.
+    if (srcMap_.empty())
+        return false;
+    getCellMatAndFreeSpace(srcMap_, cellMat_, freeSpaceVec_);
+
+    neuralizedMat_ = Mat(cellMat_.rows, cellMat_.cols, CV_32F);
+    //Astarmap = Mat(cellMat_.rows, cellMat_.cols, CV_32F);
+    initializeNeuralMat(cellMat_, neuralizedMat_);
+    //Astarmap = neuralizedMat_;
+    return true;
+}
+
+void CleaningPathPlanning::getCellMatAndFreeSpace(Mat srcImg, Mat &cellMat, vector<cellIndex> &freeSpaceVec)
+{
+    cellMat = Mat(srcImg.rows / SIZE_OF_CELL, srcImg.cols / SIZE_OF_CELL, srcImg.type()); //cellMat是以之前规定的cell为最小单位重新划分的矩阵
+
+    freeSpaceVec.clear();
+    bool isFree = true;
+    int r = 0, c = 0, i = 0, j = 0;
+    for (r = 0; r < cellMat.rows; r++)
+    {
+        for (c = 0; c < cellMat.cols; c++)
+        {
+            isFree = true;
+            for (i = 0; i < SIZE_OF_CELL; i++)
+            {
+                for (j = 0; j < SIZE_OF_CELL; j++)
+                {
+                    if (srcImg.at<uchar>(r * SIZE_OF_CELL + i, c * SIZE_OF_CELL + j) != costmap_2d::FREE_SPACE)
+                    {
+                        isFree = false;
+                        i = SIZE_OF_CELL;
+                        break;
+                    }
+                }
+            }
+            if (isFree)
+            {
+                cellIndex ci;
+                ci.row = r;
+                ci.col = c;
+                ci.theta = 0;
+                freeSpaceVec.push_back(ci);
+                cellMat.at<uchar>(r, c) = costmap_2d::FREE_SPACE; //0
+            }
+            else
+            {
+                cellMat.at<uchar>(r, c) = costmap_2d::LETHAL_OBSTACLE;
+            } //254
+        }
+    }
+    cout << "freespace size:" << freeSpaceVec.size() << endl;
+    //imwrite("cellMat.jpg",cellMat);
+    return;
+}
+
+void CleaningPathPlanning::initializeNeuralMat(Mat cellMat, Mat neuralizedMat)
+{
+    int i = 0, j = 0;
+    for (i = 0; i < neuralizedMat.rows; i++)
+    {
+        for (j = 0; j < neuralizedMat.cols; j++)
+        {
+            if (cellMat.at<uchar>(i, j) == costmap_2d::LETHAL_OBSTACLE)
+                neuralizedMat.at<float>(i, j) = -100000.0; 
+            else
+                neuralizedMat.at<float>(i, j) = 50.0 / j; //这里的1.0/j的用意是什么？这里有没有考虑到j=0时刻的问题呢？   hjr注
+        }
+    }
+    return;
+}
+
+void CleaningPathPlanning::writeResult(Mat resultmat, vector<cellIndex> pathVec)
+{
+    int i = 0, j = 0;
+    Point initpoint = Point(pathVec[0].col * SIZE_OF_CELL + SIZE_OF_CELL / 2, pathVec[0].row * SIZE_OF_CELL + SIZE_OF_CELL / 2); //这里是取中心位置的意思吗？
+    for (i = 1; i < pathVec.size(); i++)
+    {
+        Point cupoint = Point(pathVec[i].col * SIZE_OF_CELL + SIZE_OF_CELL / 2, pathVec[i].row * SIZE_OF_CELL + SIZE_OF_CELL / 2);
+        if (sqrt((initpoint.x - cupoint.x) * (initpoint.x - cupoint.x) + (initpoint.y - cupoint.y) * (initpoint.y - cupoint.y)) > 2)
+        {
+            line(resultmat, initpoint, cupoint, Scalar(0, 255, 0), 1, 8); //line就是划线函数
+        }
+        else
+            line(resultmat, initpoint, cupoint, Scalar(0, 0, 255), 1);
+        initpoint = cupoint;
+        cout << "The point of step " << i << " is: " << pathVec[i].row << " " << pathVec[i].col << endl;
+        /*resultMat.at<Vec3b>(pathVec[i].row*SIZE_OF_CELL,pathVec[i].col*SIZE_OF_CELL)[0] = 0;
+        resultMat.at<Vec3b>(pathVec[i].row*SIZE_OF_CELL,pathVec[i].col*SIZE_OF_CELL)[1] = 0;
+        resultMat.at<Vec3b>(pathVec[i].row*SIZE_OF_CELL,pathVec[i].col*SIZE_OF_CELL)[2] = 255;*/
+    }
+    //    imshow("resultMat",resultmat);
+    //    waitKey(0);
+    imwrite("reaultMat.jpg", resultmat);
+}
+
+void CleaningPathPlanning::writeResult(Mat resultmat, vector<cv::Point2i> pathVec)
+{
+    int i = 0, j = 0;
+    Point initpoint = pathVec[0];
+    for (i = 1; i < pathVec.size(); i++)
+    {
+        Point cupoint = pathVec[i];
+        line(resultmat, initpoint, cupoint, Scalar(0, 0, 255), 1);
+        initpoint = cupoint;
+        //std::cout<<"X: "<<cupoint.x<<","<<"Y:"<<cupoint<<std::endl;
+    }
+    //    imshow("resultMat",resultmat);
+    //    waitKey(0);
+}
+
+void CleaningPathPlanning::mainPlanningLoop()
+{
+    cellIndex initPoint, nextPoint, currentPoint;
+    //    initPoint.row = cellMat_.rows/2; //initPoint to be made interface.
+    //    initPoint.col = cellMat_.cols/2;
+
+    initPoint.theta = 90;
+    bool isok = costmap2d_ros_->getRobotPose(initPose_);
+    if (!isok)
+    {
+        ROS_INFO("Failed to get robot location! Please check where goes wrong!");
+        return;
+    }
+    //initPoint.row = initPose_.getOrigin().y()
+    unsigned int mx, my;
+    double wx = initPose_.pose.position.x; //获取原点的x坐标
+    double wy = initPose_.pose.position.y;
+    //geometry_msgs::PoseStamped current_position;
+    //tf::poseStampedTFToMsg(global_pose, current_position);
+
+    bool getmapcoor = costmap2d_->worldToMap(wx, wy, mx, my);
+    if (!getmapcoor)
+    {
+        ROS_INFO("Failed to get robot location in map! Please check where goes wrong!");
+        return;
+    }
+    initPoint.row = cellMat_.rows - my / SIZE_OF_CELL - 1; //再研究一下这个行列之间的转换问题。
+    initPoint.col = mx / SIZE_OF_CELL;                     //这里貌似不光有行列转换
+
+    currentPoint = initPoint; //将初始化的点赋予给当前点。
+    pathVec_.clear();
+    pathVec_.push_back(initPoint);
+
+    float initTheta = initPoint.theta; //initial orientation
+    const float c_0 = 50;              //这貌似是后面算法中要用到的参数。0.001
+    float e = 0.0, v = 0.0, v_1 = 0.0, deltaTheta = 0.0, lasttheta = initTheta, PI = 3.14159;
+    int fx;
+    vector<float> thetaVec = {0, 45, 90, 135, 180, 225, 270, 315};
+    //vector<float> thetaVec = {0,315,270,225,180,135,90,45};
+    //the main planning loop
+    //while(freeSpaceVec_.size()>0)
+
+    for (int loop = 0; loop < 9000; loop++)
+    {
+        //erase current point from free space first.
+        vector<cellIndex>::iterator it;
+
+        /*
+	for(it=freeSpaceVec_.begin();it!=freeSpaceVec_.end();)
+        {
+            if((*it).row==nextPoint.row && (*it).col==nextPoint.col)
+            {it = freeSpaceVec_.erase(it);continue;}
+            it++;
+        }
+
+        */
+
+        //compute neiborhood's activities
+        //hjr注：目前我认为这里进行的是有关方向上的抉择。
+        int maxIndex = 0; //目前尚不清楚这两个参数最后是干啥的。
+        float max_v = -300;
+        neuralizedMat_.at<float>(currentPoint.row, currentPoint.col) = -250.0;
+        lasttheta = currentPoint.theta;
+        for (int id = 0; id < 8; id++)
+        {
+            deltaTheta = max(thetaVec[id], lasttheta) - min(thetaVec[id], lasttheta);
+            if (deltaTheta > 180)
+                deltaTheta = 360 - deltaTheta;
+            e = 1 - abs(deltaTheta) / 180; //角度参数？
+            switch (id)
+            {
+            case 0:
+                if (currentPoint.col == neuralizedMat_.cols - 1)
+                {
+                    v = -100000;
+                    break;
+                } //处于边界？
+                v = neuralizedMat_.at<float>(currentPoint.row, currentPoint.col + 1) + c_0 * e;
+
+                break;
+            case 1:
+                if (currentPoint.col == neuralizedMat_.cols - 1 || currentPoint.row == 0)
+                {
+                    v = -100000;
+                    break;
+                }
+                v = neuralizedMat_.at<float>(currentPoint.row - 1, currentPoint.col + 1) + c_0 * e - 200;
+
+                break;
+            case 2:
+                if (currentPoint.row == 0)
+                {
+                    v = -100000;
+                    break;
+                }
+                v = neuralizedMat_.at<float>(currentPoint.row - 1, currentPoint.col) + c_0 * e;
+
+                break;
+            case 3:
+                if (currentPoint.col == 0 || currentPoint.row == 0)
+                {
+                    v = -100000;
+                    break;
+                }
+                v = neuralizedMat_.at<float>(currentPoint.row - 1, currentPoint.col - 1) + c_0 * e - 200;
+
+                break;
+            case 4:
+                if (currentPoint.col == 0)
+                {
+                    v = -100000;
+                    break;
+                }
+                v = neuralizedMat_.at<float>(currentPoint.row, currentPoint.col - 1) + c_0 * e;
+
+                break;
+            case 5:
+                if (currentPoint.col == 0 || currentPoint.row == neuralizedMat_.rows - 1)
+                {
+                    v = -100000;
+                    break;
+                }
+                v = neuralizedMat_.at<float>(currentPoint.row + 1, currentPoint.col - 1) + c_0 * e - 200;
+
+                break;
+            case 6:
+                if (currentPoint.row == neuralizedMat_.rows - 1)
+                {
+                    v = -100000;
+                    break;
+                }
+                v = neuralizedMat_.at<float>(currentPoint.row + 1, currentPoint.col) + c_0 * e;
+
+                break;
+            case 7:
+                if (currentPoint.col == neuralizedMat_.cols - 1 || currentPoint.row == neuralizedMat_.rows - 1)
+                {
+                    v = -100000;
+                    break;
+                }
+                v = neuralizedMat_.at<float>(currentPoint.row + 1, currentPoint.col + 1) + c_0 * e - 200;
+
+                break;
+            default:
+                break;
+            }
+            if (v > max_v)
+            {
+                max_v = v;
+                maxIndex = id;
+            }
+
+            if (v == max_v && id > maxIndex)
+            {
+                max_v = v;
+                maxIndex = id;
+            }
+        }
+
+        if (max_v <= 0) //接下来应该是在处理距离上的关系。
+        {
+            float dist = 0.0, min_dist = 100000000;
+            //vector<cellIndex>::iterator min_iter;
+            int ii = 0, min_index = -1;
+            for (it = freeSpaceVec_.begin(); it != freeSpaceVec_.end(); it++)
+            {
+                if (neuralizedMat_.at<float>((*it).row, (*it).col) > 0)
+                {
+                    if (Boundingjudge((*it).row, (*it).col)) //周围是否存在-250的点
+                    {
+                        dist = sqrt((currentPoint.row - (*it).row) * (currentPoint.row - (*it).row) + (currentPoint.col - (*it).col) * (currentPoint.col - (*it).col));
+                        if (dist < min_dist)
+                        {
+                            min_dist = dist;
+                            min_index = ii;
+                        }
+                    }
+                }
+                ii++;
+            }
+            //if(min_dist==0 || min_index == -1)
+            //{break;}
+            //else
+            if (min_index != -1 && min_dist != 100000000)
+            {
+                cout << "next point index: " << min_index << endl;
+                cout << "distance: " << min_dist << endl;
+                nextPoint = freeSpaceVec_[min_index];
+                currentPoint = nextPoint;
+                pathVec_.push_back(nextPoint);
+
+                continue;
+            }
+            else //产生了自锁现象
+            {
+                ROS_INFO("The program has been dead because of the self-locking");
+                ROS_INFO("The program has gone through %ld steps", pathVec_.size());
+                break;
+            }
+        }
+
+        //hjr自己编写部分------------------------------------------------------------------------------
+
+        //next point.
+        switch (maxIndex)
+        {
+        case 0:
+            nextPoint.row = currentPoint.row;
+            nextPoint.col = currentPoint.col + 1;
+            break;
+        case 1:
+            nextPoint.row = currentPoint.row - 1;
+            nextPoint.col = currentPoint.col + 1;
+            break;
+        case 2:
+            nextPoint.row = currentPoint.row - 1;
+            nextPoint.col = currentPoint.col;
+            break;
+        case 3:
+            nextPoint.row = currentPoint.row - 1;
+            nextPoint.col = currentPoint.col - 1;
+            break;
+        case 4:
+            nextPoint.row = currentPoint.row;
+            nextPoint.col = currentPoint.col - 1;
+            break;
+        case 5:
+            nextPoint.row = currentPoint.row + 1;
+            nextPoint.col = currentPoint.col - 1;
+            break;
+        case 6:
+            nextPoint.row = currentPoint.row + 1;
+            nextPoint.col = currentPoint.col;
+            break;
+        case 7:
+            nextPoint.row = currentPoint.row + 1;
+            nextPoint.col = currentPoint.col + 1;
+            break;
+        default:
+            break;
+        }
+        nextPoint.theta = thetaVec[maxIndex];
+        currentPoint = nextPoint;
+        pathVec_.push_back(nextPoint);
+
+        //for(it=freeSpaceVec_.begin();it!=freeSpaceVec_.end();)
+        // {
+        //     if((*it).row==nextPoint.row && (*it).col==nextPoint.col)
+        //     {it = freeSpaceVec_.erase(it);continue;}
+        //      it++;
+        //  }
+    }
+
+    // Mat resultMat = Mat(srcMap_.rows, srcMap_.cols, CV_8UC3);
+    // writeResult(resultMat, pathVec_);
+}
+
+double CleaningPathPlanning::distance(Point2i pta, Point2i ptb)
+{
+    return sqrt((pta.x - ptb.x) * (pta.x - ptb.x) + (pta.y - ptb.y) * (pta.y - ptb.y));
+}
+
+bool CleaningPathPlanning::findElement(vector<Point2i> pointsVec, Point2i pt, int &index)
+{
+    for (int i = 0; i < pointsVec.size(); i++)
+    {
+        if (pointsVec[i].x == pt.x && pointsVec[i].y == pt.y)
+        {
+            index = i;
+            return true;
+        }
+    }
+    index = -1;
+    return false;
+}
+
+bool CleaningPathPlanning::initializeCoveredGrid() //在这里我对CoverGrid的理解为覆盖栅格。
+{
+    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap2d_->getMutex()));
+    double resolution = costmap2d_->getResolution(); //分辨率
+
+    covered_path_grid_.header.frame_id = "map"; //covered_path_grid_是costmap库中的占据栅格地图消息。
+    covered_path_grid_.header.stamp = ros::Time::now();
+    covered_path_grid_.info.resolution = resolution;
+
+    covered_path_grid_.info.width = costmap2d_->getSizeInCellsX();
+    covered_path_grid_.info.height = costmap2d_->getSizeInCellsY();
+
+    double wx, wy;
+    costmap2d_->mapToWorld(0, 0, wx, wy); //从地图坐标系转换至世界坐标系。
+    covered_path_grid_.info.origin.position.x = wx - resolution / 2;
+    covered_path_grid_.info.origin.position.y = wy - resolution / 2;
+    covered_path_grid_.info.origin.position.z = 0.0;
+    covered_path_grid_.info.origin.orientation.w = 1.0;
+
+    covered_path_grid_.data.resize(covered_path_grid_.info.width * covered_path_grid_.info.height); //这里可以理解为一共有多少个栅格，所以长×宽
+
+    unsigned char *data = costmap2d_->getCharMap(); //返回一个指针，指向一个底层无符号字符数组，数组中存储着代价值，这个数组貌似还可以用来作为代价地图。
+    for (unsigned int i = 0; i < covered_path_grid_.data.size(); i++)
+    {
+        /*if(data[i]==costmap_2d::FREE_SPACE)
+            covered_path_grid_.data[i] = costmap_2d::FREE_SPACE;
+        else
+            covered_path_grid_.data[i] = 0;*/
+        covered_path_grid_.data[i] = data[i]; //这里我理解为将代价值赋予到栅格地图的每个对应栅格当中去。
+    }
+    return true;
+}
+
+//hjr---------------------------------------------------------
+bool CleaningPathPlanning::Boundingjudge(int a, int b)
+{
+    int num = 0;
+    for (int i = -1; i <= 1; i++)
+    {
+        for (int m = -1; m <= 1; m++)
+        {
+            if (i == 0 && m == 0)
+            {
+                continue;
+            }
+            if (neuralizedMat_.at<float>((a + i), (b + m)) == -250.0)
+                num++;
+        }
+    }
+    if (num != 0)
+        return true;
+    else
+        return false;
+}
--- a/clean_robot/src/CleaningPathPlanner.h
+++ b/clean_robot/src/CleaningPathPlanner.h
@ -0,0 +1,100 @@
+/***
+ * @brief: cleaning robot path planning
+ * @author: Wang
+ * @date: 20170702
+***/
+
+#ifndef CLEANINGPATHPLANNING_H
+#define CLEANINGPATHPLANNING_H
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <vector>
+#include <string>
+
+#include "tf/tf.h"
+#include "tf/transform_listener.h"
+#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Path.h>
+
+using namespace cv;
+using namespace std;
+
+constexpr double PI = 3.14159;
+
+struct cellIndex
+{
+    int row;
+    int col;
+    double theta; //{0, 45,90,135,180,225,270,315}     角度信息   hjr 注
+};
+
+
+/*************************************************
+ *
+ * 读取栅格地图并根据占据信息获取其对应的空闲（可行走）空间，
+ * 按照遍历算法规划行走路线。
+ *
+ * **********************************************/
+class CleaningPathPlanning
+{
+public:
+    //CleaningPathPlanning() = delete;
+    CleaningPathPlanning(costmap_2d::Costmap2DROS *costmap2d_ros);
+
+    vector<geometry_msgs::PoseStamped> GetPathInROS();
+    vector<geometry_msgs::PoseStamped> GetBorderTrackingPathInROS();
+
+    void SetCoveredGrid(double wx, double wy);
+    int GetSizeOfCell() { return this->SIZE_OF_CELL; }
+    bool Boundingjudge(int a, int b);
+    //for visualization
+    void PublishCoveragePath();
+    void PublishGrid();
+
+
+private:
+    //helper functions.
+    bool initializeMats();
+    bool initializeCoveredGrid();
+    void getCellMatAndFreeSpace(Mat srcImg, Mat &cellMat, vector<cellIndex> &freeSpaceVec);
+    void initializeNeuralMat(Mat cellMat, Mat neuralizedMat);
+    void writeResult(Mat resultmat, vector<cellIndex> pathVec);
+    void writeResult(cv::Mat resultmat, std::vector<cv::Point2i> pathVec);
+    void mainPlanningLoop();
+    double distance(Point2i pta, Point2i ptb);
+    bool findElement(vector<cv::Point2i> pointsVec, cv::Point2i pt, int &index);
+    void publishPlan(const std::vector<geometry_msgs::PoseStamped> &path);
+    bool cellContainsPoint(cv::Point2i pt, cellIndex cell);
+
+    void GetBorderTrackingPathInCV(vector<cv::Point2i> &resultVec);
+    vector<cellIndex> GetPathInCV();
+
+    bool initialized_;
+    Mat srcMap_;
+    Mat cellMat_;
+    Mat neuralizedMat_;
+    vector<cellIndex> freeSpaceVec_;
+    vector<cellIndex> pathVec_;
+    vector<geometry_msgs::PoseStamped> pathVecInROS_;
+
+    double resolution_;
+    ros::Publisher plan_pub_;
+    ros::Publisher grid_pub_;
+    nav_msgs::OccupancyGrid covered_path_grid_;
+
+    //tf::TransformListener &tf_;
+    geometry_msgs::PoseStamped initPose_;
+    
+    costmap_2d::Costmap2D *costmap2d_;
+    costmap_2d::Costmap2DROS *costmap2d_ros_;
+
+    int SIZE_OF_CELL; //must be odd number.
+    int GRID_COVERED_VALUE;
+};
+
+#endif // CLEANINGPATHPLANNING_H
--- a/clean_robot/src/PathPlanningNode.cpp
+++ b/clean_robot/src/PathPlanningNode.cpp
@ -0,0 +1,37 @@
+#include "CleaningPathPlanner.h"
+#include <boost/shared_ptr.hpp>
+#include <costmap_2d/costmap_2d_ros.h>
+
+namespace cm = costmap_2d;
+namespace rm = geometry_msgs;
+
+using std::vector;
+using rm::PoseStamped;
+using std::string;
+using cm::Costmap2D;
+using cm::Costmap2DROS;
+
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "path_planning_node");//hjr
+
+    tf2_ros::Buffer tf;
+    tf2_ros::TransformListener tf2_listener(tf);
+    costmap_2d::Costmap2DROS lcr("cleaning_costmap", tf);
+    //planner_costmap_ros_->pause();
+
+    ros::Duration(5).sleep();
+    CleaningPathPlanning clr(&lcr);
+    clr.GetPathInROS();
+    //clr.GetBorderTrackingPathInROS();   
+    ros::Rate r(1);
+    while(ros::ok()){       
+      clr.PublishCoveragePath();
+      ros::spinOnce();
+      r.sleep();
+    }
+
+    ros::shutdown();//关闭节点以及所有与之相关的发布，订阅，调用与服务。
+    return 0;
+}
+
--- a/clean_robot/src/next_goal.cpp
+++ b/clean_robot/src/next_goal.cpp
@ -0,0 +1,223 @@
+
+/*This code is used to plan the trajectory of the robot  
+*/
+
+#include <ros/ros.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include "nav_msgs/Odometry.h"
+#include "nav_msgs/Path.h"
+#include "geometry_msgs/PoseStamped.h"
+#include <tf/transform_listener.h>
+#include <iostream>
+#include <fstream>
+
+using namespace std;
+using namespace tf;
+
+float x_current;
+float y_current;
+
+float normeNextGoal;
+
+class quaternion_ros
+{
+public:
+  float w;
+  float x;
+  float y;
+  float z;
+
+  quaternion_ros();
+
+  void toQuaternion(float pitch, float roll, float yaw);
+};
+
+void quaternion_ros::toQuaternion(float pitch, float roll, float yaw)
+{
+
+  float cy = cos(yaw * 0.5);
+  float sy = sin(yaw * 0.5);
+  float cr = cos(roll * 0.5);
+  float sr = sin(roll * 0.5);
+  float cp = cos(pitch * 0.5);
+  float sp = sin(pitch * 0.5);
+
+  w = cy * cr * cp + sy * sr * sp;
+  x = cy * sr * cp - sy * cr * sp;
+  y = cy * cr * sp + sy * sr * cp;
+  z = sy * cr * cp - cy * sr * sp;
+}
+
+quaternion_ros::quaternion_ros()
+{
+  w = 1;
+  x = 0;
+  y = 0;
+  z = 0;
+}
+
+class Path_planned
+{
+public:
+  struct Goal
+  {
+    float x;
+    float y;
+    bool visited;
+  };
+
+  vector<Goal> Path;
+
+  Path_planned();
+  //Path_planned(float x, float y, bool visited);
+  void addGoal(float X, float Y, bool visit);
+};
+
+Path_planned::Path_planned()
+{
+}
+
+//Path_planned(float x, float y, bool visited)
+
+void Path_planned::addGoal(float X, float Y, bool visit)
+{
+  Path_planned::Goal newGoal;
+  newGoal.x = X;
+  newGoal.y = Y;
+  newGoal.visited = visit;
+  Path.push_back(newGoal);
+}
+
+Path_planned planned_path;
+nav_msgs::Path passed_path;
+ros::Publisher pub_passed_path;
+void pose_callback(const nav_msgs::Odometry &poses)
+{ //里程计回调函数,用来计算当前机器人位置与前面目标点的距离,判断是否要发新的幕摆点
+  x_current = poses.pose.pose.position.x;
+  y_current = poses.pose.pose.position.y;
+  passed_path.header = poses.header;
+  geometry_msgs::PoseStamped p;
+  p.header = poses.header;
+  p.pose = poses.pose.pose;
+  passed_path.poses.emplace_back(p);
+  pub_passed_path.publish(passed_path);
+}
+
+int taille_last_path = 0;
+bool new_path = false;
+
+//接受规划的路径
+void path_callback(const nav_msgs::Path &path)
+{
+  //注意为了rviz显示方便 路径一直在发,但是这里只用接受一次就好,当规划的路径发生变化时候再重新装载
+  if ((planned_path.Path.size() == 0) || (path.poses.size() != taille_last_path))
+  {
+    planned_path.Path.clear();
+    new_path = true;
+    for (int i = 0; i < path.poses.size(); i++)
+    {
+      planned_path.addGoal(path.poses[i].pose.position.x, path.poses[i].pose.position.y, false);
+
+      cout << path.poses[i].pose.position.x << " " << path.poses[i].pose.position.y << endl;
+    }
+    cout << "Recv path size:" << path.poses.size() << endl;
+    taille_last_path = path.poses.size();
+  }
+}
+
+// int **count_antonin(char *)
+
+int main(int argc, char *argv[])
+{
+  srand(time(0));
+  ros::init(argc, argv, "next_goal");
+  ros::NodeHandle next_goal;
+  ros::Subscriber sub1 = next_goal.subscribe("/odom", 1000, pose_callback);
+  ros::Subscriber sub2 = next_goal.subscribe("/path_planning_node/cleaning_plan_nodehandle/cleaning_path", 1000, path_callback);
+
+  ros::Publisher pub1 = next_goal.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1000);
+  pub_passed_path = next_goal.advertise<nav_msgs::Path>("/clean_robot/passed_path", 1000);
+
+  ros::Rate loop_rate(10);
+
+  geometry_msgs::PoseStamped goal_msgs;
+  int count = 0;
+  double angle;
+  bool goal_reached = false;
+  //获取发送下一个点的阈值
+  if (!next_goal.getParam("/NextGoal/tolerance_goal", normeNextGoal))
+  {
+    ROS_ERROR("Please set your tolerance_goal");
+    return 0;
+  }
+  ROS_INFO("tolerance_goal=%f", normeNextGoal);
+
+  while (ros::ok())
+  {
+    ros::spinOnce();
+    if (new_path)
+    {
+      count = 0;
+      new_path = false;
+    }
+    //当前处理的点
+    cout << " count : " << count << endl;
+    if (!planned_path.Path.empty())
+    {
+      //当前距离达到了
+      if (sqrt(pow(x_current - planned_path.Path[count].x, 2) + pow(y_current - planned_path.Path[count].y, 2)) <= normeNextGoal)
+      {
+        count++;
+        goal_reached = false;
+      }
+      if (goal_reached == false)
+      {
+        goal_msgs.header.frame_id = "odom";
+        goal_msgs.header.stamp = ros::Time::now();
+        goal_msgs.pose.position.x = planned_path.Path[count].x;
+        goal_msgs.pose.position.y = planned_path.Path[count].y;
+        goal_msgs.pose.position.z = 0;
+        if (count < planned_path.Path.size())
+        {//计算发布的yaw，不过还有bug 但是不影响使用，yaw不会产生太大影响
+          angle = atan2(planned_path.Path[count + 1].y - planned_path.Path[count].y, planned_path.Path[count + 1].x - planned_path.Path[count].x);
+        }
+        else
+        {
+          angle = atan2(planned_path.Path[0].y - planned_path.Path[count].y, planned_path.Path[0].x - planned_path.Path[count].x);
+        }
+        cout << angle << endl;
+        quaternion_ros q;
+        q.toQuaternion(0, 0, float(angle));
+        goal_msgs.pose.orientation.w = q.w;
+        goal_msgs.pose.orientation.x = q.x;
+        goal_msgs.pose.orientation.y = q.y;
+        if (planned_path.Path[count].x < planned_path.Path[count + 1].x)
+        {
+          goal_msgs.pose.orientation.z = 0;
+        }
+        if (planned_path.Path[count].x > planned_path.Path[count + 1].x)
+        {
+          goal_msgs.pose.orientation.z = 2;
+        }
+
+        cout << " NEW GOAL " << endl;
+        cout << " x = " << planned_path.Path[count].x << " y = " << planned_path.Path[count].y << endl;
+
+        goal_reached = true;
+        pub1.publish(goal_msgs);
+      }
+      cout << x_current << " " << y_current << endl;
+      //当前
+      cout << planned_path.Path[count].x << " " << planned_path.Path[count].y << endl;
+      //目标
+      cout << " DISTANCE : " << sqrt((x_current - planned_path.Path[count].x) * (x_current - planned_path.Path[count].x) + (y_current - planned_path.Path[count].y) * (y_current - planned_path.Path[count].y)) << endl;
+      // 距离公式
+    }
+
+    loop_rate.sleep();
+  }
+
+  return 0;
+}
--- a/clean_robot/worlds/clean_room.world
+++ b/clean_robot/worlds/clean_room.world
@ -0,0 +1,592 @@
+<sdf version='1.6'>
+  <world name='default'>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose frame=''>0 0 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.1 0.1 0.1 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.5 -1</direction>
+    </light>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+    </model>
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <physics name='default_physics' default='0' type='ode'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>1</shadows>
+    </scene>
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <latitude_deg>0</latitude_deg>
+      <longitude_deg>0</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+    <model name='clean_room'>
+      <pose frame=''>-0.225 -3.145 0 0 -0 0</pose>
+      <link name='Wall_0'>
+        <collision name='Wall_0_Collision'>
+          <geometry>
+            <box>
+              <size>6 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_0_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>6 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/CeilingTiled</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>0 1.925 0 0 -0 0</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_1'>
+        <collision name='Wall_1_Collision'>
+          <geometry>
+            <box>
+              <size>4 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_1_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/CeilingTiled</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>2.925 0 0 0 0 -1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_12'>
+        <collision name='Wall_12_Collision'>
+          <geometry>
+            <box>
+              <size>1.25 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_12_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1.25 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Wood</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>-0.032459 1.37074 0 0 0 -1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_15'>
+        <collision name='Wall_15_Collision'>
+          <geometry>
+            <box>
+              <size>1.5 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_15_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1.5 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Wood</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>-0.033608 -1.2111 0 0 -0 1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_2'>
+        <collision name='Wall_2_Collision'>
+          <geometry>
+            <box>
+              <size>6 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_2_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>6 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/CeilingTiled</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>0 -1.925 0 0 -0 3.14159</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_3'>
+        <collision name='Wall_3_Collision'>
+          <geometry>
+            <box>
+              <size>4 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_3_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/CeilingTiled</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>-2.925 0 0 0 -0 1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <static>1</static>
+    </model>
+    <model name='unit_box_0'>
+      <pose frame=''>-0.943901 -1.17332 0.5 0 -0 0</pose>
+      <link name='link'>
+        <inertial>
+          <mass>1</mass>
+          <inertia>
+            <ixx>0.166667</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.166667</iyy>
+            <iyz>0</iyz>
+            <izz>0.166667</izz>
+          </inertia>
+        </inertial>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <name>Gazebo/Grey</name>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+    </model>
+    <state world_name='default'>
+      <sim_time>202 804000000</sim_time>
+      <real_time>203 218099251</real_time>
+      <wall_time>1592817139 630737498</wall_time>
+      <iterations>202804</iterations>
+      <model name='clean_room'>
+        <pose frame=''>-2.27449 -1.25584 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='Wall_0'>
+          <pose frame=''>-2.27449 0.669159 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_1'>
+          <pose frame=''>0.650514 -1.25584 0 0 0 -1.5708</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_12'>
+          <pose frame=''>-2.30694 0.114899 0 0 0 -1.5708</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_15'>
+          <pose frame=''>-2.30809 -2.46694 0 0 -0 1.5708</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_2'>
+          <pose frame=''>-2.27449 -3.18084 0 0 -0 3.14159</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_3'>
+          <pose frame=''>-5.19949 -1.25584 0 0 -0 1.5708</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='ground_plane'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='unit_box_0'>
+        <pose frame=''>-0.943901 -1.17332 0.862371 0 -0 0</pose>
+        <scale>1 1 1.65575</scale>
+        <link name='link'>
+          <pose frame=''>-0.943901 -1.17332 0.862371 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>-0.004709 -9.78112 9.78158 0.712677 -0.009414 -4.3e-05</acceleration>
+          <wrench>-0.004709 -9.78112 9.78158 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='unit_box_1'>
+        <pose frame=''>-0.589858 -2.38679 0.5 0 -0 0</pose>
+        <scale>1 0.451115 1</scale>
+        <link name='link'>
+          <pose frame=''>-0.589858 -2.38679 0.5 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='unit_box_2'>
+        <pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
+        <scale>1 1.44086 1</scale>
+        <link name='link'>
+          <pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <light name='sun'>
+        <pose frame=''>0 0 10 0 -0 0</pose>
+      </light>
+    </state>
+    <model name='unit_box_1'>
+      <pose frame=''>-0.221262 -2.44008 0.5 0 -0 0</pose>
+      <link name='link'>
+        <inertial>
+          <mass>1</mass>
+          <inertia>
+            <ixx>0.166667</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.166667</iyy>
+            <iyz>0</iyz>
+            <izz>0.166667</izz>
+          </inertia>
+        </inertial>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <name>Gazebo/Grey</name>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+    </model>
+    <model name='unit_box_2'>
+      <pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
+      <link name='link'>
+        <inertial>
+          <mass>1</mass>
+          <inertia>
+            <ixx>0.166667</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.166667</iyy>
+            <iyz>0</iyz>
+            <izz>0.166667</izz>
+          </inertia>
+        </inertial>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>1 0.999995 1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>1 0.999995 1</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <name>Gazebo/Grey</name>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+    </model>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>-4.40017 -4.63981 10.4277 0.0395 1.41825 0.665003</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+  </world>
+</sdf>
--- a/clean_robot/worlds/clean_room_new.world
+++ b/clean_robot/worlds/clean_room_new.world
@ -0,0 +1,592 @@
+<sdf version='1.6'>
+  <world name='default'>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose frame=''>0 0 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.1 0.1 0.1 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.5 -1</direction>
+    </light>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+    </model>
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <physics name='default_physics' default='0' type='ode'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>1</shadows>
+    </scene>
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <latitude_deg>0</latitude_deg>
+      <longitude_deg>0</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+    <model name='clean_room'>
+      <pose frame=''>-0.225 -3.145 0 0 -0 0</pose>
+      <link name='Wall_0'>
+        <collision name='Wall_0_Collision'>
+          <geometry>
+            <box>
+              <size>6 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_0_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>6 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/CeilingTiled</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>0 1.925 0 0 -0 0</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_1'>
+        <collision name='Wall_1_Collision'>
+          <geometry>
+            <box>
+              <size>4 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_1_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/CeilingTiled</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>2.925 0 0 0 0 -1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_12'>
+        <collision name='Wall_12_Collision'>
+          <geometry>
+            <box>
+              <size>1.25 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_12_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1.25 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Wood</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>-0.032459 1.37074 0 0 0 -1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_15'>
+        <collision name='Wall_15_Collision'>
+          <geometry>
+            <box>
+              <size>1.5 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_15_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1.5 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Wood</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>-0.033608 -1.2111 0 0 -0 1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_2'>
+        <collision name='Wall_2_Collision'>
+          <geometry>
+            <box>
+              <size>6 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_2_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>6 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/CeilingTiled</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>0 -1.925 0 0 -0 3.14159</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_3'>
+        <collision name='Wall_3_Collision'>
+          <geometry>
+            <box>
+              <size>4 0.15 2.5</size>
+            </box>
+          </geometry>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='Wall_3_Visual'>
+          <pose frame=''>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4 0.15 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/CeilingTiled</name>
+            </script>
+            <ambient>1 1 1 1</ambient>
+          </material>
+        </visual>
+        <pose frame=''>-2.925 0 0 0 -0 1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <static>1</static>
+    </model>
+    <model name='unit_box_0'>
+      <pose frame=''>-0.943901 -1.17332 0.5 0 -0 0</pose>
+      <link name='link'>
+        <inertial>
+          <mass>1</mass>
+          <inertia>
+            <ixx>0.166667</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.166667</iyy>
+            <iyz>0</iyz>
+            <izz>0.166667</izz>
+          </inertia>
+        </inertial>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <name>Gazebo/Grey</name>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+    </model>
+    <state world_name='default'>
+      <sim_time>211 64000000</sim_time>
+      <real_time>8 289779519</real_time>
+      <wall_time>1593047453 346825405</wall_time>
+      <iterations>8260</iterations>
+      <model name='clean_room'>
+        <pose frame=''>-2.27449 -1.25584 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='Wall_0'>
+          <pose frame=''>-2.27449 0.669159 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_1'>
+          <pose frame=''>0.650514 -1.25584 0 0 0 -1.5708</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_12'>
+          <pose frame=''>-2.30694 0.114899 0 0 0 -1.5708</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_15'>
+          <pose frame=''>-2.30809 -2.46694 0 0 -0 1.5708</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_2'>
+          <pose frame=''>-2.27449 -3.18084 0 0 -0 3.14159</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_3'>
+          <pose frame=''>-5.19949 -1.25584 0 0 -0 1.5708</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='ground_plane'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='unit_box_0'>
+        <pose frame=''>-0.943902 -1.17416 0.827873 4e-06 -1e-06 3e-06</pose>
+        <scale>1 1 1.65575</scale>
+        <link name='link'>
+          <pose frame=''>-0.943902 -1.17416 0.827873 4e-06 -1e-06 3e-06</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>2.2913 -2.31936 -5.06986 -0.472089 0.332594 -3.05735</acceleration>
+          <wrench>2.2913 -2.31936 -5.06986 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='unit_box_1'>
+        <pose frame=''>-0.589861 -2.72216 0.499997 0 -7e-06 0</pose>
+        <scale>1 0.451115 1</scale>
+        <link name='link'>
+          <pose frame=''>-0.589861 -2.72216 0.499997 0 -7e-06 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>-0.000647 -0.913095 0.411631 1.82618 -0.00123 0.0001</acceleration>
+          <wrench>-0.000647 -0.913095 0.411631 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='unit_box_2'>
+        <pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
+        <scale>1 1.44086 1</scale>
+        <link name='link'>
+          <pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>9.67937 0.027687 9.68609 -0.055112 0.511953 -7.7e-05</acceleration>
+          <wrench>9.67937 0.027687 9.68609 0 -0 0</wrench>
+        </link>
+      </model>
+      <light name='sun'>
+        <pose frame=''>0 0 10 0 -0 0</pose>
+      </light>
+    </state>
+    <model name='unit_box_1'>
+      <pose frame=''>-0.221262 -2.44008 0.5 0 -0 0</pose>
+      <link name='link'>
+        <inertial>
+          <mass>1</mass>
+          <inertia>
+            <ixx>0.166667</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.166667</iyy>
+            <iyz>0</iyz>
+            <izz>0.166667</izz>
+          </inertia>
+        </inertial>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <name>Gazebo/Grey</name>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+    </model>
+    <model name='unit_box_2'>
+      <pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
+      <link name='link'>
+        <inertial>
+          <mass>1</mass>
+          <inertia>
+            <ixx>0.166667</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.166667</iyy>
+            <iyz>0</iyz>
+            <izz>0.166667</izz>
+          </inertia>
+        </inertial>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>1 0.999993 1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>1 0.999993 1</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <name>Gazebo/Grey</name>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+    </model>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>-1.88641 -2.99318 3.5742 -0 1.23425 0.953003</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+  </world>
+</sdf>
--- a/explore/CHANGELOG.rst
+++ b/explore/CHANGELOG.rst
@ -0,0 +1,44 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package explore_lite
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.1.1 (2017-12-16)
+------------------
+* explore: fix min_frontier_size bug
+  * min_frontier_size parameter was not used to at all
+  * adjust min_frontier size filtering according to map resolution
+* fix bugs in CMakeLists.txt
+  * install nodes in packages, so they get shipped in debian packages. fixes `#11 <https://github.com/hrnr/m-explore/issues/11>`_
+* explore: update documentation
+* Contributors: Jiri Horner
+
+2.1.0 (2017-10-30)
+------------------
+* explore: get rid of boost
+* explore: rework launchfiles
+* new visualisation of frontiers
+* remove navfn library from dependencies
+* use frontier seach algorithm from Paul Bovbel
+  * much better than previous version of search
+  * https://github.com/paulbovbel/frontier_exploration.git
+* fix deadlock in explore
+  * reworked expore to use timer instead of separate thread for replanning
+  * fix deadlock occuring between makePlan and goal reached callback
+* Contributors: Jiri Horner
+
+2.0.0 (2017-03-26)
+------------------
+* explore: migrate to package format 2
+* explore: remove internal version of navfn_ros
+  * my changes are included in the ros since kinetic
+* Contributors: Jiri Horner
+
+1.0.1 (2017-03-25)
+------------------
+* update documentation
+* Contributors: Jiri Horner
+
+1.0.0 (2016-05-11)
+------------------
+* initial release
+* Contributors: Jiri Horner, duhadway-bosch, pitzer
--- a/explore/CMakeLists.txt
+++ b/explore/CMakeLists.txt
@ -0,0 +1,79 @@
+cmake_minimum_required(VERSION 3.1)
+project(explore_lite)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  actionlib_msgs
+  costmap_2d
+  geometry_msgs
+  map_msgs
+  move_base_msgs
+  nav_msgs
+  roscpp
+  std_msgs
+  tf
+  visualization_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS
+    actionlib_msgs
+    geometry_msgs
+    map_msgs
+    move_base_msgs
+    nav_msgs
+    std_msgs
+    visualization_msgs
+)
+
+###########
+## Build ##
+###########
+# c++11 support required
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+## Specify additional locations of header files
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  include
+)
+
+add_executable(explore
+  src/costmap_client.cpp
+  src/explore.cpp
+  src/frontier_search.cpp
+)
+add_dependencies(explore ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(explore ${catkin_LIBRARIES})
+
+#############
+## Install ##
+#############
+
+# install nodes
+install(TARGETS explore
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# install roslaunch files
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
+
+#############
+## Testing ##
+#############
+if(CATKIN_ENABLE_TESTING)
+  find_package(roslaunch REQUIRED)
+
+  # test all launch files
+  roslaunch_add_file_check(launch)
+endif()
--- a/explore/doc/architecture.dia
+++ b/explore/doc/architecture.dia
--- a/explore/doc/screenshot.png
+++ b/explore/doc/screenshot.png
--- a/explore/doc/wiki_doc.txt
+++ b/explore/doc/wiki_doc.txt
@ -0,0 +1,148 @@
+<<PackageHeader(explore_lite)>>
+
+<<GitHubIssues(hrnr/m-explore)>>
+
+<<TOC(4)>>
+
+== Overview ==
+This package provides greedy frontier-based exploration. When node is running, robot will greedily explore its environment until no frontiers could be found. Movement commands will be send to [[move_base]].
+
+{{attachment:screenshot.png||width="755px"}}
+
+Unlike similar packages, {{{explore_lite}}} does not create its own costmap, which makes it easier to configure and more efficient (lighter on resources). Node simply subscribes to <<MsgLink(nav_msgs/OccupancyGrid)>> messages. Commands for robot movement are send to [[move_base]] node.
+
+Node can do frontier filtering and can operate even on non-inflated maps. Goal blacklisting allows to deal with places inaccessible for robot.
+
+<<Youtube(op0L0LyGNwY&rel=0)>>
+
+== Architecture ==
+{{{explore_lite}}} uses [[move_base]] for navigation. You need to run properly configured [[move_base]] node.
+
+{{attachment:architecture.svg||width="755px"}}
+
+{{{explore_lite}}} subscribes to a <<MsgLink(nav_msgs/OccupancyGrid)>> and <<MsgLink(map_msgs/OccupancyGridUpdate)>> messages to construct a map where it looks for frontiers. You can either use costmap published by [[move_base]] (ie. `<move_base>/global_costmap/costmap`) or you can use map constructed by mapping algorithm (SLAM).
+
+Depending on your environment you may achieve better results with either SLAM map or costmap published by `move_base`. Advantage of `move_base` costmap is the inflation which helps to deal with some very small unexplorable frontiers. When you are using a raw map produced by SLAM you should set the `min_frontier_size` parameter to some reasonable number to deal with the small frontiers. For details on both setups check the `explore.launch` and `explore_costmap.launch` launch files.
+
+== Setup ==
+
+Before starting experimenting with {{{explore_lite}}} you need to have working [[move_base]] for navigation. You should be able to navigate with [[move_base]] manually through [[rviz]]. Please refer to [[navigation#Tutorials]] for setting up [[move_base]] and the rest of the navigation stack with your robot.
+
+You should be also able to to navigate with [[move_base]] though unknown space in the map. If you set the goal to unknown place in the map, planning and navigating should work. With most planners this should work by default, refer to [[navfn#Parameters]] if you need to setup this for [[navfn]] planner (but should be enabled by default). Navigation through unknown space is required for {{{explore_lite}}}.
+
+If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`.
+
+If you have [[move_base]] configured correctly, you can start experimenting with {{{explore_lite}}}. Provided `explore.launch` should work out-of-the box in most cases, but as always you might need to adjust topic names and frame names according to your setup.
+
+== ROS API ==
+{{{
+#!clearsilver CS/NodeAPI
+
+name = explore
+desc = Provides exploration services offered by this package. Exploration will start immediately after node initialization.
+
+pub {
+  0.name  = ~frontiers
+  0.type = visualization_msgs/MarkerArray
+  0.desc = Visualization of frontiers considered by exploring algorithm. Each frontier is visualized by frontier points in blue and with a small sphere, which visualize the cost of the frontiers (costlier frontiers will have smaller spheres).
+}
+sub {
+  0.name = costmap
+  0.type = nav_msgs/OccupancyGrid
+  0.desc = Map which will be used for exploration planning. Can be either costmap from [[move_base]] or map created by SLAM (see above). Occupancy grid must have got properly marked unknown space, mapping algorithms usually track unknown space by default. If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`.
+
+  1.name = costmap_updates
+  1.type = map_msgs/OccupancyGridUpdate
+  1.desc = Incremental updates on costmap. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic.
+}
+
+param {
+  0.name = ~robot_base_frame
+  0.default = `base_link`
+  0.type = string
+  0.desc = The name of the base frame of the robot. This is used for determining robot position on map. Mandatory.
+
+  1.name = ~costmap_topic
+  1.default = `costmap`
+  1.type = string
+  1.desc = Specifies topic of source <<MsgLink(nav_msgs/OccupancyGrid)>>. Mandatory.
+
+  3.name = ~costmap_updates_topic
+  3.default = `costmap_updates`
+  3.type = string
+  3.desc = Specifies topic of source <<MsgLink(map_msgs/OccupancyGridUpdate)>>. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic.
+
+  4.name = ~visualize
+  4.default = `false`
+  4.type = bool
+  4.desc = Specifies whether or not publish visualized frontiers.
+
+  6.name = ~planner_frequency
+  6.default = `1.0`
+  6.type = double
+  6.desc = Rate in Hz at which new frontiers will computed and goal reconsidered.
+
+  7.name = ~progress_timeout
+  7.default = `30.0`
+  7.type = double
+  7.desc = Time in seconds. When robot do not make any progress for `progress_timeout`, current goal will be abandoned.
+
+  8.name = ~potential_scale
+  8.default = `1e-3`
+  8.type = double
+  8.desc = Used for weighting frontiers. This multiplicative parameter affects frontier potential component of the frontier weight (distance to frontier).
+
+  9.name = ~orientation_scale
+  9.default = `0`
+  9.type = double
+  9.desc = Used for weighting frontiers. This multiplicative parameter affects frontier orientation component of the frontier weight. This parameter does currently nothing and is provided solely for forward compatibility.
+
+  10.name = ~gain_scale
+  10.default = `1.0`
+  10.type = double
+  10.desc = Used for weighting frontiers. This multiplicative parameter affects frontier gain component of the frontier weight (frontier size).
+
+  11.name = ~transform_tolerance
+  11.default = `0.3`
+  11.type = double
+  11.desc = Transform tolerance to use when transforming robot pose.
+
+  12.name = ~min_frontier_size
+  12.default = `0.5`
+  12.type = double
+  12.desc = Minimum size of the frontier to consider the frontier as the exploration goal. In meters.
+}
+
+req_tf {
+  0.from = global_frame
+  0.to = robot_base_frame
+  0.desc = This transformation is usually provided by mapping algorithm. Those frames are usually called `map` and `base_link`. For adjusting `robot_base_frame` name see respective parameter. You don't need to set `global_frame`. The name for `global_frame` will be sourced from `costmap_topic` automatically.
+}
+
+act_called {
+  0.name = move_base
+  0.type = move_base_msgs/MoveBaseAction
+  0.desc = [[move_base]] actionlib API for posting goals. See [[move_base#Action API]] for details. This expects [[move_base]] node in the same namespace as `explore_lite`, you may want to remap this node if this is not true.
+}
+}}}
+
+== Acknowledgements ==
+
+This package was developed as part of my bachelor thesis at [[http://www.mff.cuni.cz/to.en/|Charles University]] in Prague.
+
+{{{
+@masterthesis{Hörner2016,
+  author = {Jiří Hörner},
+  title = {Map-merging for multi-robot system},
+  address = {Prague},
+  year = {2016},
+  school = {Charles University in Prague, Faculty of Mathematics and Physics},
+  type = {Bachelor's thesis},
+  URL = {https://is.cuni.cz/webapps/zzp/detail/174125/},
+}
+}}}
+
+This project was initially based on [[explore]] package by Charles !DuHadway. Most of the node has been rewritten since then. The current frontier search algorithm is based on [[frontier_exploration]] by Paul Bovbel.
+
+## AUTOGENERATED DON'T DELETE
+## CategoryPackage
--- a/explore/include/explore/costmap_client.h
+++ b/explore/include/explore/costmap_client.h
@ -0,0 +1,131 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#ifndef COSTMAP_CLIENT_
+#define COSTMAP_CLIENT_
+
+#include <costmap_2d/costmap_2d.h>
+#include <geometry_msgs/Pose.h>
+#include <map_msgs/OccupancyGridUpdate.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include <tf/transform_listener.h>
+
+namespace explore
+{
+class Costmap2DClient
+{
+public:
+  /**
+   * @brief Contructs client and start listening
+   * @details Constructor will block until first map update is received and
+   * map is ready to use, also will block before trasformation
+   * robot_base_frame <-> global_frame is available.
+   *
+   * @param param_nh node hadle to retrieve parameters from
+   * @param subscription_nh node hadle where topics will be subscribed
+   * @param tf_listener Will be used for transformation of robot pose.
+   */
+  Costmap2DClient(ros::NodeHandle& param_nh, ros::NodeHandle& subscription_nh,
+                  const tf::TransformListener* tf_listener);
+  /**
+   * @brief Get the pose of the robot in the global frame of the costmap
+   * @return pose of the robot in the global frame of the costmap
+   */
+  geometry_msgs::Pose getRobotPose() const;
+
+  /**
+   * @brief Return a pointer to the "master" costmap which receives updates from
+   * all the layers.
+   *
+   * This pointer will stay the same for the lifetime of Costmap2DClient object.
+   */
+  costmap_2d::Costmap2D* getCostmap()
+  {
+    return &costmap_;
+  }
+
+  /**
+   * @brief Return a pointer to the "master" costmap which receives updates from
+   * all the layers.
+   *
+   * This pointer will stay the same for the lifetime of Costmap2DClient object.
+   */
+  const costmap_2d::Costmap2D* getCostmap() const
+  {
+    return &costmap_;
+  }
+
+  /**
+   * @brief  Returns the global frame of the costmap
+   * @return The global frame of the costmap
+   */
+  const std::string& getGlobalFrameID() const
+  {
+    return global_frame_;
+  }
+
+  /**
+   * @brief  Returns the local frame of the costmap
+   * @return The local frame of the costmap
+   */
+  const std::string& getBaseFrameID() const
+  {
+    return robot_base_frame_;
+  }
+
+protected:
+  void updateFullMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+  void updatePartialMap(const map_msgs::OccupancyGridUpdate::ConstPtr& msg);
+
+  costmap_2d::Costmap2D costmap_;
+
+  const tf::TransformListener* const tf_;  ///< @brief Used for transforming
+                                           /// point clouds
+  std::string global_frame_;      ///< @brief The global frame for the costmap
+  std::string robot_base_frame_;  ///< @brief The frame_id of the robot base
+  double transform_tolerance_;    ///< timeout before transform errors
+
+private:
+  // will be unsubscribed at destruction
+  ros::Subscriber costmap_sub_;
+  ros::Subscriber costmap_updates_sub_;
+};
+
+}  // namespace explore
+
+#endif
--- a/explore/include/explore/costmap_tools.h
+++ b/explore/include/explore/costmap_tools.h
@ -0,0 +1,133 @@
+#ifndef COSTMAP_TOOLS_H_
+#define COSTMAP_TOOLS_H_
+
+#include <costmap_2d/costmap_2d.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/PolygonStamped.h>
+#include <ros/ros.h>
+
+namespace frontier_exploration
+{
+/**
+ * @brief Determine 4-connected neighbourhood of an input cell, checking for map
+ * edges
+ * @param idx input cell index
+ * @param costmap Reference to map data
+ * @return neighbour cell indexes
+ */
+std::vector<unsigned int> nhood4(unsigned int idx,
+                                 const costmap_2d::Costmap2D& costmap)
+{
+  // get 4-connected neighbourhood indexes, check for edge of map
+  std::vector<unsigned int> out;
+
+  unsigned int size_x_ = costmap.getSizeInCellsX(),
+               size_y_ = costmap.getSizeInCellsY();
+
+  if (idx > size_x_ * size_y_ - 1) {
+    ROS_WARN("Evaluating nhood for offmap point");
+    return out;
+  }
+
+  if (idx % size_x_ > 0) {
+    out.push_back(idx - 1);
+  }
+  if (idx % size_x_ < size_x_ - 1) {
+    out.push_back(idx + 1);
+  }
+  if (idx >= size_x_) {
+    out.push_back(idx - size_x_);
+  }
+  if (idx < size_x_ * (size_y_ - 1)) {
+    out.push_back(idx + size_x_);
+  }
+  return out;
+}
+
+/**
+ * @brief Determine 8-connected neighbourhood of an input cell, checking for map
+ * edges
+ * @param idx input cell index
+ * @param costmap Reference to map data
+ * @return neighbour cell indexes
+ */
+std::vector<unsigned int> nhood8(unsigned int idx,
+                                 const costmap_2d::Costmap2D& costmap)
+{
+  // get 8-connected neighbourhood indexes, check for edge of map
+  std::vector<unsigned int> out = nhood4(idx, costmap);
+
+  unsigned int size_x_ = costmap.getSizeInCellsX(),
+               size_y_ = costmap.getSizeInCellsY();
+
+  if (idx > size_x_ * size_y_ - 1) {
+    return out;
+  }
+
+  if (idx % size_x_ > 0 && idx >= size_x_) {
+    out.push_back(idx - 1 - size_x_);
+  }
+  if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) {
+    out.push_back(idx - 1 + size_x_);
+  }
+  if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) {
+    out.push_back(idx + 1 - size_x_);
+  }
+  if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) {
+    out.push_back(idx + 1 + size_x_);
+  }
+
+  return out;
+}
+
+/**
+ * @brief Find nearest cell of a specified value
+ * @param result Index of located cell
+ * @param start Index initial cell to search from
+ * @param val Specified value to search for
+ * @param costmap Reference to map data
+ * @return True if a cell with the requested value was found
+ */
+bool nearestCell(unsigned int& result, unsigned int start, unsigned char val,
+                 const costmap_2d::Costmap2D& costmap)
+{
+  const unsigned char* map = costmap.getCharMap();
+  const unsigned int size_x = costmap.getSizeInCellsX(),
+                     size_y = costmap.getSizeInCellsY();
+
+  if (start >= size_x * size_y) {
+    return false;
+  }
+
+  // initialize breadth first search
+  std::queue<unsigned int> bfs;
+  std::vector<bool> visited_flag(size_x * size_y, false);
+
+  // push initial cell
+  bfs.push(start);
+  visited_flag[start] = true;
+
+  // search for neighbouring cell matching value
+  while (!bfs.empty()) {
+    unsigned int idx = bfs.front();
+    bfs.pop();
+
+    // return if cell of correct value is found
+    if (map[idx] == val) {
+      result = idx;
+      return true;
+    }
+
+    // iterate over all adjacent unvisited cells
+    for (unsigned nbr : nhood8(idx, costmap)) {
+      if (!visited_flag[nbr]) {
+        bfs.push(nbr);
+        visited_flag[nbr] = true;
+      }
+    }
+  }
+
+  return false;
+}
+}
+#endif
--- a/explore/include/explore/explore.h
+++ b/explore/include/explore/explore.h
@ -0,0 +1,114 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Robert Bosch LLC.
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+#ifndef NAV_EXPLORE_H_
+#define NAV_EXPLORE_H_
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include <actionlib/client/simple_action_client.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <move_base_msgs/MoveBaseAction.h>
+#include <ros/ros.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <explore/costmap_client.h>
+#include <explore/frontier_search.h>
+
+namespace explore
+{
+/**
+ * @class Explore
+ * @brief A class adhering to the robot_actions::Action interface that moves the
+ * robot base to explore its environment.
+ */
+class Explore
+{
+public:
+  Explore();
+  ~Explore();
+
+  void start();
+  void stop();
+
+private:
+  /**
+   * @brief  Make a global plan
+   */
+  void makePlan();
+
+  /**
+   * @brief  Publish a frontiers as markers
+   */
+  void visualizeFrontiers(
+      const std::vector<frontier_exploration::Frontier>& frontiers);
+
+  void reachedGoal(const actionlib::SimpleClientGoalState& status,
+                   const move_base_msgs::MoveBaseResultConstPtr& result,
+                   const geometry_msgs::Point& frontier_goal);
+
+  bool goalOnBlacklist(const geometry_msgs::Point& goal);
+
+  ros::NodeHandle private_nh_;
+  ros::NodeHandle relative_nh_;
+  ros::Publisher marker_array_publisher_;
+  tf::TransformListener tf_listener_;
+
+  Costmap2DClient costmap_client_;
+  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>
+      move_base_client_;
+  frontier_exploration::FrontierSearch search_;
+  ros::Timer exploring_timer_;
+  ros::Timer oneshot_;
+
+  std::vector<geometry_msgs::Point> frontier_blacklist_;
+  geometry_msgs::Point prev_goal_;
+  double prev_distance_;
+  ros::Time last_progress_;
+  size_t last_markers_count_;
+
+  // parameters
+  double planner_frequency_;
+  double potential_scale_, orientation_scale_, gain_scale_;
+  ros::Duration progress_timeout_;
+  bool visualize_;
+};
+}
+
+#endif
--- a/explore/include/explore/frontier_search.h
+++ b/explore/include/explore/frontier_search.h
@ -0,0 +1,88 @@
+#ifndef FRONTIER_SEARCH_H_
+#define FRONTIER_SEARCH_H_
+
+#include <costmap_2d/costmap_2d.h>
+
+namespace frontier_exploration
+{
+/**
+ * @brief Represents a frontier
+ *
+ */
+struct Frontier {
+  std::uint32_t size;
+  double min_distance;
+  double cost;
+  geometry_msgs::Point initial;
+  geometry_msgs::Point centroid;
+  geometry_msgs::Point middle;
+  std::vector<geometry_msgs::Point> points;
+};
+
+/**
+ * @brief Thread-safe implementation of a frontier-search task for an input
+ * costmap.
+ */
+class FrontierSearch
+{
+public:
+  FrontierSearch()
+  {
+  }
+
+  /**
+   * @brief Constructor for search task
+   * @param costmap Reference to costmap data to search.
+   */
+  FrontierSearch(costmap_2d::Costmap2D* costmap, double potential_scale,
+                 double gain_scale, double min_frontier_size);
+
+  /**
+   * @brief Runs search implementation, outward from the start position
+   * @param position Initial position to search from
+   * @return List of frontiers, if any
+   */
+  std::vector<Frontier> searchFrom(geometry_msgs::Point position);
+
+protected:
+  /**
+   * @brief Starting from an initial cell, build a frontier from valid adjacent
+   * cells
+   * @param initial_cell Index of cell to start frontier building
+   * @param reference Reference index to calculate position from
+   * @param frontier_flag Flag vector indicating which cells are already marked
+   * as frontiers
+   * @return new frontier
+   */
+  Frontier buildNewFrontier(unsigned int initial_cell, unsigned int reference,
+                            std::vector<bool>& frontier_flag);
+
+  /**
+   * @brief isNewFrontierCell Evaluate if candidate cell is a valid candidate
+   * for a new frontier.
+   * @param idx Index of candidate cell
+   * @param frontier_flag Flag vector indicating which cells are already marked
+   * as frontiers
+   * @return true if the cell is frontier cell
+   */
+  bool isNewFrontierCell(unsigned int idx,
+                         const std::vector<bool>& frontier_flag);
+
+  /**
+   * @brief computes frontier cost
+   * @details cost function is defined by potential_scale and gain_scale
+   *
+   * @param frontier frontier for which compute the cost
+   * @return cost of the frontier
+   */
+  double frontierCost(const Frontier& frontier);
+
+private:
+  costmap_2d::Costmap2D* costmap_;
+  unsigned char* map_;
+  unsigned int size_x_, size_y_;
+  double potential_scale_, gain_scale_;
+  double min_frontier_size_;
+};
+}
+#endif
--- a/explore/launch/explore.launch
+++ b/explore/launch/explore.launch
@ -0,0 +1,25 @@
+<launch>
+<!-- move_base -->
+<include file="$(find turtlebot3_navigation)/launch/move_base.launch">
+   <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
+</include>
+
+<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
+  <param name="robot_base_frame" value="base_link"/>
+  <param name="costmap_topic" value="map"/>
+  <param name="costmap_updates_topic" value="map_updates"/>
+  <param name="visualize" value="true"/>
+  <param name="planner_frequency" value="0.33"/>
+  <param name="progress_timeout" value="50.0"/>
+  <param name="potential_scale" value="3.0"/>
+  <param name="orientation_scale" value="0.0"/>
+  <param name="gain_scale" value="1.0"/>
+  <param name="transform_tolerance" value="0.3"/>
+  <param name="min_frontier_size" value="0.6"/>
+</node>
+
+ <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
+   <param name="publish_frequency" type="double" value="50.0" />
+</node>
+
+</launch>
--- a/explore/launch/explore_costmap.launch
+++ b/explore/launch/explore_costmap.launch
@ -0,0 +1,15 @@
+<launch>
+<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
+  <param name="robot_base_frame" value="base_link"/>
+  <param name="costmap_topic" value="move_base/global_costmap/costmap"/>
+  <param name="costmap_updates_topic" value="move_base/global_costmap/costmap_updates"/>
+  <param name="visualize" value="true"/>
+  <param name="planner_frequency" value="0.33"/>
+  <param name="progress_timeout" value="30.0"/>
+  <param name="potential_scale" value="3.0"/>
+  <param name="orientation_scale" value="0.0"/>
+  <param name="gain_scale" value="1.0"/>
+  <param name="transform_tolerance" value="0.3"/>
+  <param name="min_frontier_size" value="0.5"/>
+</node>
+</launch>
--- a/explore/launch/explore_multi.launch
+++ b/explore/launch/explore_multi.launch
@ -0,0 +1,239 @@
+<launch>
+ 
+<!-- Argument -->
+  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
+  <arg name="move_forward_only" default="false"/>
+  <arg name="cmd_vel_topic" default="/cmd_vel" />
+  <arg name="odom_topic" default="odom" />
+ 
+<group ns="tb3_0">
+<!-- Turtlebot3 -->
+  <include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml">
+    <arg name="model" value="$(arg model)" />
+  </include>
+ 
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
+    <param name="publish_frequency" type="double" value="50.0" />
+    <param name="tf_prefix" value="tb3_0" />
+  </node>
+ 
+<!-- move_base -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
+    <rosparam file="$(find explore_lite)/param/first_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_0/global_costmap" />
+    <rosparam file="$(find explore_lite)/param/first_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_0/local_costmap" />
+    <rosparam file="$(find explore_lite)/param/first_local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find explore_lite)/param/first_global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find explore_lite)/param/move_base_params.yaml" command="load" />
+    <rosparam file="$(find explore_lite)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
+    <remap from="cmd_vel" to="/tb3_0/cmd_vel"/>
+    <remap from="odom" to="tb3_0/odom"/>
+    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
+  </node>
+ 
+<!-- SLAM -->
+  <node pkg="gmapping" type="slam_gmapping" name="turtlebot3_slam_gmapping" output="screen">
+    <param name="base_frame" value="/tb3_0/base_footprint"/>
+    <param name="odom_frame" value="/tb3_0/odom"/>
+    <param name="map_frame"  value="/tb3_0/map"/>
+    <param name="map_update_interval" value="2.0"/>
+    <param name="maxUrange" value="3.0"/>
+    <param name="sigma" value="0.05"/>
+    <param name="kernelSize" value="1"/>
+    <param name="lstep" value="0.05"/>
+    <param name="astep" value="0.05"/>
+    <param name="iterations" value="5"/>
+    <param name="lsigma" value="0.075"/>
+    <param name="ogain" value="3.0"/>
+    <param name="lskip" value="0"/>
+    <param name="minimumScore" value="50"/>
+    <param name="srr" value="0.1"/>
+    <param name="srt" value="0.2"/>
+    <param name="str" value="0.1"/>
+    <param name="stt" value="0.2"/>
+    <param name="linearUpdate" value="1.0"/>
+    <param name="angularUpdate" value="0.2"/>
+    <param name="temporalUpdate" value="0.5"/>
+    <param name="resampleThreshold" value="0.5"/>
+    <param name="particles" value="100"/>
+    <param name="xmin" value="-10.0"/>
+    <param name="ymin" value="-10.0"/>
+    <param name="xmax" value="10.0"/>
+    <param name="ymax" value="10.0"/>
+    <param name="delta" value="0.05"/>
+    <param name="llsamplerange" value="0.01"/>
+    <param name="llsamplestep" value="0.01"/>
+    <param name="lasamplerange" value="0.005"/>
+    <param name="lasamplestep" value="0.005"/>
+  </node>
+ 
+<!-- Explore -->
+<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
+  <param name="robot_base_frame" value="/tb3_0/base_footprint"/>
+  <param name="costmap_topic" value="/tb3_0/map"/>
+  <param name="costmap_updates_topic" value="/tb3_0/map_updates"/>
+  <param name="visualize" value="true"/>
+  <param name="planner_frequency" value="0.33"/>
+  <param name="progress_timeout" value="30.0"/>
+  <param name="potential_scale" value="3.0"/>
+  <param name="orientation_scale" value="0.0"/>
+  <param name="gain_scale" value="1.0"/>
+  <param name="transform_tolerance" value="0.3"/>
+  <param name="min_frontier_size" value="0.75"/>
+</node>
+</group>
+ 
+<group ns="tb3_1">
+<!-- Turtlebot3 -->
+  <include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml">
+    <arg name="model" value="$(arg model)" />
+  </include>
+  
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
+    <param name="publish_frequency" type="double" value="50.0" />
+    <param name="tf_prefix" value="tb3_1" />
+  </node>
+ 
+<!-- move_base -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
+    <rosparam file="$(find explore_lite)/param/second_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_1/global_costmap" />
+    <rosparam file="$(find explore_lite)/param/second_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_1/local_costmap" />
+    <rosparam file="$(find explore_lite)/param/second_local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find explore_lite)/param/second_global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find explore_lite)/param/move_base_params.yaml" command="load" />
+    <rosparam file="$(find explore_lite)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
+    <remap from="cmd_vel" to="/tb3_1/cmd_vel"/>
+    <remap from="odom" to="/tb3_1/odom"/>
+    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
+  </node>
+ 
+  <node pkg="gmapping" type="slam_gmapping" name="turtlebot3_slam_gmapping" output="screen">
+    <param name="base_frame" value="/tb3_1/base_footprint"/>
+    <param name="odom_frame" value="/tb3_1/odom"/>
+    <param name="map_frame"  value="/tb3_1/map"/>
+    <param name="map_update_interval" value="2.0"/>
+    <param name="maxUrange" value="3.0"/>
+    <param name="sigma" value="0.05"/>
+    <param name="kernelSize" value="1"/>
+    <param name="lstep" value="0.05"/>
+    <param name="astep" value="0.05"/>
+    <param name="iterations" value="5"/>
+    <param name="lsigma" value="0.075"/>
+    <param name="ogain" value="3.0"/>
+    <param name="lskip" value="0"/>
+    <param name="minimumScore" value="50"/>
+    <param name="srr" value="0.1"/>
+    <param name="srt" value="0.2"/>
+    <param name="str" value="0.1"/>
+    <param name="stt" value="0.2"/>
+    <param name="linearUpdate" value="1.0"/>
+    <param name="angularUpdate" value="0.2"/>
+    <param name="temporalUpdate" value="0.5"/>
+    <param name="resampleThreshold" value="0.5"/>
+    <param name="particles" value="100"/>
+    <param name="xmin" value="-10.0"/>
+    <param name="ymin" value="-10.0"/>
+    <param name="xmax" value="10.0"/>
+    <param name="ymax" value="10.0"/>
+    <param name="delta" value="0.05"/>
+    <param name="llsamplerange" value="0.01"/>
+    <param name="llsamplestep" value="0.01"/>
+    <param name="lasamplerange" value="0.005"/>
+    <param name="lasamplestep" value="0.005"/>
+  </node>
+ 
+<!-- Explore -->
+<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
+  <param name="robot_base_frame" value="/tb3_1/base_footprint"/>
+  <param name="costmap_topic" value="/tb3_1/map"/>
+  <param name="costmap_updates_topic" value="/tb3_1/map_updates"/>
+  <param name="visualize" value="true"/>
+  <param name="planner_frequency" value="0.33"/>
+  <param name="progress_timeout" value="30.0"/>
+  <param name="potential_scale" value="3.0"/>
+  <param name="orientation_scale" value="0.0"/>
+  <param name="gain_scale" value="1.0"/>
+  <param name="transform_tolerance" value="0.3"/>
+  <param name="min_frontier_size" value="0.75"/>
+</node>
+</group>
+ 
+<group ns="tb3_2">
+<!-- Turtlebot3 -->
+  <include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml">
+    <arg name="model" value="$(arg model)" />
+  </include>
+  
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
+    <param name="publish_frequency" type="double" value="50.0" />
+    <param name="tf_prefix" value="tb3_2" />
+  </node>
+ 
+<!-- move_base -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
+    <rosparam file="$(find explore_lite)/param/third_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_2/global_costmap" />
+    <rosparam file="$(find explore_lite)/param/third_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_2/local_costmap" />
+    <rosparam file="$(find explore_lite)/param/third_local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find explore_lite)/param/third_global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find explore_lite)/param/move_base_params.yaml" command="load" />
+    <rosparam file="$(find explore_lite)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
+    <remap from="cmd_vel" to="/tb3_2/cmd_vel"/>
+    <remap from="odom" to="/tb3_2/odom"/>
+    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
+  </node>
+ 
+<!-- SLAM -->
+  <node pkg="gmapping" type="slam_gmapping" name="turtlebot3_slam_gmapping" output="screen">
+    <param name="base_frame" value="/tb3_2/base_footprint"/>
+    <param name="odom_frame" value="/tb3_2/odom"/>
+    <param name="map_frame"  value="/tb3_2/map"/>
+    <param name="map_update_interval" value="2.0"/>
+    <param name="maxUrange" value="3.0"/>
+    <param name="sigma" value="0.05"/>
+    <param name="kernelSize" value="1"/>
+    <param name="lstep" value="0.05"/>
+    <param name="astep" value="0.05"/>
+    <param name="iterations" value="5"/>
+    <param name="lsigma" value="0.075"/>
+    <param name="ogain" value="3.0"/>
+    <param name="lskip" value="0"/>
+    <param name="minimumScore" value="50"/>
+    <param name="srr" value="0.1"/>
+    <param name="srt" value="0.2"/>
+    <param name="str" value="0.1"/>
+    <param name="stt" value="0.2"/>
+    <param name="linearUpdate" value="1.0"/>
+    <param name="angularUpdate" value="0.2"/>
+    <param name="temporalUpdate" value="0.5"/>
+    <param name="resampleThreshold" value="0.5"/>
+    <param name="particles" value="100"/>
+    <param name="xmin" value="-10.0"/>
+    <param name="ymin" value="-10.0"/>
+    <param name="xmax" value="10.0"/>
+    <param name="ymax" value="10.0"/>
+    <param name="delta" value="0.05"/>
+    <param name="llsamplerange" value="0.01"/>
+    <param name="llsamplestep" value="0.01"/>
+    <param name="lasamplerange" value="0.005"/>
+    <param name="lasamplestep" value="0.005"/>
+  </node>
+ 
+<!-- Explore -->
+<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
+  <param name="robot_base_frame" value="/tb3_2/base_footprint"/>
+  <param name="costmap_topic" value="/tb3_2/map"/>
+  <param name="costmap_updates_topic" value="/tb3_2/map_updates"/>
+  <param name="visualize" value="true"/>
+  <param name="planner_frequency" value="0.33"/>
+  <param name="progress_timeout" value="30.0"/>
+  <param name="potential_scale" value="3.0"/>
+  <param name="orientation_scale" value="0.0"/>
+  <param name="gain_scale" value="1.0"/>
+  <param name="transform_tolerance" value="0.3"/>
+  <param name="min_frontier_size" value="0.75"/>
+</node>
+</group>
+ 
+</launch>
--- a/explore/package.xml
+++ b/explore/package.xml
@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>explore_lite</name>
+  <version>2.1.1</version>
+
+  <description>Lightweight frontier-based exploration.</description>
+
+  <author email="laeqten@gmail.com">Jiri Horner</author>
+  <maintainer email="laeqten@gmail.com">Jiri Horner</maintainer>
+  <license>BSD</license>
+  <url>http://wiki.ros.org/explore_lite</url>
+
+  <!-- acknowledgements
+  <author>Charles DuHadway</author>
+  <url>http://ros.org/wiki/explore</url>
+  <author>Paul Bovbel</author>
+  <url>https://github.com/paulbovbel/frontier_exploration</url>
+  -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>move_base_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>map_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>actionlib_msgs</depend>
+  <depend>tf</depend>
+  <depend>costmap_2d</depend>
+  <depend>actionlib</depend>
+
+  <test_depend>roslaunch</test_depend>
+</package>
--- a/explore/src/costmap_client.cpp
+++ b/explore/src/costmap_client.cpp
@ -0,0 +1,252 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#include <explore/costmap_client.h>
+
+#include <functional>
+#include <mutex>
+#include <string>
+
+namespace explore
+{
+// static translation table to speed things up
+std::array<unsigned char, 256> init_translation_table();
+static const std::array<unsigned char, 256> cost_translation_table__ =
+    init_translation_table();
+
+Costmap2DClient::Costmap2DClient(ros::NodeHandle& param_nh,
+                                 ros::NodeHandle& subscription_nh,
+                                 const tf::TransformListener* tf)
+  : tf_(tf)
+{
+  std::string costmap_topic;
+  std::string footprint_topic;
+  std::string costmap_updates_topic;
+  param_nh.param("costmap_topic", costmap_topic, std::string("costmap"));
+  param_nh.param("costmap_updates_topic", costmap_updates_topic,
+                 std::string("costmap_updates"));
+  param_nh.param("robot_base_frame", robot_base_frame_,
+                 std::string("base_link"));
+  // transform tolerance is used for all tf transforms here
+  param_nh.param("transform_tolerance", transform_tolerance_, 0.3);
+
+  /* initialize costmap */
+  costmap_sub_ = subscription_nh.subscribe<nav_msgs::OccupancyGrid>(
+      costmap_topic, 1000,
+      [this](const nav_msgs::OccupancyGrid::ConstPtr& msg) {
+        updateFullMap(msg);
+      });
+  ROS_INFO("Waiting for costmap to become available, topic: %s",
+           costmap_topic.c_str());
+  auto costmap_msg = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>(
+      costmap_topic, subscription_nh);
+  updateFullMap(costmap_msg);
+
+  /* subscribe to map updates */
+  costmap_updates_sub_ =
+      subscription_nh.subscribe<map_msgs::OccupancyGridUpdate>(
+          costmap_updates_topic, 1000,
+          [this](const map_msgs::OccupancyGridUpdate::ConstPtr& msg) {
+            updatePartialMap(msg);
+          });
+
+  /* resolve tf prefix for robot_base_frame */
+  std::string tf_prefix = tf::getPrefixParam(param_nh);
+  robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);
+
+  // we need to make sure that the transform between the robot base frame and
+  // the global frame is available
+  /* tf transform is necessary for getRobotPose */
+  ros::Time last_error = ros::Time::now();
+  std::string tf_error;
+  while (ros::ok() &&
+         !tf_->waitForTransform(global_frame_, robot_base_frame_, ros::Time(),
+                                ros::Duration(0.1), ros::Duration(0.01),
+                                &tf_error)) {
+    ros::spinOnce();
+    if (last_error + ros::Duration(5.0) < ros::Time::now()) {
+      ROS_WARN(
+          "Timed out waiting for transform from %s to %s to become available "
+          "before subscribing to costmap, tf error: %s",
+          robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
+      last_error = ros::Time::now();
+    }
+    // The error string will accumulate and errors will typically be the same,
+    // so the last
+    // will do for the warning above. Reset the string here to avoid
+    // accumulation.
+    tf_error.clear();
+  }
+}
+
+void Costmap2DClient::updateFullMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+  global_frame_ = msg->header.frame_id;
+
+  unsigned int size_in_cells_x = msg->info.width;
+  unsigned int size_in_cells_y = msg->info.height;
+  double resolution = msg->info.resolution;
+  double origin_x = msg->info.origin.position.x;
+  double origin_y = msg->info.origin.position.y;
+
+  ROS_DEBUG("received full new map, resizing to: %d, %d", size_in_cells_x,
+            size_in_cells_y);
+  costmap_.resizeMap(size_in_cells_x, size_in_cells_y, resolution, origin_x,
+                     origin_y);
+
+  // lock as we are accessing raw underlying map
+  auto* mutex = costmap_.getMutex();
+  std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
+
+  // fill map with data
+  unsigned char* costmap_data = costmap_.getCharMap();
+  size_t costmap_size = costmap_.getSizeInCellsX() * costmap_.getSizeInCellsY();
+  ROS_DEBUG("full map update, %lu values", costmap_size);
+  for (size_t i = 0; i < costmap_size && i < msg->data.size(); ++i) {
+    unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
+    costmap_data[i] = cost_translation_table__[cell_cost];
+  }
+  ROS_DEBUG("map updated, written %lu values", costmap_size);
+}
+
+void Costmap2DClient::updatePartialMap(
+    const map_msgs::OccupancyGridUpdate::ConstPtr& msg)
+{
+  ROS_DEBUG("received partial map update");
+  global_frame_ = msg->header.frame_id;
+
+  if (msg->x < 0 || msg->y < 0) {
+    ROS_ERROR("negative coordinates, invalid update. x: %d, y: %d", msg->x,
+              msg->y);
+    return;
+  }
+
+  size_t x0 = static_cast<size_t>(msg->x);
+  size_t y0 = static_cast<size_t>(msg->y);
+  size_t xn = msg->width + x0;
+  size_t yn = msg->height + y0;
+
+  // lock as we are accessing raw underlying map
+  auto* mutex = costmap_.getMutex();
+  std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
+
+  size_t costmap_xn = costmap_.getSizeInCellsX();
+  size_t costmap_yn = costmap_.getSizeInCellsY();
+
+  if (xn > costmap_xn || x0 > costmap_xn || yn > costmap_yn ||
+      y0 > costmap_yn) {
+    ROS_WARN("received update doesn't fully fit into existing map, "
+             "only part will be copied. received: [%lu, %lu], [%lu, %lu] "
+             "map is: [0, %lu], [0, %lu]",
+             x0, xn, y0, yn, costmap_xn, costmap_yn);
+  }
+
+  // update map with data
+  unsigned char* costmap_data = costmap_.getCharMap();
+  size_t i = 0;
+  for (size_t y = y0; y < yn && y < costmap_yn; ++y) {
+    for (size_t x = x0; x < xn && x < costmap_xn; ++x) {
+      size_t idx = costmap_.getIndex(x, y);
+      unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
+      costmap_data[idx] = cost_translation_table__[cell_cost];
+      ++i;
+    }
+  }
+}
+
+geometry_msgs::Pose Costmap2DClient::getRobotPose() const
+{
+  tf::Stamped<tf::Pose> global_pose;
+  global_pose.setIdentity();
+  tf::Stamped<tf::Pose> robot_pose;
+  robot_pose.setIdentity();
+  robot_pose.frame_id_ = robot_base_frame_;
+  robot_pose.stamp_ = ros::Time();
+  ros::Time current_time =
+      ros::Time::now();  // save time for checking tf delay later
+
+  // get the global pose of the robot
+  try {
+    tf_->transformPose(global_frame_, robot_pose, global_pose);
+  } catch (tf::LookupException& ex) {
+    ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot "
+                            "pose: %s\n",
+                       ex.what());
+    return {};
+  } catch (tf::ConnectivityException& ex) {
+    ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n",
+                       ex.what());
+    return {};
+  } catch (tf::ExtrapolationException& ex) {
+    ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n",
+                       ex.what());
+    return {};
+  }
+  // check global_pose timeout
+  if (current_time.toSec() - global_pose.stamp_.toSec() >
+      transform_tolerance_) {
+    ROS_WARN_THROTTLE(1.0, "Costmap2DClient transform timeout. Current time: "
+                           "%.4f, global_pose stamp: %.4f, tolerance: %.4f",
+                      current_time.toSec(), global_pose.stamp_.toSec(),
+                      transform_tolerance_);
+    return {};
+  }
+
+  geometry_msgs::PoseStamped msg;
+  tf::poseStampedTFToMsg(global_pose, msg);
+  return msg.pose;
+}
+
+std::array<unsigned char, 256> init_translation_table()
+{
+  std::array<unsigned char, 256> cost_translation_table;
+
+  // lineary mapped from [0..100] to [0..255]
+  for (size_t i = 0; i < 256; ++i) {
+    cost_translation_table[i] =
+        static_cast<unsigned char>(1 + (251 * (i - 1)) / 97);
+  }
+
+  // special values:
+  cost_translation_table[0] = 0;      // NO obstacle
+  cost_translation_table[99] = 253;   // INSCRIBED obstacle
+  cost_translation_table[100] = 254;  // LETHAL obstacle
+  cost_translation_table[static_cast<unsigned char>(-1)] = 255;  // UNKNOWN
+
+  return cost_translation_table;
+}
+
+}  // namespace explore
--- a/explore/src/explore.cpp
+++ b/explore/src/explore.cpp
@ -0,0 +1,308 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Robert Bosch LLC.
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#include <explore/explore.h>
+
+#include <thread>
+
+inline static bool operator==(const geometry_msgs::Point& one,
+                              const geometry_msgs::Point& two)
+{
+  double dx = one.x - two.x;
+  double dy = one.y - two.y;
+  double dist = sqrt(dx * dx + dy * dy);
+  return dist < 0.01;
+}
+
+namespace explore
+{
+Explore::Explore()
+  : private_nh_("~")
+  , tf_listener_(ros::Duration(10.0))
+  , costmap_client_(private_nh_, relative_nh_, &tf_listener_)
+  , move_base_client_("move_base")
+  , prev_distance_(0)
+  , last_markers_count_(0)
+{
+  double timeout;
+  double min_frontier_size;
+  private_nh_.param("planner_frequency", planner_frequency_, 1.0);
+  private_nh_.param("progress_timeout", timeout, 30.0);
+  progress_timeout_ = ros::Duration(timeout);
+  private_nh_.param("visualize", visualize_, false);
+  private_nh_.param("potential_scale", potential_scale_, 1e-3);
+  private_nh_.param("orientation_scale", orientation_scale_, 0.0);
+  private_nh_.param("gain_scale", gain_scale_, 1.0);
+  private_nh_.param("min_frontier_size", min_frontier_size, 0.5);
+
+  search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
+                                                 potential_scale_, gain_scale_,
+                                                 min_frontier_size);
+
+  if (visualize_) {
+    marker_array_publisher_ =
+        private_nh_.advertise<visualization_msgs::MarkerArray>("frontiers", 10);
+  }
+
+  ROS_INFO("Waiting to connect to move_base server");
+  move_base_client_.waitForServer();
+  ROS_INFO("Connected to move_base server");
+
+  exploring_timer_ =
+      relative_nh_.createTimer(ros::Duration(1. / planner_frequency_),
+                               [this](const ros::TimerEvent&) { makePlan(); });
+}
+
+Explore::~Explore()
+{
+  stop();
+}
+
+void Explore::visualizeFrontiers(
+    const std::vector<frontier_exploration::Frontier>& frontiers)
+{
+  std_msgs::ColorRGBA blue;
+  blue.r = 0;
+  blue.g = 0;
+  blue.b = 1.0;
+  blue.a = 1.0;
+  std_msgs::ColorRGBA red;
+  red.r = 1.0;
+  red.g = 0;
+  red.b = 0;
+  red.a = 1.0;
+  std_msgs::ColorRGBA green;
+  green.r = 0;
+  green.g = 1.0;
+  green.b = 0;
+  green.a = 1.0;
+
+  ROS_DEBUG("visualising %lu frontiers", frontiers.size());
+  visualization_msgs::MarkerArray markers_msg;
+  std::vector<visualization_msgs::Marker>& markers = markers_msg.markers;
+  visualization_msgs::Marker m;
+
+  m.header.frame_id = costmap_client_.getGlobalFrameID();
+  m.header.stamp = ros::Time::now();
+  m.ns = "frontiers";
+  m.scale.x = 1.0;
+  m.scale.y = 1.0;
+  m.scale.z = 1.0;
+  m.color.r = 0;
+  m.color.g = 0;
+  m.color.b = 255;
+  m.color.a = 255;
+  // lives forever
+  m.lifetime = ros::Duration(0);
+  m.frame_locked = true;
+
+  // weighted frontiers are always sorted
+  double min_cost = frontiers.empty() ? 0. : frontiers.front().cost;
+
+  m.action = visualization_msgs::Marker::ADD;
+  size_t id = 0;
+  for (auto& frontier : frontiers) {
+    m.type = visualization_msgs::Marker::POINTS;
+    m.id = int(id);
+    m.pose.position = {};
+    m.scale.x = 0.1;
+    m.scale.y = 0.1;
+    m.scale.z = 0.1;
+    m.points = frontier.points;
+    if (goalOnBlacklist(frontier.centroid)) {
+      m.color = red;
+    } else {
+      m.color = blue;
+    }
+    markers.push_back(m);
+    ++id;
+    m.type = visualization_msgs::Marker::SPHERE;
+    m.id = int(id);
+    m.pose.position = frontier.initial;
+    // scale frontier according to its cost (costier frontiers will be smaller)
+    double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5);
+    m.scale.x = scale;
+    m.scale.y = scale;
+    m.scale.z = scale;
+    m.points = {};
+    m.color = green;
+    markers.push_back(m);
+    ++id;
+  }
+  size_t current_markers_count = markers.size();
+
+  // delete previous markers, which are now unused
+  m.action = visualization_msgs::Marker::DELETE;
+  for (; id < last_markers_count_; ++id) {
+    m.id = int(id);
+    markers.push_back(m);
+  }
+
+  last_markers_count_ = current_markers_count;
+  marker_array_publisher_.publish(markers_msg);
+}
+
+void Explore::makePlan()
+{
+  // find frontiers
+  auto pose = costmap_client_.getRobotPose();
+  // get frontiers sorted according to cost
+  auto frontiers = search_.searchFrom(pose.position);
+  ROS_DEBUG("found %lu frontiers", frontiers.size());
+  for (size_t i = 0; i < frontiers.size(); ++i) {
+    ROS_DEBUG("frontier %zd cost: %f", i, frontiers[i].cost);
+  }
+
+  if (frontiers.empty()) {
+    stop();
+    return;
+  }
+
+  // publish frontiers as visualization markers
+  if (visualize_) {
+    visualizeFrontiers(frontiers);
+  }
+
+  // find non blacklisted frontier
+  auto frontier =
+      std::find_if_not(frontiers.begin(), frontiers.end(),
+                       [this](const frontier_exploration::Frontier& f) {
+                         return goalOnBlacklist(f.centroid);
+                       });
+  if (frontier == frontiers.end()) {
+    stop();
+    return;
+  }
+  geometry_msgs::Point target_position = frontier->centroid;
+
+  // time out if we are not making any progress
+  bool same_goal = prev_goal_ == target_position;
+  prev_goal_ = target_position;
+  if (!same_goal || prev_distance_ > frontier->min_distance) {
+    // we have different goal or we made some progress
+    last_progress_ = ros::Time::now();
+    prev_distance_ = frontier->min_distance;
+  }
+  // black list if we've made no progress for a long time
+  if (ros::Time::now() - last_progress_ > progress_timeout_) {
+    frontier_blacklist_.push_back(target_position);
+    ROS_DEBUG("Adding current goal to black list");
+    makePlan();
+    return;
+  }
+
+  // we don't need to do anything if we still pursuing the same goal
+  if (same_goal) {
+    return;
+  }
+
+  // send goal to move_base if we have something new to pursue
+  move_base_msgs::MoveBaseGoal goal;
+  goal.target_pose.pose.position = target_position;
+  goal.target_pose.pose.orientation.w = 1.;
+  goal.target_pose.header.frame_id = costmap_client_.getGlobalFrameID();
+  goal.target_pose.header.stamp = ros::Time::now();
+  move_base_client_.sendGoal(
+      goal, [this, target_position](
+                const actionlib::SimpleClientGoalState& status,
+                const move_base_msgs::MoveBaseResultConstPtr& result) {
+        reachedGoal(status, result, target_position);
+      });
+}
+
+bool Explore::goalOnBlacklist(const geometry_msgs::Point& goal)
+{
+  constexpr static size_t tolerace = 5;
+  costmap_2d::Costmap2D* costmap2d = costmap_client_.getCostmap();
+
+  // check if a goal is on the blacklist for goals that we're pursuing
+  for (auto& frontier_goal : frontier_blacklist_) {
+    double x_diff = fabs(goal.x - frontier_goal.x);
+    double y_diff = fabs(goal.y - frontier_goal.y);
+
+    if (x_diff < tolerace * costmap2d->getResolution() &&
+        y_diff < tolerace * costmap2d->getResolution())
+      return true;
+  }
+  return false;
+}
+
+void Explore::reachedGoal(const actionlib::SimpleClientGoalState& status,
+                          const move_base_msgs::MoveBaseResultConstPtr&,
+                          const geometry_msgs::Point& frontier_goal)
+{
+  ROS_DEBUG("Reached goal with status: %s", status.toString().c_str());
+  if (status == actionlib::SimpleClientGoalState::ABORTED) {
+    frontier_blacklist_.push_back(frontier_goal);
+    ROS_DEBUG("Adding current goal to black list");
+  }
+
+  // find new goal immediatelly regardless of planning frequency.
+  // execute via timer to prevent dead lock in move_base_client (this is
+  // callback for sendGoal, which is called in makePlan). the timer must live
+  // until callback is executed.
+  oneshot_ = relative_nh_.createTimer(
+      ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); },
+      true);
+}
+
+void Explore::start()
+{
+  exploring_timer_.start();
+}
+
+void Explore::stop()
+{
+  move_base_client_.cancelAllGoals();
+  exploring_timer_.stop();
+  ROS_INFO("Exploration stopped.");
+}
+
+}  // namespace explore
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "explore");
+  if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
+                                     ros::console::levels::Debug)) {
+    ros::console::notifyLoggerLevelsChanged();
+  }
+  explore::Explore explore;
+  ros::spin();
+
+  return 0;
+}
--- a/explore/src/frontier_search.cpp
+++ b/explore/src/frontier_search.cpp
@ -0,0 +1,197 @@
+#include <explore/frontier_search.h>
+
+#include <mutex>
+
+#include <costmap_2d/cost_values.h>
+#include <costmap_2d/costmap_2d.h>
+#include <geometry_msgs/Point.h>
+
+#include <explore/costmap_tools.h>
+
+namespace frontier_exploration
+{
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::FREE_SPACE;
+
+FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap,
+                               double potential_scale, double gain_scale,
+                               double min_frontier_size)
+  : costmap_(costmap)
+  , potential_scale_(potential_scale)
+  , gain_scale_(gain_scale)
+  , min_frontier_size_(min_frontier_size)
+{
+}
+
+std::vector<Frontier> FrontierSearch::searchFrom(geometry_msgs::Point position)
+{
+  std::vector<Frontier> frontier_list;
+
+  // Sanity check that robot is inside costmap bounds before searching
+  unsigned int mx, my;
+  if (!costmap_->worldToMap(position.x, position.y, mx, my)) {
+    ROS_ERROR("Robot out of costmap bounds, cannot search for frontiers");
+    return frontier_list;
+  }
+
+  // make sure map is consistent and locked for duration of search
+  std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+
+  map_ = costmap_->getCharMap();
+  size_x_ = costmap_->getSizeInCellsX();
+  size_y_ = costmap_->getSizeInCellsY();
+
+  // initialize flag arrays to keep track of visited and frontier cells
+  std::vector<bool> frontier_flag(size_x_ * size_y_, false);
+  std::vector<bool> visited_flag(size_x_ * size_y_, false);
+
+  // initialize breadth first search
+  std::queue<unsigned int> bfs;
+
+  // find closest clear cell to start search
+  unsigned int clear, pos = costmap_->getIndex(mx, my);
+  if (nearestCell(clear, pos, FREE_SPACE, *costmap_)) {
+    bfs.push(clear);
+  } else {
+    bfs.push(pos);
+    ROS_WARN("Could not find nearby clear cell to start search");
+  }
+  visited_flag[bfs.front()] = true;
+
+  while (!bfs.empty()) {
+    unsigned int idx = bfs.front();
+    bfs.pop();
+
+    // iterate over 4-connected neighbourhood
+    for (unsigned nbr : nhood4(idx, *costmap_)) {
+      // add to queue all free, unvisited cells, use descending search in case
+      // initialized on non-free cell
+      if (map_[nbr] <= map_[idx] && !visited_flag[nbr]) {
+        visited_flag[nbr] = true;
+        bfs.push(nbr);
+        // check if cell is new frontier cell (unvisited, NO_INFORMATION, free
+        // neighbour)
+      } else if (isNewFrontierCell(nbr, frontier_flag)) {
+        frontier_flag[nbr] = true;
+        Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag);
+        if (new_frontier.size * costmap_->getResolution() >=
+            min_frontier_size_) {
+          frontier_list.push_back(new_frontier);
+        }
+      }
+    }
+  }
+
+  // set costs of frontiers
+  for (auto& frontier : frontier_list) {
+    frontier.cost = frontierCost(frontier);
+  }
+  std::sort(
+      frontier_list.begin(), frontier_list.end(),
+      [](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; });
+
+  return frontier_list;
+}
+
+Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell,
+                                          unsigned int reference,
+                                          std::vector<bool>& frontier_flag)
+{
+  // initialize frontier structure
+  Frontier output;
+  output.centroid.x = 0;
+  output.centroid.y = 0;
+  output.size = 1;
+  output.min_distance = std::numeric_limits<double>::infinity();
+
+  // record initial contact point for frontier
+  unsigned int ix, iy;
+  costmap_->indexToCells(initial_cell, ix, iy);
+  costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y);
+
+  // push initial gridcell onto queue
+  std::queue<unsigned int> bfs;
+  bfs.push(initial_cell);
+
+  // cache reference position in world coords
+  unsigned int rx, ry;
+  double reference_x, reference_y;
+  costmap_->indexToCells(reference, rx, ry);
+  costmap_->mapToWorld(rx, ry, reference_x, reference_y);
+
+  while (!bfs.empty()) {
+    unsigned int idx = bfs.front();
+    bfs.pop();
+
+    // try adding cells in 8-connected neighborhood to frontier
+    for (unsigned int nbr : nhood8(idx, *costmap_)) {
+      // check if neighbour is a potential frontier cell
+      if (isNewFrontierCell(nbr, frontier_flag)) {
+        // mark cell as frontier
+        frontier_flag[nbr] = true;
+        unsigned int mx, my;
+        double wx, wy;
+        costmap_->indexToCells(nbr, mx, my);
+        costmap_->mapToWorld(mx, my, wx, wy);
+
+        geometry_msgs::Point point;
+        point.x = wx;
+        point.y = wy;
+        output.points.push_back(point);
+
+        // update frontier size
+        output.size++;
+
+        // update centroid of frontier
+        output.centroid.x += wx;
+        output.centroid.y += wy;
+
+        // determine frontier's distance from robot, going by closest gridcell
+        // to robot
+        double distance = sqrt(pow((double(reference_x) - double(wx)), 2.0) +
+                               pow((double(reference_y) - double(wy)), 2.0));
+        if (distance < output.min_distance) {
+          output.min_distance = distance;
+          output.middle.x = wx;
+          output.middle.y = wy;
+        }
+
+        // add to queue for breadth first search
+        bfs.push(nbr);
+      }
+    }
+  }
+
+  // average out frontier centroid
+  output.centroid.x /= output.size;
+  output.centroid.y /= output.size;
+  return output;
+}
+
+bool FrontierSearch::isNewFrontierCell(unsigned int idx,
+                                       const std::vector<bool>& frontier_flag)
+{
+  // check that cell is unknown and not already marked as frontier
+  if (map_[idx] != NO_INFORMATION || frontier_flag[idx]) {
+    return false;
+  }
+
+  // frontier cells should have at least one cell in 4-connected neighbourhood
+  // that is free
+  for (unsigned int nbr : nhood4(idx, *costmap_)) {
+    if (map_[nbr] == FREE_SPACE) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
+double FrontierSearch::frontierCost(const Frontier& frontier)
+{
+  return (potential_scale_ * frontier.min_distance *
+          costmap_->getResolution()) -
+         (gain_scale_ * frontier.size * costmap_->getResolution());
+}
+}
--- a/img/exploration.png
+++ b/img/exploration.png
--- a/img/fcpp.png
+++ b/img/fcpp.png
--- a/img/result.png
+++ b/img/result.png
--- a/img/structure.png
+++ b/img/structure.png
--- a/ros-clean-robot.pdf
+++ b/ros-clean-robot.pdf