commit 11fff5f14ac0d9a72b4ffb55c2c8bb3ba9e28b2a Author: zhangrelay Date: Mon Mar 14 20:24:35 2022 +0800 Initial commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..bdf689c --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +/CMakeLists.txt +*.pyc +*.o +.vscode \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..2bb1d68 --- /dev/null +++ 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) + diff --git a/clean_robot/CMakeLists.txt b/clean_robot/CMakeLists.txt new file mode 100644 index 0000000..1c5c111 --- /dev/null +++ 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} +) \ No newline at end of file diff --git a/clean_robot/config/cleaning_costmap_params.yaml b/clean_robot/config/cleaning_costmap_params.yaml new file mode 100644 index 0000000..0340003 --- /dev/null +++ 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 diff --git a/clean_robot/config/costmap_common_params.yaml b/clean_robot/config/costmap_common_params.yaml new file mode 100644 index 0000000..94ccae0 --- /dev/null +++ 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 diff --git a/clean_robot/include/CleaningPathPlanner.h b/clean_robot/include/CleaningPathPlanner.h new file mode 100644 index 0000000..8a75508 --- /dev/null +++ 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 +#include +#include +#include +#include +#include + +#include "tf/tf.h" +#include "tf/transform_listener.h" +#include +#include +#include +#include + +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 GetPathInROS(); + vector 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 &freeSpaceVec); + void initializeNeuralMat(Mat cellMat, Mat neuralizedMat); + void writeResult(Mat resultmat,vector pathVec); + void writeResult(cv::Mat resultmat,std::vector pathVec); + void mainPlanningLoop(); + double distance(Point2i pta,Point2i ptb); + bool findElement(vector pointsVec,cv::Point2i pt, int&index); + void publishPlan(const std::vector& path); + bool cellContainsPoint(cv::Point2i pt,cellIndex cell); + + void GetBorderTrackingPathInCV(vector&resultVec); + vector GetPathInCV(); + + + bool initialized_; + Mat srcMap_; + Mat cellMat_; + Mat neuralizedMat_; + vector freeSpaceVec_; + vector pathVec_; + vector pathVecInROS_; + + double resolution_; + ros::Publisher plan_pub_; + ros::Publisher grid_pub_; + nav_msgs::OccupancyGrid covered_path_grid_; + + //tf::TransformListener &tf_; + tf::Stamped 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 diff --git a/clean_robot/launch/amcl.launch b/clean_robot/launch/amcl.launch new file mode 100644 index 0000000..7ad0465 --- /dev/null +++ b/clean_robot/launch/amcl.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/clean_robot/launch/auto_slam.launch b/clean_robot/launch/auto_slam.launch new file mode 100644 index 0000000..7c53063 --- /dev/null +++ b/clean_robot/launch/auto_slam.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/clean_robot/launch/clean_work.launch b/clean_robot/launch/clean_work.launch new file mode 100644 index 0000000..679b913 --- /dev/null +++ b/clean_robot/launch/clean_work.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/clean_robot/launch/gazebo.launch b/clean_robot/launch/gazebo.launch new file mode 100644 index 0000000..61e7631 --- /dev/null +++ b/clean_robot/launch/gazebo.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/clean_robot/launch/move_base.launch b/clean_robot/launch/move_base.launch new file mode 100644 index 0000000..ee71fcb --- /dev/null +++ b/clean_robot/launch/move_base.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/clean_robot/launch/nav_slam.launch b/clean_robot/launch/nav_slam.launch new file mode 100644 index 0000000..7193b4e --- /dev/null +++ b/clean_robot/launch/nav_slam.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/clean_robot/launch/turtlebot3_navigation.launch b/clean_robot/launch/turtlebot3_navigation.launch new file mode 100644 index 0000000..54c4f73 --- /dev/null +++ b/clean_robot/launch/turtlebot3_navigation.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/clean_robot/maps/clean_room.pgm b/clean_robot/maps/clean_room.pgm new file mode 100644 index 0000000..cefb973 Binary files /dev/null and b/clean_robot/maps/clean_room.pgm differ diff --git a/clean_robot/maps/clean_room.yaml b/clean_robot/maps/clean_room.yaml new file mode 100644 index 0000000..ff9c44d --- /dev/null +++ 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 + diff --git a/clean_robot/maps/clean_room_new.pgm b/clean_robot/maps/clean_room_new.pgm new file mode 100644 index 0000000..8c479a8 Binary files /dev/null and b/clean_robot/maps/clean_room_new.pgm differ diff --git a/clean_robot/maps/clean_room_new.yaml b/clean_robot/maps/clean_room_new.yaml new file mode 100644 index 0000000..545bcc0 --- /dev/null +++ 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 + diff --git a/clean_robot/package.xml b/clean_robot/package.xml new file mode 100644 index 0000000..81ee954 --- /dev/null +++ b/clean_robot/package.xml @@ -0,0 +1,80 @@ + + + clean_robot + 0.0.0 + The clean_robot package + + + + + derek + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + actionlib + costmap_2d + geometry_msgs + move_base_msgs + nav_msgs + roscpp + tf + actionlib + costmap_2d + geometry_msgs + move_base_msgs + nav_msgs + roscpp + tf + actionlib + costmap_2d + geometry_msgs + move_base_msgs + nav_msgs + roscpp + tf + + + + + + + + diff --git a/clean_robot/param/base_local_planner_params.yaml b/clean_robot/param/base_local_planner_params.yaml new file mode 100644 index 0000000..2c5896e --- /dev/null +++ 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 diff --git a/clean_robot/param/costmap_common_params_burger.yaml b/clean_robot/param/costmap_common_params_burger.yaml new file mode 100644 index 0000000..a30b928 --- /dev/null +++ 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} \ No newline at end of file diff --git a/clean_robot/param/costmap_common_params_waffle.yaml b/clean_robot/param/costmap_common_params_waffle.yaml new file mode 100644 index 0000000..3d5a6d7 --- /dev/null +++ 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} diff --git a/clean_robot/param/costmap_common_params_waffle_pi.yaml b/clean_robot/param/costmap_common_params_waffle_pi.yaml new file mode 100644 index 0000000..3d5a6d7 --- /dev/null +++ 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} diff --git a/clean_robot/param/dwa_local_planner_params_burger.yaml b/clean_robot/param/dwa_local_planner_params_burger.yaml new file mode 100644 index 0000000..f625674 --- /dev/null +++ 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 diff --git a/clean_robot/param/dwa_local_planner_params_waffle.yaml b/clean_robot/param/dwa_local_planner_params_waffle.yaml new file mode 100644 index 0000000..b89997f --- /dev/null +++ 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 diff --git a/clean_robot/param/dwa_local_planner_params_waffle_pi.yaml b/clean_robot/param/dwa_local_planner_params_waffle_pi.yaml new file mode 100644 index 0000000..b89997f --- /dev/null +++ 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 diff --git a/clean_robot/param/global_costmap_params.yaml b/clean_robot/param/global_costmap_params.yaml new file mode 100644 index 0000000..bf475f5 --- /dev/null +++ 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 + diff --git a/clean_robot/param/local_costmap_params.yaml b/clean_robot/param/local_costmap_params.yaml new file mode 100644 index 0000000..98c1845 --- /dev/null +++ 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 + diff --git a/clean_robot/param/move_base_params.yaml b/clean_robot/param/move_base_params.yaml new file mode 100644 index 0000000..f042cd6 --- /dev/null +++ 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 + diff --git a/clean_robot/rviz/turtlebot3_karto.rviz b/clean_robot/rviz/turtlebot3_karto.rviz new file mode 100644 index 0000000..019ee20 --- /dev/null +++ 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 diff --git a/clean_robot/rviz/turtlebot3_navigation.rviz b/clean_robot/rviz/turtlebot3_navigation.rviz new file mode 100644 index 0000000..43cbce5 --- /dev/null +++ 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: + 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: + 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 diff --git a/clean_robot/src/CleaningPathPlanner.cpp b/clean_robot/src/CleaningPathPlanner.cpp new file mode 100644 index 0000000..5b50bf8 --- /dev/null +++ b/clean_robot/src/CleaningPathPlanner.cpp @@ -0,0 +1,724 @@ +#include "CleaningPathPlanner.h" +#include + +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("cleaning_path", 1); + grid_pub_ = private_nh.advertise("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(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 CleaningPathPlanning::GetPathInROS() +{ + // vector resultVec; + if (!pathVecInROS_.empty()) + pathVecInROS_.clear(); //清空操作 + geometry_msgs::PoseStamped posestamped; + geometry_msgs::Pose pose; + vector cellvec; + cellvec = GetPathInCV(); + /*trasnsform*/ + vector::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 CleaningPathPlanning::GetBorderTrackingPathInROS() +{ + //vector resultPathInROS; + if (!pathVecInROS_.empty()) + pathVecInROS_.clear(); + geometry_msgs::PoseStamped posestamped; + geometry_msgs::Pose pose; + vector resultCV; + GetBorderTrackingPathInCV(resultCV); + vector 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 &resultVec) +{ + std::vector 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(r,c)); + if (srcMap_.at(r, c) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { + ROS_INFO("entered once!"); + if (srcMap_.at(r, c - 1) != srcMap_.at(r, c + 1) && (srcMap_.at(r, c - 1) == costmap_2d::FREE_SPACE || srcMap_.at(r, c + 1) == costmap_2d::FREE_SPACE)) + { + temppoint.x = c; + temppoint.y = r; + borderPointsIndexVec.push_back(temppoint); + } + else if (srcMap_.at(r - 1, c) != srcMap_.at(r + 1, c) && (srcMap_.at(r - 1, c) == costmap_2d::FREE_SPACE || srcMap_.at(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 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 CleaningPathPlanning::GetPathInCV() +{ + mainPlanningLoop(); + return this->pathVec_; +} + +void CleaningPathPlanning::PublishCoveragePath() +{ + publishPlan(this->pathVecInROS_); +} + +void CleaningPathPlanning::publishPlan(const std::vector &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 &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(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(r, c) = costmap_2d::FREE_SPACE; //0 + } + else + { + cellMat.at(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(i, j) == costmap_2d::LETHAL_OBSTACLE) + neuralizedMat.at(i, j) = -100000.0; + else + neuralizedMat.at(i, j) = 50.0 / j; //这里的1.0/j的用意是什么？这里有没有考虑到j=0时刻的问题呢？ hjr注 + } + } + return; +} + +void CleaningPathPlanning::writeResult(Mat resultmat, vector 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(pathVec[i].row*SIZE_OF_CELL,pathVec[i].col*SIZE_OF_CELL)[0] = 0; + resultMat.at(pathVec[i].row*SIZE_OF_CELL,pathVec[i].col*SIZE_OF_CELL)[1] = 0; + resultMat.at(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 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: "<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 thetaVec = {0, 45, 90, 135, 180, 225, 270, 315}; + //vector 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::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(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(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(currentPoint.row - 1, currentPoint.col + 1) + c_0 * e - 200; + + break; + case 2: + if (currentPoint.row == 0) + { + v = -100000; + break; + } + v = neuralizedMat_.at(currentPoint.row - 1, currentPoint.col) + c_0 * e; + + break; + case 3: + if (currentPoint.col == 0 || currentPoint.row == 0) + { + v = -100000; + break; + } + v = neuralizedMat_.at(currentPoint.row - 1, currentPoint.col - 1) + c_0 * e - 200; + + break; + case 4: + if (currentPoint.col == 0) + { + v = -100000; + break; + } + v = neuralizedMat_.at(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(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(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(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::iterator min_iter; + int ii = 0, min_index = -1; + for (it = freeSpaceVec_.begin(); it != freeSpaceVec_.end(); it++) + { + if (neuralizedMat_.at((*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 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 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((a + i), (b + m)) == -250.0) + num++; + } + } + if (num != 0) + return true; + else + return false; +} diff --git a/clean_robot/src/CleaningPathPlanner.h b/clean_robot/src/CleaningPathPlanner.h new file mode 100644 index 0000000..bf2d74d --- /dev/null +++ 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 +#include +#include +#include +#include +#include + +#include "tf/tf.h" +#include "tf/transform_listener.h" +#include +#include +#include +#include + +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 GetPathInROS(); + vector 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 &freeSpaceVec); + void initializeNeuralMat(Mat cellMat, Mat neuralizedMat); + void writeResult(Mat resultmat, vector pathVec); + void writeResult(cv::Mat resultmat, std::vector pathVec); + void mainPlanningLoop(); + double distance(Point2i pta, Point2i ptb); + bool findElement(vector pointsVec, cv::Point2i pt, int &index); + void publishPlan(const std::vector &path); + bool cellContainsPoint(cv::Point2i pt, cellIndex cell); + + void GetBorderTrackingPathInCV(vector &resultVec); + vector GetPathInCV(); + + bool initialized_; + Mat srcMap_; + Mat cellMat_; + Mat neuralizedMat_; + vector freeSpaceVec_; + vector pathVec_; + vector 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 diff --git a/clean_robot/src/PathPlanningNode.cpp b/clean_robot/src/PathPlanningNode.cpp new file mode 100644 index 0000000..d3724f1 --- /dev/null +++ b/clean_robot/src/PathPlanningNode.cpp @@ -0,0 +1,37 @@ +#include "CleaningPathPlanner.h" +#include +#include + +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; +} + diff --git a/clean_robot/src/next_goal.cpp b/clean_robot/src/next_goal.cpp new file mode 100644 index 0000000..f4a1d50 --- /dev/null +++ b/clean_robot/src/next_goal.cpp @@ -0,0 +1,223 @@ + +/*This code is used to plan the trajectory of the robot +*/ + +#include +#include +#include +#include +#include "nav_msgs/Odometry.h" +#include "nav_msgs/Path.h" +#include "geometry_msgs/PoseStamped.h" +#include +#include +#include + +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 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("/move_base_simple/goal", 1000); + pub_passed_path = next_goal.advertise("/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; +} diff --git a/clean_robot/worlds/clean_room.world b/clean_robot/worlds/clean_room.world new file mode 100644 index 0000000..1f5cd3c --- /dev/null +++ b/clean_robot/worlds/clean_room.world @@ -0,0 +1,592 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.1 0.1 0.1 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.5 -1 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 1 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + -0.225 -3.145 0 0 -0 0 + + + + + 6 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 6 0.15 2.5 + + + + + 1 1 1 1 + + + 0 1.925 0 0 -0 0 + 0 + 0 + 1 + + + + + + 4 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4 0.15 2.5 + + + + + 1 1 1 1 + + + 2.925 0 0 0 0 -1.5708 + 0 + 0 + 1 + + + + + + 1.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 1.25 0.15 2.5 + + + + + 1 1 1 1 + + + -0.032459 1.37074 0 0 0 -1.5708 + 0 + 0 + 1 + + + + + + 1.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 1.5 0.15 2.5 + + + + + 1 1 1 1 + + + -0.033608 -1.2111 0 0 -0 1.5708 + 0 + 0 + 1 + + + + + + 6 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 6 0.15 2.5 + + + + + 1 1 1 1 + + + 0 -1.925 0 0 -0 3.14159 + 0 + 0 + 1 + + + + + + 4 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4 0.15 2.5 + + + + + 1 1 1 1 + + + -2.925 0 0 0 -0 1.5708 + 0 + 0 + 1 + + 1 + + + -0.943901 -1.17332 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 1 + + + + 202 804000000 + 203 218099251 + 1592817139 630737498 + 202804 + + -2.27449 -1.25584 0 0 -0 0 + 1 1 1 + + -2.27449 0.669159 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.650514 -1.25584 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.30694 0.114899 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.30809 -2.46694 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.27449 -3.18084 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.19949 -1.25584 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.943901 -1.17332 0.862371 0 -0 0 + 1 1 1.65575 + + -0.943901 -1.17332 0.862371 0 -0 0 + 0 0 0 0 -0 0 + -0.004709 -9.78112 9.78158 0.712677 -0.009414 -4.3e-05 + -0.004709 -9.78112 9.78158 0 -0 0 + + + + -0.589858 -2.38679 0.5 0 -0 0 + 1 0.451115 1 + + -0.589858 -2.38679 0.5 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -3.70594 -1.29835 0.5 0 -0 0 + 1 1.44086 1 + + -3.70594 -1.29835 0.5 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + -0.221262 -2.44008 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 1 + + + + -3.70594 -1.29835 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 0.999995 1 + + + 10 + + + + + + + + + + + + + + + + + 1 0.999995 1 + + + + + + + 0 + 0 + 1 + + + + + -4.40017 -4.63981 10.4277 0.0395 1.41825 0.665003 + orbit + perspective + + + + diff --git a/clean_robot/worlds/clean_room_new.world b/clean_robot/worlds/clean_room_new.world new file mode 100644 index 0000000..af5d05e --- /dev/null +++ b/clean_robot/worlds/clean_room_new.world @@ -0,0 +1,592 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.1 0.1 0.1 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.5 -1 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 1 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + -0.225 -3.145 0 0 -0 0 + + + + + 6 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 6 0.15 2.5 + + + + + 1 1 1 1 + + + 0 1.925 0 0 -0 0 + 0 + 0 + 1 + + + + + + 4 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4 0.15 2.5 + + + + + 1 1 1 1 + + + 2.925 0 0 0 0 -1.5708 + 0 + 0 + 1 + + + + + + 1.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 1.25 0.15 2.5 + + + + + 1 1 1 1 + + + -0.032459 1.37074 0 0 0 -1.5708 + 0 + 0 + 1 + + + + + + 1.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 1.5 0.15 2.5 + + + + + 1 1 1 1 + + + -0.033608 -1.2111 0 0 -0 1.5708 + 0 + 0 + 1 + + + + + + 6 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 6 0.15 2.5 + + + + + 1 1 1 1 + + + 0 -1.925 0 0 -0 3.14159 + 0 + 0 + 1 + + + + + + 4 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4 0.15 2.5 + + + + + 1 1 1 1 + + + -2.925 0 0 0 -0 1.5708 + 0 + 0 + 1 + + 1 + + + -0.943901 -1.17332 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 1 + + + + 211 64000000 + 8 289779519 + 1593047453 346825405 + 8260 + + -2.27449 -1.25584 0 0 -0 0 + 1 1 1 + + -2.27449 0.669159 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.650514 -1.25584 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.30694 0.114899 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.30809 -2.46694 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.27449 -3.18084 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.19949 -1.25584 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.943902 -1.17416 0.827873 4e-06 -1e-06 3e-06 + 1 1 1.65575 + + -0.943902 -1.17416 0.827873 4e-06 -1e-06 3e-06 + 0 0 0 0 -0 0 + 2.2913 -2.31936 -5.06986 -0.472089 0.332594 -3.05735 + 2.2913 -2.31936 -5.06986 0 -0 0 + + + + -0.589861 -2.72216 0.499997 0 -7e-06 0 + 1 0.451115 1 + + -0.589861 -2.72216 0.499997 0 -7e-06 0 + 0 0 0 0 -0 0 + -0.000647 -0.913095 0.411631 1.82618 -0.00123 0.0001 + -0.000647 -0.913095 0.411631 0 -0 0 + + + + -3.70594 -1.29835 0.5 0 -0 0 + 1 1.44086 1 + + -3.70594 -1.29835 0.5 0 -0 0 + 0 0 0 0 -0 0 + 9.67937 0.027687 9.68609 -0.055112 0.511953 -7.7e-05 + 9.67937 0.027687 9.68609 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + -0.221262 -2.44008 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 1 + + + + -3.70594 -1.29835 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 0.999993 1 + + + 10 + + + + + + + + + + + + + + + + + 1 0.999993 1 + + + + + + + 0 + 0 + 1 + + + + + -1.88641 -2.99318 3.5742 -0 1.23425 0.953003 + orbit + perspective + + + + diff --git a/explore/CHANGELOG.rst b/explore/CHANGELOG.rst new file mode 100644 index 0000000..00d4650 --- /dev/null +++ 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 `_ +* 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 diff --git a/explore/CMakeLists.txt b/explore/CMakeLists.txt new file mode 100644 index 0000000..dd82e3b --- /dev/null +++ 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() diff --git a/explore/doc/architecture.dia b/explore/doc/architecture.dia new file mode 100644 index 0000000..cb0a472 Binary files /dev/null and b/explore/doc/architecture.dia differ diff --git a/explore/doc/screenshot.png b/explore/doc/screenshot.png new file mode 100644 index 0000000..216ade5 Binary files /dev/null and b/explore/doc/screenshot.png differ diff --git a/explore/doc/wiki_doc.txt b/explore/doc/wiki_doc.txt new file mode 100644 index 0000000..5ddb4fe --- /dev/null +++ b/explore/doc/wiki_doc.txt @@ -0,0 +1,148 @@ +<> + +<> + +<> + +== 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 <> 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. + +<> + +== 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 <> and <> messages to construct a map where it looks for frontiers. You can either use costmap published by [[move_base]] (ie. `/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 <>. Mandatory. + + 3.name = ~costmap_updates_topic + 3.default = `costmap_updates` + 3.type = string + 3.desc = Specifies topic of source <>. 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 diff --git a/explore/include/explore/costmap_client.h b/explore/include/explore/costmap_client.h new file mode 100644 index 0000000..06442f6 --- /dev/null +++ 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 +#include +#include +#include +#include +#include +#include + +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 diff --git a/explore/include/explore/costmap_tools.h b/explore/include/explore/costmap_tools.h new file mode 100644 index 0000000..887caa8 --- /dev/null +++ b/explore/include/explore/costmap_tools.h @@ -0,0 +1,133 @@ +#ifndef COSTMAP_TOOLS_H_ +#define COSTMAP_TOOLS_H_ + +#include +#include +#include +#include + +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 nhood4(unsigned int idx, + const costmap_2d::Costmap2D& costmap) +{ + // get 4-connected neighbourhood indexes, check for edge of map + std::vector 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 nhood8(unsigned int idx, + const costmap_2d::Costmap2D& costmap) +{ + // get 8-connected neighbourhood indexes, check for edge of map + std::vector 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 bfs; + std::vector 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 diff --git a/explore/include/explore/explore.h b/explore/include/explore/explore.h new file mode 100644 index 0000000..52cdbcf --- /dev/null +++ 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 +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +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& 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_client_; + frontier_exploration::FrontierSearch search_; + ros::Timer exploring_timer_; + ros::Timer oneshot_; + + std::vector 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 diff --git a/explore/include/explore/frontier_search.h b/explore/include/explore/frontier_search.h new file mode 100644 index 0000000..e0e228e --- /dev/null +++ b/explore/include/explore/frontier_search.h @@ -0,0 +1,88 @@ +#ifndef FRONTIER_SEARCH_H_ +#define FRONTIER_SEARCH_H_ + +#include + +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 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 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& 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& 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 diff --git a/explore/launch/explore.launch b/explore/launch/explore.launch new file mode 100644 index 0000000..6b168d8 --- /dev/null +++ b/explore/launch/explore.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/explore/launch/explore_costmap.launch b/explore/launch/explore_costmap.launch new file mode 100644 index 0000000..da851b2 --- /dev/null +++ b/explore/launch/explore_costmap.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/explore/launch/explore_multi.launch b/explore/launch/explore_multi.launch new file mode 100644 index 0000000..066896a --- /dev/null +++ b/explore/launch/explore_multi.launch @@ -0,0 +1,239 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/explore/package.xml b/explore/package.xml new file mode 100644 index 0000000..f986b24 --- /dev/null +++ b/explore/package.xml @@ -0,0 +1,34 @@ + + + explore_lite + 2.1.1 + + Lightweight frontier-based exploration. + + Jiri Horner + Jiri Horner + BSD + http://wiki.ros.org/explore_lite + + + catkin + + roscpp + std_msgs + move_base_msgs + visualization_msgs + geometry_msgs + map_msgs + nav_msgs + actionlib_msgs + tf + costmap_2d + actionlib + + roslaunch + diff --git a/explore/src/costmap_client.cpp b/explore/src/costmap_client.cpp new file mode 100644 index 0000000..613a124 --- /dev/null +++ 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 + +#include +#include +#include + +namespace explore +{ +// static translation table to speed things up +std::array init_translation_table(); +static const std::array 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( + 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( + costmap_topic, subscription_nh); + updateFullMap(costmap_msg); + + /* subscribe to map updates */ + costmap_updates_sub_ = + subscription_nh.subscribe( + 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 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(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(msg->x); + size_t y0 = static_cast(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 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(msg->data[i]); + costmap_data[idx] = cost_translation_table__[cell_cost]; + ++i; + } + } +} + +geometry_msgs::Pose Costmap2DClient::getRobotPose() const +{ + tf::Stamped global_pose; + global_pose.setIdentity(); + tf::Stamped 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 init_translation_table() +{ + std::array 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(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(-1)] = 255; // UNKNOWN + + return cost_translation_table; +} + +} // namespace explore diff --git a/explore/src/explore.cpp b/explore/src/explore.cpp new file mode 100644 index 0000000..a577fd8 --- /dev/null +++ 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 + +#include + +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("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& 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& 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; +} diff --git a/explore/src/frontier_search.cpp b/explore/src/frontier_search.cpp new file mode 100644 index 0000000..c873a9c --- /dev/null +++ b/explore/src/frontier_search.cpp @@ -0,0 +1,197 @@ +#include + +#include + +#include +#include +#include + +#include + +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 FrontierSearch::searchFrom(geometry_msgs::Point position) +{ + std::vector 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 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 frontier_flag(size_x_ * size_y_, false); + std::vector visited_flag(size_x_ * size_y_, false); + + // initialize breadth first search + std::queue 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& frontier_flag) +{ + // initialize frontier structure + Frontier output; + output.centroid.x = 0; + output.centroid.y = 0; + output.size = 1; + output.min_distance = std::numeric_limits::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 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& 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()); +} +} diff --git a/img/exploration.png b/img/exploration.png new file mode 100644 index 0000000..9eadb97 Binary files /dev/null and b/img/exploration.png differ diff --git a/img/fcpp.png b/img/fcpp.png new file mode 100644 index 0000000..c238a9d Binary files /dev/null and b/img/fcpp.png differ diff --git a/img/result.png b/img/result.png new file mode 100644 index 0000000..53606a5 Binary files /dev/null and b/img/result.png differ diff --git a/img/structure.png b/img/structure.png new file mode 100644 index 0000000..0c66495 Binary files /dev/null and b/img/structure.png differ diff --git a/ros-clean-robot.pdf b/ros-clean-robot.pdf new file mode 100644 index 0000000..1d1f143 Binary files /dev/null and b/ros-clean-robot.pdf differ