├── .gitignore ├── README.md ├── img └── cover.png ├── src ├── CMakeLists.txt ├── auto_nav │ ├── CMakeLists.txt │ ├── config │ │ ├── base_local_planner_params.yaml │ │ ├── cleaning_costmap_params.yaml │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ └── local_costmap_params.yaml │ ├── include │ │ └── path_planning.h │ ├── launch │ │ ├── amcl.launch │ │ ├── auto_slam.launch │ │ ├── clean_work.launch │ │ ├── move.launch │ │ └── save_map.launch │ ├── map │ │ ├── home_map.pgm │ │ └── home_map.yaml │ ├── package.xml │ ├── rviz │ │ ├── clean_work.rviz │ │ └── slam.rviz │ ├── scripts │ │ └── return_to_start.py │ └── src │ │ ├── next_goal.cpp │ │ ├── path_planning.cpp │ │ ├── path_planning_node.cpp │ │ ├── test_goal.cpp │ │ └── test_path.cpp ├── m-explore │ ├── explore │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── doc │ │ │ ├── architecture.dia │ │ │ ├── screenshot.png │ │ │ └── wiki_doc.txt │ │ ├── include │ │ │ └── explore │ │ │ │ ├── costmap_client.h │ │ │ │ ├── costmap_tools.h │ │ │ │ ├── explore.h │ │ │ │ └── frontier_search.h │ │ ├── launch │ │ │ ├── explore.launch │ │ │ └── explore_costmap.launch │ │ ├── package.xml │ │ └── src │ │ │ ├── costmap_client.cpp │ │ │ ├── explore.cpp │ │ │ └── frontier_search.cpp │ └── map_merge │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── doc │ │ ├── architecture.dia │ │ ├── screenshot.jpg │ │ └── wiki_doc.txt │ │ ├── include │ │ ├── combine_grids │ │ │ ├── grid_compositor.h │ │ │ ├── grid_warper.h │ │ │ └── merging_pipeline.h │ │ └── map_merge │ │ │ └── map_merge.h │ │ ├── launch │ │ ├── experiments │ │ │ ├── 3-robots.ttt.launch │ │ │ ├── demo-scene-1.ttt.launch │ │ │ └── demo-scene-orientation-exp.ttt.launch │ │ ├── from_map_server.launch │ │ ├── map_merge.launch │ │ └── map_merge.rviz │ │ ├── package.xml │ │ ├── src │ │ ├── combine_grids │ │ │ ├── estimation_internal.h │ │ │ ├── grid_compositor.cpp │ │ │ ├── grid_warper.cpp │ │ │ └── merging_pipeline.cpp │ │ └── map_merge.cpp │ │ └── test │ │ ├── test_merging_pipeline.cpp │ │ └── testing_helpers.h ├── manual_nav │ ├── CMakeLists.txt │ ├── config │ │ ├── base_local_planner_params.yaml │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ └── local_costmap_params.yaml │ ├── launch │ │ ├── amcl.launch │ │ ├── move.launch │ │ ├── nav.launch │ │ ├── save_map.launch │ │ └── slam.launch │ ├── map │ │ ├── home_map.pgm │ │ └── home_map.yaml │ └── package.xml └── robot │ ├── CMakeLists.txt │ ├── config │ └── robot.rviz │ ├── launch │ └── robot.launch │ ├── package.xml │ ├── urdf │ ├── car.xacro │ ├── car_base.xacro │ ├── car_camera.xacro │ ├── car_laser.xacro │ ├── car_move.xacro │ └── head.xacro │ └── worlds │ ├── home.world │ └── testhome.world └── 基于ROS系统的移动扫地机器人.pdf /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | build/ 3 | devel/ 4 | .catkin_workspace -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Simulation of Sweeping Robot in ROS(Noetic) 2 | - ![cover](./img/cover.png) 3 | - [demo video](https://www.bilibili.com/video/BV1Fe41117gR/?share_source=copy_web&vd_source=e67cc43f2e8443b722a5f50ef79db03e) 4 | 5 | ## English 6 | ### Package Description 7 | - **robot**: Contains the .urdf file for the cleaning robot and the .world file for the environment. 8 | - **m-explore**: Open-source explore_lite algorithm. 9 | - **manual_nav**: Package for manual mapping and manual navigation. 10 | - **auto_nav**: Package for autonomous mapping and autonomous navigation. 11 | - *path_planning.cpp*: Contains the path planning code for the cleaning robot. 12 | - *path_planning_node.cpp*: Node responsible for publishing the planned path. 13 | - *next_goal.cpp*: Node responsible for publishing the next goal point. 14 | 15 | ### Usage Instructions 16 | 1. Compile the project in the root directory: ``catkin_make`` 17 | 2. Refresh the environment variables: ``source devel/setup.bash`` 18 | 3. Depending on the functionality you want to run: 19 | - For autonomous mapping: ``roslaunch auto_nav auto_slam.launch`` After mapping, save the map using ``roslaunch auto_nav save_map.launch`` 20 | - For autonomous navigation: ``roslaunch auto_nav clean_work.launch`` 21 | - For manual mapping: ``roslaunch manual_nav slam.launch`` After mapping, save the map using ``roslaunch manual_nav save_map.launch`` 22 | - For manual navigation: ``roslaunch manual_nav nav.launch`` 23 | 24 | ### Reference Links 25 | - [https://github.com/hrnr/m-explore](https://github.com/hrnr/m-explore) 26 | - [https://github.com/peterWon/CleaningRobot](https://github.com/peterWon/CleaningRobot) 27 | - [https://github.com/mywisdomfly/Clean-robot-turtlebot3](https://github.com/mywisdomfly/Clean-robot-turtlebot3) 28 | 29 | 30 | 31 | ## 中文 32 | ### 功能包说明 33 | - **robot** 包含扫地机器人的.urdf文件和环境的.world文件 34 | - **m-explore** 开源的explore_lite算法 35 | - **manual_nav** 手动建图和手动导航的功能包 36 | - **auto_nav** 自主建图和自主导航的功能包 37 | - *path_planning.cpp* 为扫地路径规划代码 38 | - *path_planning_node.cpp* 发布规划好的路径的节点 39 | - *next_goal.cpp* 发布下一个目标点的节点 40 | 41 | ### 使用说明 42 | 1. 在该工程的根目录编译项目： ``catkin_make`` 43 | 2. 刷新环境变量： ``source devel/setup.bash`` 44 | 3. 根据你想运行的功能： 45 | - 若要自主建图：``roslaunch auto_nav auto_slam.launch`` 建图完成后``roslaunch auto_nav save_map.launch``保存地图 46 | - 若要自主导航：``roslaunch auto_nav clean_work.launch`` 47 | - 若要手动建图：``roslaunch manual_nav slam.launch`` 建图完成后``roslaunch manual_nav save_map.launch``保存地图 48 | - 若要手动导航：``roslaunch manual_nav nav.launch`` 49 | 50 | ### 参考链接 51 | - [https://github.com/hrnr/m-explore](https://github.com/hrnr/m-explore) 52 | - [https://github.com/peterWon/CleaningRobot](https://github.com/peterWon/CleaningRobot) 53 | - [https://github.com/mywisdomfly/Clean-robot-turtlebot3](https://github.com/mywisdomfly/Clean-robot-turtlebot3) 54 | -------------------------------------------------------------------------------- /img/cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/scybd/Sweeping-Robot/3c35cece54b061cbc3345794e1664cd715e1c429/img/cover.png -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/auto_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(auto_nav) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | amcl 12 | actionlib 13 | costmap_2d 14 | geometry_msgs 15 | gmapping 16 | map_server 17 | move_base 18 | move_base_msgs 19 | roscpp 20 | rospy 21 | roslaunch 22 | std_msgs 23 | std_srvs 24 | nav_msgs 25 | tf 26 | ) 27 | find_package(OpenCV REQUIRED) 28 | include_directories(${OpenCV_INCLUDE_DIRS}) 29 | find_package(catkin REQUIRED 30 | COMPONENTS 31 | angles 32 | costmap_2d 33 | geometry_msgs 34 | nav_msgs 35 | roscpp 36 | tf 37 | ) 38 | 39 | ## System dependencies are found with CMake's conventions 40 | # find_package(Boost REQUIRED COMPONENTS system) 41 | 42 | 43 | ## Uncomment this if the package has a setup.py. This macro ensures 44 | ## modules and global scripts declared therein get installed 45 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 46 | # catkin_python_setup() 47 | 48 | ################################################ 49 | ## Declare ROS messages, services and actions ## 50 | ################################################ 51 | 52 | ## To declare and build messages, services or actions from within this 53 | ## package, follow these steps: 54 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 55 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 56 | ## * In the file package.xml: 57 | ## * add a build_depend tag for "message_generation" 58 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 59 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 60 | ## but can be declared for certainty nonetheless: 61 | ## * add a exec_depend tag for "message_runtime" 62 | ## * In this file (CMakeLists.txt): 63 | ## * add "message_generation" and every package in MSG_DEP_SET to 64 | ## find_package(catkin REQUIRED COMPONENTS ...) 65 | ## * add "message_runtime" and every package in MSG_DEP_SET to 66 | ## catkin_package(CATKIN_DEPENDS ...) 67 | ## * uncomment the add_*_files sections below as needed 68 | ## and list every .msg/.srv/.action file to be processed 69 | ## * uncomment the generate_messages entry below 70 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 71 | 72 | ## Generate messages in the 'msg' folder 73 | # add_message_files( 74 | # FILES 75 | # Message1.msg 76 | # Message2.msg 77 | # ) 78 | 79 | ## Generate services in the 'srv' folder 80 | # add_service_files( 81 | # FILES 82 | # Service1.srv 83 | # Service2.srv 84 | # ) 85 | 86 | ## Generate actions in the 'action' folder 87 | # add_action_files( 88 | # FILES 89 | # Action1.action 90 | # Action2.action 91 | # ) 92 | 93 | ## Generate added messages and services with any dependencies listed here 94 | # generate_messages( 95 | # DEPENDENCIES 96 | # std_msgs 97 | # ) 98 | 99 | ################################################ 100 | ## Declare ROS dynamic reconfigure parameters ## 101 | ################################################ 102 | 103 | ## To declare and build dynamic reconfigure parameters within this 104 | ## package, follow these steps: 105 | ## * In the file package.xml: 106 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 107 | ## * In this file (CMakeLists.txt): 108 | ## * add "dynamic_reconfigure" to 109 | ## find_package(catkin REQUIRED COMPONENTS ...) 110 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 111 | ## and list every .cfg file to be processed 112 | 113 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 114 | # generate_dynamic_reconfigure_options( 115 | # cfg/DynReconf1.cfg 116 | # cfg/DynReconf2.cfg 117 | # ) 118 | 119 | ################################### 120 | ## catkin specific configuration ## 121 | ################################### 122 | ## The catkin_package macro generates cmake config files for your package 123 | ## Declare things to be passed to dependent projects 124 | ## INCLUDE_DIRS: uncomment this if your package contains header files 125 | ## LIBRARIES: libraries you create in this project that dependent projects also need 126 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 127 | ## DEPENDS: system dependencies of this project that dependent projects also need 128 | catkin_package( 129 | INCLUDE_DIRS include 130 | LIBRARIES auto_nav 131 | CATKIN_DEPENDS amcl actionlib costmap_2d gmapping map_server move_base roscpp std_msgs geometry_msgs move_base_msgs nav_msgs tf 132 | DEPENDS system_lib 133 | ) 134 | 135 | ########### 136 | ## Build ## 137 | ########### 138 | 139 | ## Specify additional locations of header files 140 | ## Your package locations should be listed before other locations 141 | include_directories( 142 | include 143 | ${catkin_INCLUDE_DIRS} 144 | ) 145 | 146 | ## Declare a C++ library 147 | # add_library(${PROJECT_NAME} 148 | # src/${PROJECT_NAME}/auto_nav.cpp 149 | # ) 150 | 151 | ## Add cmake target dependencies of the library 152 | ## as an example, code may need to be generated before libraries 153 | ## either from message generation or dynamic reconfigure 154 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 155 | 156 | ## Declare a C++ executable 157 | ## With catkin_make all packages are built within a single CMake context 158 | ## The recommended prefix ensures that target names across packages don't collide 159 | add_executable(path_planning src/path_planning.cpp src/path_planning_node.cpp) 160 | add_executable(next_goal src/next_goal.cpp) 161 | add_executable(test_goal src/test_goal.cpp) 162 | add_executable(test_path src/test_path.cpp) 163 | ## Rename C++ executable without prefix 164 | ## The above recommended prefix causes long target names, the following renames the 165 | ## target back to the shorter version for ease of user use 166 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 167 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 168 | 169 | ## Add cmake target dependencies of the executable 170 | ## same as for the library above 171 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 172 | 173 | ## Specify libraries to link a library or executable target against 174 | # target_link_libraries(${PROJECT_NAME}_node 175 | # ${catkin_LIBRARIES} 176 | # ) 177 | target_link_libraries(path_planning ${catkin_LIBRARIES} ${OpenCV_LIBS}) 178 | target_link_libraries(next_goal 179 | ${catkin_LIBRARIES} 180 | ) 181 | target_link_libraries(test_goal 182 | ${catkin_LIBRARIES} 183 | ) 184 | target_link_libraries(test_path 185 | ${catkin_LIBRARIES} 186 | ) 187 | ############# 188 | ## Install ## 189 | ############# 190 | 191 | # all install targets should use catkin DESTINATION variables 192 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 193 | 194 | ## Mark executable scripts (Python etc.) for installation 195 | ## in contrast to setup.py, you can choose the destination 196 | # catkin_install_python(PROGRAMS 197 | # scripts/return_to_start.py 198 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 199 | # ) 200 | 201 | ## Mark executables for installation 202 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 203 | # install(TARGETS ${PROJECT_NAME}_node 204 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 205 | # ) 206 | 207 | ## Mark libraries for installation 208 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 209 | # install(TARGETS ${PROJECT_NAME} 210 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 211 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 212 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 213 | # ) 214 | 215 | ## Mark cpp header files for installation 216 | # install(DIRECTORY include/${PROJECT_NAME}/ 217 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 218 | # FILES_MATCHING PATTERN "*.h" 219 | # PATTERN ".svn" EXCLUDE 220 | # ) 221 | 222 | ## Mark other files for installation (e.g. launch and bag files, etc.) 223 | # install(FILES 224 | # # myfile1 225 | # # myfile2 226 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 227 | # ) 228 | 229 | ############# 230 | ## Testing ## 231 | ############# 232 | 233 | ## Add gtest based cpp test target and link libraries 234 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_auto_nav.cpp) 235 | # if(TARGET ${PROJECT_NAME}-test) 236 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 237 | # endif() 238 | 239 | ## Add folders to be run by python nosetests 240 | # catkin_add_nosetests(test) 241 | -------------------------------------------------------------------------------- /src/auto_nav/config/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | 3 | # Robot Configuration Parameters 4 | max_vel_x: 0.5 # X 方向最大速度 5 | min_vel_x: -0.5 # X 方向最小速度 6 | 7 | max_vel_theta: 1.0 8 | min_vel_theta: -1.0 9 | min_in_place_vel_theta: 1.0 10 | 11 | acc_lim_x: 1.0 # X 加速限制 12 | acc_lim_y: 0.0 # Y 加速限制 13 | acc_lim_theta: 0.6 # 角速度加速限制 14 | 15 | # Goal Tolerance Parameters，目标公差 16 | xy_goal_tolerance: 0.10 17 | yaw_goal_tolerance: 0.05 18 | 19 | # Differential-drive robot configuration 20 | # 是否是全向移动机器人 21 | holonomic_robot: false 22 | 23 | # Forward Simulation Parameters，前进模拟参数 24 | sim_time: 0.8 25 | vx_samples: 18 26 | vtheta_samples: 20 27 | sim_granularity: 0.05 28 | -------------------------------------------------------------------------------- /src/auto_nav/config/cleaning_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | cleaning_costmap: 2 | global_frame: map 3 | robot_base_frame: base_footprint 4 | update_frequency: 1.0 5 | publish_frequency: 1.0 6 | static_map: true 7 | rolling_window: false 8 | resolution: 0.05 9 | transform_tolerance: 1.0 10 | inflation_radius: 0.15 11 | map_type: costmap 12 | 13 | cleaning_plan_nodehandle: 14 | size_of_cell: 3 15 | grid_covered_value: 200 -------------------------------------------------------------------------------- /src/auto_nav/config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | #机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint 2 | robot_radius: 0.12 #圆形 3 | # footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] 4 | 5 | obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图 6 | raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物 7 | 8 | 9 | #膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物 10 | inflation_radius: 0.2 11 | #代价比例系数，越大则代价值越小 12 | cost_scaling_factor: 3.0 13 | 14 | #地图类型 15 | map_type: costmap 16 | #导航包所需要的传感器 17 | observation_sources: scan 18 | #对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。 19 | scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true} 20 | 21 | max_obstacle_height: 0.6 22 | min_obstacle_height: 0.0 23 | 24 | # if set to false,then obstacle layer take unknow space to be free. 25 | obstacle_layer: 26 | track_unknown_space: true 27 | -------------------------------------------------------------------------------- /src/auto_nav/config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map #地图坐标系 3 | robot_base_frame: base_footprint #机器人坐标系 4 | # 以此实现坐标变换 5 | 6 | update_frequency: 1.0 # 代价地图更新频率 7 | publish_frequency: 1.0 # 代价地图的发布频率 8 | transform_tolerance: 0.5 # 等待坐标变换发布信息的超时时间 9 | 10 | static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false. 11 | -------------------------------------------------------------------------------- /src/auto_nav/config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom #里程计坐标系 3 | robot_base_frame: base_footprint #机器人坐标系 4 | 5 | update_frequency: 10.0 #代价地图更新频率 6 | publish_frequency: 10.0 #代价地图的发布频率 7 | transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间 8 | 9 | static_map: false #不需要静态地图，可以提升导航效果 10 | rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化 11 | width: 3 # 局部地图宽度 单位是 m 12 | height: 3 # 局部地图高度 单位是 m 13 | resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致 14 | -------------------------------------------------------------------------------- /src/auto_nav/include/path_planning.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_PLANNING_H 2 | #define PATH_PLANNING_H 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "tf/tf.h" 13 | #include "tf/transform_listener.h" 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using namespace cv; 20 | using namespace std; 21 | 22 | 23 | constexpr double PI =3.14159; 24 | 25 | 26 | struct CellIndex 27 | { 28 | int row; 29 | int col; 30 | double theta; // {0,45,90,135,180,225,270,315} 31 | }; 32 | 33 | 34 | class PathPlanning 35 | { 36 | public: 37 | PathPlanning(costmap_2d::Costmap2DROS *costmap2dRos); 38 | 39 | vector getPathInROS(); // 得到最终在ROS中的路径 40 | 41 | void publishCoveragePath(); // 用于可视化 42 | 43 | int getSizeOfCell() { return this->m_cellSize; } // 获取一个单元格有几个栅格，只能为奇数 44 | 45 | private: 46 | void initMat(); // 初始化m_cellMat和m_neuralMat 47 | void initCoveredGrid(); // 初始化m_coveredPathGrid 48 | void getPathInCV(); // 利用OpenCV的API求出规划路径，之后再转到ros中 49 | bool boundingJudge(int a, int b); // 判断 50 | void publishPlan(const std::vector &path); 51 | 52 | bool m_initialized; // 类初始化成功与否标志位 53 | costmap_2d::Costmap2DROS *m_cosmap2dRos; // A ROS wrapper for a 2D Costmap，处理订阅以PointCloud或LaserScan消息形式提供障碍物观察结果的主题。 54 | costmap_2d::Costmap2D* m_costmap2d; // 原始代价地图的指针 55 | Mat m_srcMap; // 原始代价地图转为Mat 56 | Mat m_cellMat; // 原始栅格地图按m_CELL_SIZE的大小合并后的地图 57 | Mat m_neuralMat; // 该地图用于路径规划 58 | vector m_freeSpaceVec; // m_cellMat中无障碍的地方 59 | nav_msgs::OccupancyGrid m_coveredPathGrid; // 占据栅格地图， 0 表示未占据, 1 表示占据, -1 表示未知 60 | 61 | geometry_msgs::PoseStamped m_initPose; // 初始位姿 62 | vector m_pathVec; // 利用OpenCV的API求得的路径 63 | vector m_pathVecInROS; // m_pathVec转到ros中的路径 64 | 65 | ros::Publisher m_planPub; // 发布规划好的路径，rviz显示 66 | ros::Publisher m_gridPub; // 发布机器人走过的路径，rviz显示 67 | 68 | int m_cellSize; // 新的栅格大小，必须是原来栅格的奇数倍 69 | int m_gridCoveredValue; 70 | }; 71 | 72 | #endif -------------------------------------------------------------------------------- /src/auto_nav/launch/amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /src/auto_nav/launch/auto_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/auto_nav/launch/clean_work.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/auto_nav/launch/move.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /src/auto_nav/launch/save_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/auto_nav/map/home_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/scybd/Sweeping-Robot/3c35cece54b061cbc3345794e1664cd715e1c429/src/auto_nav/map/home_map.pgm -------------------------------------------------------------------------------- /src/auto_nav/map/home_map.yaml: -------------------------------------------------------------------------------- 1 | image: /home/thy/MyROS/SweepingRobot/src/auto_nav/map/home_map.pgm 2 | resolution: 0.050000 3 | origin: [-100.000000, -100.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /src/auto_nav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | auto_nav 4 | 0.0.0 5 | The auto_nav package 6 | 7 | 8 | 9 | 10 | thy 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | amcl 53 | actionlib 54 | costmap_2d 55 | geometry_msgs 56 | gmapping 57 | map_server 58 | move_base 59 | roscpp 60 | rospy 61 | roslaunch 62 | std_msgs 63 | std_srvs 64 | move_base_msgs 65 | nav_msgs 66 | tf 67 | amcl 68 | actionlib 69 | costmap_2d 70 | geometry_msgs 71 | gmapping 72 | map_server 73 | move_base 74 | roscpp 75 | rospy 76 | roslaunch 77 | std_msgs 78 | std_srvs 79 | move_base_msgs 80 | nav_msgs 81 | tf 82 | amcl 83 | actionlib 84 | costmap_2d 85 | geometry_msgs 86 | gmapping 87 | map_server 88 | move_base 89 | roscpp 90 | rospy 91 | roslaunch 92 | std_msgs 93 | std_srvs 94 | move_base_msgs 95 | nav_msgs 96 | tf 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /src/auto_nav/rviz/slam.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.49117645621299744 10 | Tree Height: 719 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: LaserScan 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/RobotModel 56 | Collision Enabled: false 57 | Enabled: true 58 | Links: 59 | All Links Enabled: true 60 | Expand Joint Details: false 61 | Expand Link Details: false 62 | Expand Tree: false 63 | Link Tree Style: Links in Alphabetic Order 64 | back_wheel: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | base_footprint: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | base_link: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | camera: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | front_wheel: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | laser: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | left_wheel: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | right_wheel: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | support: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | Name: RobotModel 110 | Robot Description: robot_description 111 | TF Prefix: "" 112 | Update Interval: 0 113 | Value: true 114 | Visual Enabled: true 115 | - Alpha: 1 116 | Autocompute Intensity Bounds: true 117 | Autocompute Value Bounds: 118 | Max Value: 10 119 | Min Value: -10 120 | Value: true 121 | Axis: Z 122 | Channel Name: intensity 123 | Class: rviz/LaserScan 124 | Color: 255; 255; 255 125 | Color Transformer: Intensity 126 | Decay Time: 0 127 | Enabled: true 128 | Invert Rainbow: false 129 | Max Color: 255; 255; 255 130 | Min Color: 0; 0; 0 131 | Name: LaserScan 132 | Position Transformer: XYZ 133 | Queue Size: 10 134 | Selectable: true 135 | Size (Pixels): 3 136 | Size (m): 0.029999999329447746 137 | Style: Flat Squares 138 | Topic: /scan 139 | Unreliable: false 140 | Use Fixed Frame: true 141 | Use rainbow: true 142 | Value: true 143 | - Alpha: 0.699999988079071 144 | Class: rviz/Map 145 | Color Scheme: map 146 | Draw Behind: false 147 | Enabled: true 148 | Name: Map 149 | Topic: /map 150 | Unreliable: false 151 | Use Timestamp: false 152 | Value: true 153 | - Alpha: 1 154 | Buffer Length: 1 155 | Class: rviz/Path 156 | Color: 32; 74; 135 157 | Enabled: true 158 | Head Diameter: 0.30000001192092896 159 | Head Length: 0.20000000298023224 160 | Length: 0.30000001192092896 161 | Line Style: Billboards 162 | Line Width: 0.029999999329447746 163 | Name: Path 164 | Offset: 165 | X: 0 166 | Y: 0 167 | Z: 0 168 | Pose Color: 255; 85; 255 169 | Pose Style: None 170 | Queue Size: 10 171 | Radius: 0.029999999329447746 172 | Shaft Diameter: 0.10000000149011612 173 | Shaft Length: 0.10000000149011612 174 | Topic: /move_base/TrajectoryPlannerROS/local_plan 175 | Unreliable: false 176 | Value: true 177 | Enabled: true 178 | Global Options: 179 | Background Color: 48; 48; 48 180 | Default Light: true 181 | Fixed Frame: map 182 | Frame Rate: 30 183 | Name: root 184 | Tools: 185 | - Class: rviz/Interact 186 | Hide Inactive Objects: true 187 | - Class: rviz/MoveCamera 188 | - Class: rviz/Select 189 | - Class: rviz/FocusCamera 190 | - Class: rviz/Measure 191 | - Class: rviz/SetInitialPose 192 | Theta std deviation: 0.2617993950843811 193 | Topic: /initialpose 194 | X std deviation: 0.5 195 | Y std deviation: 0.5 196 | - Class: rviz/SetGoal 197 | Topic: /move_base_simple/goal 198 | - Class: rviz/PublishPoint 199 | Single click: true 200 | Topic: /clicked_point 201 | Value: true 202 | Views: 203 | Current: 204 | Class: rviz/Orbit 205 | Distance: 8.86345386505127 206 | Enable Stereo Rendering: 207 | Stereo Eye Separation: 0.05999999865889549 208 | Stereo Focal Distance: 1 209 | Swap Stereo Eyes: false 210 | Value: false 211 | Field of View: 0.7853981852531433 212 | Focal Point: 213 | X: 6.627509593963623 214 | Y: -4.765589237213135 215 | Z: -0.5426267385482788 216 | Focal Shape Fixed Size: true 217 | Focal Shape Size: 0.05000000074505806 218 | Invert Z Axis: false 219 | Name: Current View 220 | Near Clip Distance: 0.009999999776482582 221 | Pitch: 1.5697963237762451 222 | Target Frame: 223 | Yaw: 1.545397400856018 224 | Saved: ~ 225 | Window Geometry: 226 | Displays: 227 | collapsed: false 228 | Height: 1016 229 | Hide Left Dock: false 230 | Hide Right Dock: false 231 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 232 | Selection: 233 | collapsed: false 234 | Time: 235 | collapsed: false 236 | Tool Properties: 237 | collapsed: false 238 | Views: 239 | collapsed: false 240 | Width: 1920 241 | X: 0 242 | Y: 27 243 | -------------------------------------------------------------------------------- /src/auto_nav/scripts/return_to_start.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | import roslaunch 4 | import signal 5 | from move_base_msgs.msg import MoveBaseActionGoal 6 | from geometry_msgs.msg import Point, Quaternion 7 | 8 | 9 | # 向explore服务器发送终止请求 10 | def stop_explore_lite(): 11 | pass 12 | 13 | # 保存地图 14 | def save_map(): 15 | # 获取amcl的launch文件路径 16 | launch_file_path = roslaunch.rlutil.resolve_launch_arguments(['auto_nav', 'save_map.launch']) 17 | # 创建Launch对象 18 | launch = roslaunch.parent.ROSLaunchParent(rospy.get_param('/run_id'), launch_file_path) 19 | # 启动launch文件 20 | launch.start() 21 | try: 22 | # 等待节点结束 23 | launch.spin() 24 | finally: 25 | # 关闭launch 26 | launch.shutdown() 27 | 28 | # 启动amcl返航 29 | def run_amcl(): 30 | # 获取amcl的launch文件路径 31 | launch_file_path = roslaunch.rlutil.resolve_launch_arguments(['auto_nav', 'amcl.launch']) 32 | # 创建Launch对象 33 | launch = roslaunch.parent.ROSLaunchParent(rospy.get_param('/run_id'), launch_file_path) 34 | # 启动launch文件 35 | launch.start() 36 | try: 37 | # 等待节点结束 38 | launch.spin() 39 | finally: 40 | # 关闭launch 41 | launch.shutdown() 42 | 43 | 44 | class GoalMonitorNode: 45 | def __init__(self): 46 | rospy.init_node('return_to_start', anonymous=True) 47 | 48 | # Handle SIGINT and SIGTERM signals 49 | signal.signal(signal.SIGINT, signal.SIG_DFL) 50 | signal.signal(signal.SIGTERM, signal.SIG_DFL) 51 | 52 | # 订阅/move_base/goal话题 53 | rospy.Subscriber('/move_base/goal', MoveBaseActionGoal, self.goal_status_callback) 54 | 55 | # 定义发布器 56 | self.goal_publisher = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=1) 57 | 58 | # 设置计时器，每秒触发一次回调函数 59 | self.timer = rospy.Timer(rospy.Duration(1), self.timer_callback) 60 | 61 | # 记录上次收到消息的时间 62 | self.last_goal_time = rospy.Time.now() 63 | 64 | rospy.spin() 65 | 66 | def signal_handler(self, signum, frame): 67 | rospy.loginfo("Received signal {}, shutting down...".format(signum)) 68 | # Implement cleanup or additional shutdown logic if needed 69 | rospy.signal_shutdown("Received signal, shutting down") 70 | 71 | 72 | def goal_status_callback(self, msg): 73 | # 更新上次收到消息的时间 74 | self.last_goal_time = rospy.Time.now() 75 | 76 | def timer_callback(self, event): 77 | # 检查是否超过20秒没有收到消息 78 | if rospy.Time.now() - self.last_goal_time > rospy.Duration(25): 79 | rospy.loginfo("No goal received in the last 20 seconds. Sending a new goal.") 80 | 81 | stop_explore_lite() 82 | rospy.loginfo("1") 83 | rospy.sleep(1.0) # 确保停止 84 | rospy.loginfo("2") 85 | save_map() 86 | rospy.loginfo("3") 87 | #run_amcl() 88 | 89 | # 创建返航的目标消息 90 | new_goal = MoveBaseActionGoal() 91 | new_goal.goal_id.stamp = rospy.Time.now() 92 | new_goal.goal.target_pose.header.frame_id = 'map' 93 | new_goal.goal.target_pose.pose.position = Point(x=-8.0, y=0.0, z=0.0) 94 | new_goal.goal.target_pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) 95 | 96 | # 发布新的目标消息到/move_base/goal话题 97 | self.goal_publisher.publish(new_goal) 98 | 99 | if __name__ == '__main__': 100 | try: 101 | # Set up signal handling in the main thread 102 | signal.signal(signal.SIGINT, signal.SIG_DFL) 103 | signal.signal(signal.SIGTERM, signal.SIG_DFL) 104 | 105 | node = GoalMonitorNode() 106 | except rospy.ROSInterruptException: 107 | pass 108 | 109 | -------------------------------------------------------------------------------- /src/auto_nav/src/next_goal.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "nav_msgs/Odometry.h" 6 | #include "nav_msgs/Path.h" 7 | #include "geometry_msgs/PoseStamped.h" 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | using namespace tf; 14 | 15 | float x_current; 16 | float y_current; 17 | 18 | float normeNextGoal; // 发送下一个点的阈值 19 | 20 | // 欧拉角到四元数的转换，pitch和yaw为0 21 | void eulerAngles2Quaternion(float yaw, float& w, float& x, float& y, float& z) 22 | { 23 | float cy = cos(yaw * 0.5); 24 | float sy = sin(yaw * 0.5); 25 | float cr = cos(0 * 0.5); 26 | float sr = sin(0 * 0.5); 27 | float cp = cos(0 * 0.5); 28 | float sp = sin(0 * 0.5); 29 | 30 | w = cy * cr * cp + sy * sr * sp; 31 | x = cy * sr * cp - sy * cr * sp; 32 | y = cy * cr * sp + sy * sr * cp; 33 | z = sy * cr * cp - cy * sr * sp; 34 | } 35 | 36 | class PathPlanner 37 | { 38 | public: 39 | struct Goal 40 | { 41 | float x; 42 | float y; 43 | bool visited; 44 | }; 45 | 46 | vector Path; 47 | 48 | void addGoal(float X, float Y, bool visit); 49 | }; 50 | 51 | 52 | void PathPlanner::addGoal(float X, float Y, bool visit) 53 | { 54 | PathPlanner::Goal newGoal; 55 | newGoal.x = X; 56 | newGoal.y = Y; 57 | newGoal.visited = visit; 58 | Path.push_back(newGoal); 59 | } 60 | 61 | 62 | PathPlanner pathPlanner; 63 | nav_msgs::Path passedPath; 64 | ros::Publisher pubPassedPath; 65 | void pose_callback(const nav_msgs::Odometry &poses) 66 | { // 里程计回调函数,用来计算当前机器人位置与前面目标点的距离,判断是否要发新的目标点 67 | x_current = poses.pose.pose.position.x; 68 | y_current = poses.pose.pose.position.y; 69 | passedPath.header = poses.header; 70 | geometry_msgs::PoseStamped p; 71 | p.header = poses.header; 72 | p.pose = poses.pose.pose; 73 | passedPath.poses.emplace_back(p); 74 | pubPassedPath.publish(passedPath); 75 | } 76 | 77 | int taille_last_path = 0; 78 | bool new_path = false; 79 | 80 | 81 | // 接受规划的路径 82 | void path_callback(const nav_msgs::Path &path) 83 | { 84 | // 注意为了rviz显示方便 路径一直在发,但是这里只用接受一次就好,当规划的路径发生变化时候再重新装载,目前没有写在运动过程中重新规划路径的代码，不会进第二次 85 | if ((pathPlanner.Path.size() == 0) || (path.poses.size() != taille_last_path)) 86 | { 87 | pathPlanner.Path.clear(); 88 | new_path = true; 89 | for (int i = 0; i < path.poses.size(); i++) 90 | { 91 | pathPlanner.addGoal(path.poses[i].pose.position.x, path.poses[i].pose.position.y, false); 92 | } 93 | taille_last_path = path.poses.size(); 94 | } 95 | } 96 | 97 | 98 | int main(int argc, char *argv[]) 99 | { 100 | srand(time(0)); 101 | ros::init(argc, argv, "next_goal"); 102 | ros::NodeHandle next_goal; 103 | ros::Subscriber sub1 = next_goal.subscribe("/odom", 1000, pose_callback); 104 | ros::Subscriber sub2 = next_goal.subscribe("/plan_path", 1000, path_callback); 105 | 106 | ros::Publisher pub1 = next_goal.advertise("move_base_simple/goal", 1000); 107 | pubPassedPath = next_goal.advertise("passedPath", 1000); 108 | 109 | ros::Rate loop_rate(10); 110 | 111 | geometry_msgs::PoseStamped goal_msgs; 112 | int count = 0; 113 | double angle; 114 | bool goal_reached = false; 115 | // 获取判断到达当前目标的阈值 116 | if (!next_goal.getParam("/NextGoal/tolerance_goal", normeNextGoal)) 117 | { 118 | ROS_ERROR("Please set your tolerance_goal"); 119 | return 0; 120 | } 121 | 122 | while (ros::ok()) 123 | { 124 | ros::spinOnce(); 125 | if (new_path) 126 | { 127 | count = 0; 128 | new_path = false; 129 | } 130 | 131 | if (!pathPlanner.Path.empty()) 132 | { 133 | // 当前距离达到了 134 | if (sqrt(pow(x_current - pathPlanner.Path[count].x, 2) + pow(y_current - pathPlanner.Path[count].y, 2)) <= normeNextGoal) 135 | { 136 | count++; 137 | goal_reached = false; 138 | } 139 | if (goal_reached == false) 140 | { 141 | goal_msgs.header.frame_id = "odom"; 142 | goal_msgs.header.stamp = ros::Time::now(); 143 | goal_msgs.pose.position.x = pathPlanner.Path[count].x; 144 | goal_msgs.pose.position.y = pathPlanner.Path[count].y; 145 | goal_msgs.pose.position.z = 0; 146 | if (count < pathPlanner.Path.size()) 147 | angle = atan2(pathPlanner.Path[count + 1].y - pathPlanner.Path[count].y, pathPlanner.Path[count + 1].x - pathPlanner.Path[count].x); 148 | else 149 | angle = atan2(pathPlanner.Path[0].y - pathPlanner.Path[count].y, pathPlanner.Path[0].x - pathPlanner.Path[count].x); 150 | float w,x,y,z; 151 | eulerAngles2Quaternion(angle,w,x,y,z); 152 | goal_msgs.pose.orientation.w = w; 153 | goal_msgs.pose.orientation.x = x; 154 | goal_msgs.pose.orientation.y = y; 155 | goal_msgs.pose.orientation.z = z; 156 | 157 | goal_reached = true; 158 | pub1.publish(goal_msgs); 159 | } 160 | 161 | } 162 | 163 | loop_rate.sleep(); 164 | } 165 | 166 | return 0; 167 | } 168 | -------------------------------------------------------------------------------- /src/auto_nav/src/path_planning_node.cpp: -------------------------------------------------------------------------------- 1 | #include "path_planning.h" 2 | #include 3 | #include 4 | 5 | 6 | int main(int argc, char** argv) { 7 | ros::init(argc, argv, "path_planning"); 8 | 9 | tf2_ros::Buffer tf; 10 | tf2_ros::TransformListener tf2_listener(tf); 11 | costmap_2d::Costmap2DROS lcr("cleaning_costmap", tf); 12 | 13 | ros::Duration(5).sleep(); 14 | PathPlanning clr(&lcr); 15 | clr.getPathInROS(); // 根据栅格地图做路径规划 16 | ros::Rate r(1); 17 | while(ros::ok()){ // 循环发布规划好的路径 18 | clr.publishCoveragePath(); 19 | ros::spinOnce(); 20 | r.sleep(); 21 | } 22 | 23 | ros::shutdown(); 24 | return 0; 25 | } 26 | 27 | -------------------------------------------------------------------------------- /src/auto_nav/src/test_goal.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | typedef actionlib::SimpleActionClient MoveBaseClient; 6 | 7 | int main(int argc, char** argv){ 8 | // 初始化 ROS 节点 9 | ros::init(argc, argv, "test_goal"); 10 | 11 | // 告诉 action 客户端，我们想要连接到 move_base 服务器 12 | MoveBaseClient ac("move_base", true); 13 | 14 | // 等待 move_base action 服务器启动 15 | ROS_INFO("Waiting for the move_base action server"); 16 | ac.waitForServer(); 17 | 18 | // 设置目标 19 | move_base_msgs::MoveBaseGoal goal; 20 | goal.target_pose.header.frame_id = "map"; 21 | goal.target_pose.header.stamp = ros::Time::now(); 22 | 23 | // 定义目标位置和方向 24 | goal.target_pose.pose.position.x = 0.0; // 示例坐标 25 | goal.target_pose.pose.position.y = 1.0; 26 | goal.target_pose.pose.orientation.w = 1.0; 27 | 28 | ROS_INFO("Sending goal"); 29 | ac.sendGoal(goal); 30 | 31 | // 等待目标完成 32 | ac.waitForResult(); 33 | 34 | if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 35 | ROS_INFO("The base moved to the goal position"); 36 | else 37 | ROS_INFO("The base failed to move to the goal position"); 38 | 39 | return 0; 40 | } 41 | -------------------------------------------------------------------------------- /src/auto_nav/src/test_path.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char** argv){ 6 | // 初始化 ROS 节点 7 | ros::init(argc, argv, "test_path"); 8 | ros::NodeHandle n; 9 | 10 | // 创建一个路径发布者 11 | ros::Publisher path_pub = n.advertise("plan_path", 10); 12 | 13 | // 创建路径消息 14 | nav_msgs::Path path; 15 | path.header.frame_id = "map"; // 或者使用你使用的坐标系 16 | path.header.stamp = ros::Time::now(); 17 | 18 | // 添加路径点 19 | for (int i = 0; i < 10; ++i) { 20 | geometry_msgs::PoseStamped pose; 21 | pose.header.frame_id = "map"; 22 | pose.header.stamp = ros::Time::now(); 23 | 24 | pose.pose.position.x = i; // 示例坐标 25 | pose.pose.position.y = i; 26 | pose.pose.orientation.w = 1.0; 27 | 28 | path.poses.push_back(pose); 29 | } 30 | 31 | // 发布路径 32 | while (ros::ok()) { 33 | path_pub.publish(path); 34 | ros::spinOnce(); 35 | } 36 | 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /src/m-explore/explore/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package explore_lite 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.1.4 (2021-01-07) 6 | ------------------ 7 | * use C++14 8 | * Contributors: Jiri Horner 9 | 10 | 2.1.3 (2021-01-03) 11 | ------------------ 12 | * add missing dependencies to catkin_package calls 13 | * Contributors: Jiri Horner 14 | 15 | 2.1.2 (2021-01-02) 16 | ------------------ 17 | * support for ROS Melodic 18 | * Contributors: Jiri Horner 19 | 20 | 2.1.1 (2017-12-16) 21 | ------------------ 22 | * explore: fix min_frontier_size bug 23 | * min_frontier_size parameter was not used to at all 24 | * adjust min_frontier size filtering according to map resolution 25 | * fix bugs in CMakeLists.txt 26 | * install nodes in packages, so they get shipped in debian packages. fixes `#11 `_ 27 | * explore: update documentation 28 | * Contributors: Jiri Horner 29 | 30 | 2.1.0 (2017-10-30) 31 | ------------------ 32 | * explore: get rid of boost 33 | * explore: rework launchfiles 34 | * new visualisation of frontiers 35 | * remove navfn library from dependencies 36 | * use frontier seach algorithm from Paul Bovbel 37 | * much better than previous version of search 38 | * https://github.com/paulbovbel/frontier_exploration.git 39 | * fix deadlock in explore 40 | * reworked expore to use timer instead of separate thread for replanning 41 | * fix deadlock occuring between makePlan and goal reached callback 42 | * Contributors: Jiri Horner 43 | 44 | 2.0.0 (2017-03-26) 45 | ------------------ 46 | * explore: migrate to package format 2 47 | * explore: remove internal version of navfn_ros 48 | * my changes are included in the ros since kinetic 49 | * Contributors: Jiri Horner 50 | 51 | 1.0.1 (2017-03-25) 52 | ------------------ 53 | * update documentation 54 | * Contributors: Jiri Horner 55 | 56 | 1.0.0 (2016-05-11) 57 | ------------------ 58 | * initial release 59 | * Contributors: Jiri Horner, duhadway-bosch, pitzer 60 | -------------------------------------------------------------------------------- /src/m-explore/explore/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(explore_lite) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED COMPONENTS 6 | actionlib 7 | actionlib_msgs 8 | costmap_2d 9 | geometry_msgs 10 | map_msgs 11 | move_base_msgs 12 | nav_msgs 13 | roscpp 14 | std_msgs 15 | tf 16 | visualization_msgs 17 | ) 18 | 19 | ################################### 20 | ## catkin specific configuration ## 21 | ################################### 22 | catkin_package( 23 | CATKIN_DEPENDS 24 | actionlib 25 | actionlib_msgs 26 | costmap_2d 27 | geometry_msgs 28 | map_msgs 29 | move_base_msgs 30 | nav_msgs 31 | roscpp 32 | std_msgs 33 | tf 34 | visualization_msgs 35 | ) 36 | 37 | ########### 38 | ## Build ## 39 | ########### 40 | set(CMAKE_CXX_STANDARD 14) 41 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 42 | set(CMAKE_CXX_EXTENSIONS OFF) 43 | 44 | ## Specify additional locations of header files 45 | include_directories( 46 | ${catkin_INCLUDE_DIRS} 47 | include 48 | ) 49 | 50 | add_executable(explore 51 | src/costmap_client.cpp 52 | src/explore.cpp 53 | src/frontier_search.cpp 54 | ) 55 | add_dependencies(explore ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 56 | target_link_libraries(explore ${catkin_LIBRARIES}) 57 | 58 | ############# 59 | ## Install ## 60 | ############# 61 | 62 | # install nodes 63 | install(TARGETS explore 64 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 65 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 66 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 67 | ) 68 | 69 | # install roslaunch files 70 | install(DIRECTORY launch/ 71 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 72 | ) 73 | 74 | ############# 75 | ## Testing ## 76 | ############# 77 | if(CATKIN_ENABLE_TESTING) 78 | find_package(roslaunch REQUIRED) 79 | 80 | # test all launch files 81 | roslaunch_add_file_check(launch) 82 | endif() 83 | -------------------------------------------------------------------------------- /src/m-explore/explore/doc/architecture.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/scybd/Sweeping-Robot/3c35cece54b061cbc3345794e1664cd715e1c429/src/m-explore/explore/doc/architecture.dia -------------------------------------------------------------------------------- /src/m-explore/explore/doc/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/scybd/Sweeping-Robot/3c35cece54b061cbc3345794e1664cd715e1c429/src/m-explore/explore/doc/screenshot.png -------------------------------------------------------------------------------- /src/m-explore/explore/doc/wiki_doc.txt: -------------------------------------------------------------------------------- 1 | <> 2 | 3 | <> 4 | 5 | <> 6 | 7 | == Overview == 8 | 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]]. 9 | 10 | {{attachment:screenshot.png||width="755px"}} 11 | 12 | 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. 13 | 14 | Node can do frontier filtering and can operate even on non-inflated maps. Goal blacklisting allows to deal with places inaccessible for robot. 15 | 16 | <> 17 | 18 | == Architecture == 19 | {{{explore_lite}}} uses [[move_base]] for navigation. You need to run properly configured [[move_base]] node. 20 | 21 | {{attachment:architecture.svg||width="755px"}} 22 | 23 | {{{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). 24 | 25 | 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. 26 | 27 | == Setup == 28 | 29 | 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. 30 | 31 | 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}}}. 32 | 33 | If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`. 34 | 35 | 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. 36 | 37 | == ROS API == 38 | {{{ 39 | #!clearsilver CS/NodeAPI 40 | 41 | name = explore 42 | desc = Provides exploration services offered by this package. Exploration will start immediately after node initialization. 43 | 44 | pub { 45 | 0.name = ~frontiers 46 | 0.type = visualization_msgs/MarkerArray 47 | 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). 48 | } 49 | sub { 50 | 0.name = costmap 51 | 0.type = nav_msgs/OccupancyGrid 52 | 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`. 53 | 54 | 1.name = costmap_updates 55 | 1.type = map_msgs/OccupancyGridUpdate 56 | 1.desc = Incremental updates on costmap. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic. 57 | } 58 | 59 | param { 60 | 0.name = ~robot_base_frame 61 | 0.default = `base_link` 62 | 0.type = string 63 | 0.desc = The name of the base frame of the robot. This is used for determining robot position on map. Mandatory. 64 | 65 | 1.name = ~costmap_topic 66 | 1.default = `costmap` 67 | 1.type = string 68 | 1.desc = Specifies topic of source <>. Mandatory. 69 | 70 | 3.name = ~costmap_updates_topic 71 | 3.default = `costmap_updates` 72 | 3.type = string 73 | 3.desc = Specifies topic of source <>. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic. 74 | 75 | 4.name = ~visualize 76 | 4.default = `false` 77 | 4.type = bool 78 | 4.desc = Specifies whether or not publish visualized frontiers. 79 | 80 | 6.name = ~planner_frequency 81 | 6.default = `1.0` 82 | 6.type = double 83 | 6.desc = Rate in Hz at which new frontiers will computed and goal reconsidered. 84 | 85 | 7.name = ~progress_timeout 86 | 7.default = `30.0` 87 | 7.type = double 88 | 7.desc = Time in seconds. When robot do not make any progress for `progress_timeout`, current goal will be abandoned. 89 | 90 | 8.name = ~potential_scale 91 | 8.default = `1e-3` 92 | 8.type = double 93 | 8.desc = Used for weighting frontiers. This multiplicative parameter affects frontier potential component of the frontier weight (distance to frontier). 94 | 95 | 9.name = ~orientation_scale 96 | 9.default = `0` 97 | 9.type = double 98 | 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. 99 | 100 | 10.name = ~gain_scale 101 | 10.default = `1.0` 102 | 10.type = double 103 | 10.desc = Used for weighting frontiers. This multiplicative parameter affects frontier gain component of the frontier weight (frontier size). 104 | 105 | 11.name = ~transform_tolerance 106 | 11.default = `0.3` 107 | 11.type = double 108 | 11.desc = Transform tolerance to use when transforming robot pose. 109 | 110 | 12.name = ~min_frontier_size 111 | 12.default = `0.5` 112 | 12.type = double 113 | 12.desc = Minimum size of the frontier to consider the frontier as the exploration goal. In meters. 114 | } 115 | 116 | req_tf { 117 | 0.from = global_frame 118 | 0.to = robot_base_frame 119 | 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. 120 | } 121 | 122 | act_called { 123 | 0.name = move_base 124 | 0.type = move_base_msgs/MoveBaseAction 125 | 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. 126 | } 127 | }}} 128 | 129 | == Acknowledgements == 130 | 131 | This package was developed as part of my bachelor thesis at [[http://www.mff.cuni.cz/to.en/|Charles University]] in Prague. 132 | 133 | {{{ 134 | @masterthesis{Hörner2016, 135 | author = {Jiří Hörner}, 136 | title = {Map-merging for multi-robot system}, 137 | address = {Prague}, 138 | year = {2016}, 139 | school = {Charles University in Prague, Faculty of Mathematics and Physics}, 140 | type = {Bachelor's thesis}, 141 | URL = {https://is.cuni.cz/webapps/zzp/detail/174125/}, 142 | } 143 | }}} 144 | 145 | 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. 146 | 147 | ## AUTOGENERATED DON'T DELETE 148 | ## CategoryPackage 149 | -------------------------------------------------------------------------------- /src/m-explore/explore/include/explore/costmap_client.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #ifndef COSTMAP_CLIENT_ 38 | #define COSTMAP_CLIENT_ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace explore 49 | { 50 | class Costmap2DClient 51 | { 52 | public: 53 | /** 54 | * @brief Contructs client and start listening 55 | * @details Constructor will block until first map update is received and 56 | * map is ready to use, also will block before trasformation 57 | * robot_base_frame <-> global_frame is available. 58 | * 59 | * @param param_nh node hadle to retrieve parameters from 60 | * @param subscription_nh node hadle where topics will be subscribed 61 | * @param tf_listener Will be used for transformation of robot pose. 62 | */ 63 | Costmap2DClient(ros::NodeHandle& param_nh, ros::NodeHandle& subscription_nh, 64 | const tf::TransformListener* tf_listener); 65 | /** 66 | * @brief Get the pose of the robot in the global frame of the costmap 67 | * @return pose of the robot in the global frame of the costmap 68 | */ 69 | geometry_msgs::Pose getRobotPose() const; 70 | 71 | /** 72 | * @brief Return a pointer to the "master" costmap which receives updates from 73 | * all the layers. 74 | * 75 | * This pointer will stay the same for the lifetime of Costmap2DClient object. 76 | */ 77 | costmap_2d::Costmap2D* getCostmap() 78 | { 79 | return &costmap_; 80 | } 81 | 82 | /** 83 | * @brief Return a pointer to the "master" costmap which receives updates from 84 | * all the layers. 85 | * 86 | * This pointer will stay the same for the lifetime of Costmap2DClient object. 87 | */ 88 | const costmap_2d::Costmap2D* getCostmap() const 89 | { 90 | return &costmap_; 91 | } 92 | 93 | /** 94 | * @brief Returns the global frame of the costmap 95 | * @return The global frame of the costmap 96 | */ 97 | const std::string& getGlobalFrameID() const 98 | { 99 | return global_frame_; 100 | } 101 | 102 | /** 103 | * @brief Returns the local frame of the costmap 104 | * @return The local frame of the costmap 105 | */ 106 | const std::string& getBaseFrameID() const 107 | { 108 | return robot_base_frame_; 109 | } 110 | 111 | protected: 112 | void updateFullMap(const nav_msgs::OccupancyGrid::ConstPtr& msg); 113 | void updatePartialMap(const map_msgs::OccupancyGridUpdate::ConstPtr& msg); 114 | 115 | costmap_2d::Costmap2D costmap_; 116 | 117 | const tf::TransformListener* const tf_; ///< @brief Used for transforming 118 | /// point clouds 119 | std::string global_frame_; ///< @brief The global frame for the costmap 120 | std::string robot_base_frame_; ///< @brief The frame_id of the robot base 121 | double transform_tolerance_; ///< timeout before transform errors 122 | 123 | private: 124 | // will be unsubscribed at destruction 125 | ros::Subscriber costmap_sub_; 126 | ros::Subscriber costmap_updates_sub_; 127 | }; 128 | 129 | } // namespace explore 130 | 131 | #endif 132 | -------------------------------------------------------------------------------- /src/m-explore/explore/include/explore/costmap_tools.h: -------------------------------------------------------------------------------- 1 | #ifndef COSTMAP_TOOLS_H_ 2 | #define COSTMAP_TOOLS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace frontier_exploration 10 | { 11 | /** 12 | * @brief Determine 4-connected neighbourhood of an input cell, checking for map 13 | * edges 14 | * @param idx input cell index 15 | * @param costmap Reference to map data 16 | * @return neighbour cell indexes 17 | */ 18 | std::vector nhood4(unsigned int idx, 19 | const costmap_2d::Costmap2D& costmap) 20 | { 21 | // get 4-connected neighbourhood indexes, check for edge of map 22 | std::vector out; 23 | 24 | unsigned int size_x_ = costmap.getSizeInCellsX(), 25 | size_y_ = costmap.getSizeInCellsY(); 26 | 27 | if (idx > size_x_ * size_y_ - 1) { 28 | ROS_WARN("Evaluating nhood for offmap point"); 29 | return out; 30 | } 31 | 32 | if (idx % size_x_ > 0) { 33 | out.push_back(idx - 1); 34 | } 35 | if (idx % size_x_ < size_x_ - 1) { 36 | out.push_back(idx + 1); 37 | } 38 | if (idx >= size_x_) { 39 | out.push_back(idx - size_x_); 40 | } 41 | if (idx < size_x_ * (size_y_ - 1)) { 42 | out.push_back(idx + size_x_); 43 | } 44 | return out; 45 | } 46 | 47 | /** 48 | * @brief Determine 8-connected neighbourhood of an input cell, checking for map 49 | * edges 50 | * @param idx input cell index 51 | * @param costmap Reference to map data 52 | * @return neighbour cell indexes 53 | */ 54 | std::vector nhood8(unsigned int idx, 55 | const costmap_2d::Costmap2D& costmap) 56 | { 57 | // get 8-connected neighbourhood indexes, check for edge of map 58 | std::vector out = nhood4(idx, costmap); 59 | 60 | unsigned int size_x_ = costmap.getSizeInCellsX(), 61 | size_y_ = costmap.getSizeInCellsY(); 62 | 63 | if (idx > size_x_ * size_y_ - 1) { 64 | return out; 65 | } 66 | 67 | if (idx % size_x_ > 0 && idx >= size_x_) { 68 | out.push_back(idx - 1 - size_x_); 69 | } 70 | if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) { 71 | out.push_back(idx - 1 + size_x_); 72 | } 73 | if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) { 74 | out.push_back(idx + 1 - size_x_); 75 | } 76 | if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) { 77 | out.push_back(idx + 1 + size_x_); 78 | } 79 | 80 | return out; 81 | } 82 | 83 | /** 84 | * @brief Find nearest cell of a specified value 85 | * @param result Index of located cell 86 | * @param start Index initial cell to search from 87 | * @param val Specified value to search for 88 | * @param costmap Reference to map data 89 | * @return True if a cell with the requested value was found 90 | */ 91 | bool nearestCell(unsigned int& result, unsigned int start, unsigned char val, 92 | const costmap_2d::Costmap2D& costmap) 93 | { 94 | const unsigned char* map = costmap.getCharMap(); 95 | const unsigned int size_x = costmap.getSizeInCellsX(), 96 | size_y = costmap.getSizeInCellsY(); 97 | 98 | if (start >= size_x * size_y) { 99 | return false; 100 | } 101 | 102 | // initialize breadth first search 103 | std::queue bfs; 104 | std::vector visited_flag(size_x * size_y, false); 105 | 106 | // push initial cell 107 | bfs.push(start); 108 | visited_flag[start] = true; 109 | 110 | // search for neighbouring cell matching value 111 | while (!bfs.empty()) { 112 | unsigned int idx = bfs.front(); 113 | bfs.pop(); 114 | 115 | // return if cell of correct value is found 116 | if (map[idx] == val) { 117 | result = idx; 118 | return true; 119 | } 120 | 121 | // iterate over all adjacent unvisited cells 122 | for (unsigned nbr : nhood8(idx, costmap)) { 123 | if (!visited_flag[nbr]) { 124 | bfs.push(nbr); 125 | visited_flag[nbr] = true; 126 | } 127 | } 128 | } 129 | 130 | return false; 131 | } 132 | } 133 | #endif 134 | -------------------------------------------------------------------------------- /src/m-explore/explore/include/explore/explore.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Robert Bosch LLC. 6 | * Copyright (c) 2015-2016, Jiri Horner. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | #ifndef NAV_EXPLORE_H_ 38 | #define NAV_EXPLORE_H_ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | #include 53 | 54 | namespace explore 55 | { 56 | /** 57 | * @class Explore 58 | * @brief A class adhering to the robot_actions::Action interface that moves the 59 | * robot base to explore its environment. 60 | */ 61 | class Explore 62 | { 63 | public: 64 | Explore(); 65 | ~Explore(); 66 | 67 | void start(); 68 | void stop(); 69 | 70 | private: 71 | /** 72 | * @brief Make a global plan 73 | */ 74 | void makePlan(); 75 | 76 | /** 77 | * @brief Publish a frontiers as markers 78 | */ 79 | void visualizeFrontiers( 80 | const std::vector& frontiers); 81 | 82 | void reachedGoal(const actionlib::SimpleClientGoalState& status, 83 | const move_base_msgs::MoveBaseResultConstPtr& result, 84 | const geometry_msgs::Point& frontier_goal); 85 | 86 | bool goalOnBlacklist(const geometry_msgs::Point& goal); 87 | 88 | ros::NodeHandle private_nh_; 89 | ros::NodeHandle relative_nh_; 90 | ros::Publisher marker_array_publisher_; 91 | tf::TransformListener tf_listener_; 92 | 93 | Costmap2DClient costmap_client_; 94 | actionlib::SimpleActionClient 95 | move_base_client_; 96 | frontier_exploration::FrontierSearch search_; 97 | ros::Timer exploring_timer_; 98 | ros::Timer oneshot_; 99 | 100 | std::vector frontier_blacklist_; 101 | geometry_msgs::Point prev_goal_; 102 | double prev_distance_; 103 | ros::Time last_progress_; 104 | size_t last_markers_count_; 105 | 106 | // parameters 107 | double planner_frequency_; 108 | double potential_scale_, orientation_scale_, gain_scale_; 109 | ros::Duration progress_timeout_; 110 | bool visualize_; 111 | }; 112 | } 113 | 114 | #endif 115 | -------------------------------------------------------------------------------- /src/m-explore/explore/include/explore/frontier_search.h: -------------------------------------------------------------------------------- 1 | #ifndef FRONTIER_SEARCH_H_ 2 | #define FRONTIER_SEARCH_H_ 3 | 4 | #include 5 | 6 | namespace frontier_exploration 7 | { 8 | /** 9 | * @brief Represents a frontier 10 | * 11 | */ 12 | struct Frontier { 13 | std::uint32_t size; 14 | double min_distance; 15 | double cost; 16 | geometry_msgs::Point initial; 17 | geometry_msgs::Point centroid; 18 | geometry_msgs::Point middle; 19 | std::vector points; 20 | }; 21 | 22 | /** 23 | * @brief Thread-safe implementation of a frontier-search task for an input 24 | * costmap. 25 | */ 26 | class FrontierSearch 27 | { 28 | public: 29 | FrontierSearch() 30 | { 31 | } 32 | 33 | /** 34 | * @brief Constructor for search task 35 | * @param costmap Reference to costmap data to search. 36 | */ 37 | FrontierSearch(costmap_2d::Costmap2D* costmap, double potential_scale, 38 | double gain_scale, double min_frontier_size); 39 | 40 | /** 41 | * @brief Runs search implementation, outward from the start position 42 | * @param position Initial position to search from 43 | * @return List of frontiers, if any 44 | */ 45 | std::vector searchFrom(geometry_msgs::Point position); 46 | 47 | protected: 48 | /** 49 | * @brief Starting from an initial cell, build a frontier from valid adjacent 50 | * cells 51 | * @param initial_cell Index of cell to start frontier building 52 | * @param reference Reference index to calculate position from 53 | * @param frontier_flag Flag vector indicating which cells are already marked 54 | * as frontiers 55 | * @return new frontier 56 | */ 57 | Frontier buildNewFrontier(unsigned int initial_cell, unsigned int reference, 58 | std::vector& frontier_flag); 59 | 60 | /** 61 | * @brief isNewFrontierCell Evaluate if candidate cell is a valid candidate 62 | * for a new frontier. 63 | * @param idx Index of candidate cell 64 | * @param frontier_flag Flag vector indicating which cells are already marked 65 | * as frontiers 66 | * @return true if the cell is frontier cell 67 | */ 68 | bool isNewFrontierCell(unsigned int idx, 69 | const std::vector& frontier_flag); 70 | 71 | /** 72 | * @brief computes frontier cost 73 | * @details cost function is defined by potential_scale and gain_scale 74 | * 75 | * @param frontier frontier for which compute the cost 76 | * @return cost of the frontier 77 | */ 78 | double frontierCost(const Frontier& frontier); 79 | 80 | private: 81 | costmap_2d::Costmap2D* costmap_; 82 | unsigned char* map_; 83 | unsigned int size_x_, size_y_; 84 | double potential_scale_, gain_scale_; 85 | double min_frontier_size_; 86 | }; 87 | } 88 | #endif 89 | -------------------------------------------------------------------------------- /src/m-explore/explore/launch/explore.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /src/m-explore/explore/launch/explore_costmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/m-explore/explore/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | explore_lite 4 | 2.1.4 5 | 6 | Lightweight frontier-based exploration. 7 | 8 | Jiri Horner 9 | Jiri Horner 10 | BSD 11 | http://wiki.ros.org/explore_lite 12 | 13 | 19 | catkin 20 | 21 | roscpp 22 | std_msgs 23 | move_base_msgs 24 | visualization_msgs 25 | geometry_msgs 26 | map_msgs 27 | nav_msgs 28 | actionlib_msgs 29 | tf 30 | costmap_2d 31 | actionlib 32 | 33 | roslaunch 34 | 35 | -------------------------------------------------------------------------------- /src/m-explore/explore/src/frontier_search.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace frontier_exploration 12 | { 13 | using costmap_2d::LETHAL_OBSTACLE; 14 | using costmap_2d::NO_INFORMATION; 15 | using costmap_2d::FREE_SPACE; 16 | 17 | FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, 18 | double potential_scale, double gain_scale, 19 | double min_frontier_size) 20 | : costmap_(costmap) 21 | , potential_scale_(potential_scale) 22 | , gain_scale_(gain_scale) 23 | , min_frontier_size_(min_frontier_size) 24 | { 25 | } 26 | 27 | std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) 28 | { 29 | std::vector frontier_list; 30 | 31 | // Sanity check that robot is inside costmap bounds before searching 32 | unsigned int mx, my; 33 | if (!costmap_->worldToMap(position.x, position.y, mx, my)) { 34 | ROS_ERROR("Robot out of costmap bounds, cannot search for frontiers"); 35 | return frontier_list; 36 | } 37 | 38 | // make sure map is consistent and locked for duration of search 39 | std::lock_guard lock(*(costmap_->getMutex())); 40 | 41 | map_ = costmap_->getCharMap(); 42 | size_x_ = costmap_->getSizeInCellsX(); 43 | size_y_ = costmap_->getSizeInCellsY(); 44 | 45 | // initialize flag arrays to keep track of visited and frontier cells 46 | std::vector frontier_flag(size_x_ * size_y_, false); 47 | std::vector visited_flag(size_x_ * size_y_, false); 48 | 49 | // initialize breadth first search 50 | std::queue bfs; 51 | 52 | // find closest clear cell to start search 53 | unsigned int clear, pos = costmap_->getIndex(mx, my); 54 | if (nearestCell(clear, pos, FREE_SPACE, *costmap_)) { 55 | bfs.push(clear); 56 | } else { 57 | bfs.push(pos); 58 | ROS_WARN("Could not find nearby clear cell to start search"); 59 | } 60 | visited_flag[bfs.front()] = true; 61 | 62 | while (!bfs.empty()) { 63 | unsigned int idx = bfs.front(); 64 | bfs.pop(); 65 | 66 | // iterate over 4-connected neighbourhood 67 | for (unsigned nbr : nhood4(idx, *costmap_)) { 68 | // add to queue all free, unvisited cells, use descending search in case 69 | // initialized on non-free cell 70 | if (map_[nbr] <= map_[idx] && !visited_flag[nbr]) { 71 | visited_flag[nbr] = true; 72 | bfs.push(nbr); 73 | // check if cell is new frontier cell (unvisited, NO_INFORMATION, free 74 | // neighbour) 75 | } else if (isNewFrontierCell(nbr, frontier_flag)) { 76 | frontier_flag[nbr] = true; 77 | Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag); 78 | if (new_frontier.size * costmap_->getResolution() >= 79 | min_frontier_size_) { 80 | frontier_list.push_back(new_frontier); 81 | } 82 | } 83 | } 84 | } 85 | 86 | // set costs of frontiers 87 | for (auto& frontier : frontier_list) { 88 | frontier.cost = frontierCost(frontier); 89 | } 90 | std::sort( 91 | frontier_list.begin(), frontier_list.end(), 92 | [](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; }); 93 | 94 | return frontier_list; 95 | } 96 | 97 | Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, 98 | unsigned int reference, 99 | std::vector& frontier_flag) 100 | { 101 | // initialize frontier structure 102 | Frontier output; 103 | output.centroid.x = 0; 104 | output.centroid.y = 0; 105 | output.size = 1; 106 | output.min_distance = std::numeric_limits::infinity(); 107 | 108 | // record initial contact point for frontier 109 | unsigned int ix, iy; 110 | costmap_->indexToCells(initial_cell, ix, iy); 111 | costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y); 112 | 113 | // push initial gridcell onto queue 114 | std::queue bfs; 115 | bfs.push(initial_cell); 116 | 117 | // cache reference position in world coords 118 | unsigned int rx, ry; 119 | double reference_x, reference_y; 120 | costmap_->indexToCells(reference, rx, ry); 121 | costmap_->mapToWorld(rx, ry, reference_x, reference_y); 122 | 123 | while (!bfs.empty()) { 124 | unsigned int idx = bfs.front(); 125 | bfs.pop(); 126 | 127 | // try adding cells in 8-connected neighborhood to frontier 128 | for (unsigned int nbr : nhood8(idx, *costmap_)) { 129 | // check if neighbour is a potential frontier cell 130 | if (isNewFrontierCell(nbr, frontier_flag)) { 131 | // mark cell as frontier 132 | frontier_flag[nbr] = true; 133 | unsigned int mx, my; 134 | double wx, wy; 135 | costmap_->indexToCells(nbr, mx, my); 136 | costmap_->mapToWorld(mx, my, wx, wy); 137 | 138 | geometry_msgs::Point point; 139 | point.x = wx; 140 | point.y = wy; 141 | output.points.push_back(point); 142 | 143 | // update frontier size 144 | output.size++; 145 | 146 | // update centroid of frontier 147 | output.centroid.x += wx; 148 | output.centroid.y += wy; 149 | 150 | // determine frontier's distance from robot, going by closest gridcell 151 | // to robot 152 | double distance = sqrt(pow((double(reference_x) - double(wx)), 2.0) + 153 | pow((double(reference_y) - double(wy)), 2.0)); 154 | if (distance < output.min_distance) { 155 | output.min_distance = distance; 156 | output.middle.x = wx; 157 | output.middle.y = wy; 158 | } 159 | 160 | // add to queue for breadth first search 161 | bfs.push(nbr); 162 | } 163 | } 164 | } 165 | 166 | // average out frontier centroid 167 | output.centroid.x /= output.size; 168 | output.centroid.y /= output.size; 169 | return output; 170 | } 171 | 172 | bool FrontierSearch::isNewFrontierCell(unsigned int idx, 173 | const std::vector& frontier_flag) 174 | { 175 | // check that cell is unknown and not already marked as frontier 176 | if (map_[idx] != NO_INFORMATION || frontier_flag[idx]) { 177 | return false; 178 | } 179 | 180 | // frontier cells should have at least one cell in 4-connected neighbourhood 181 | // that is free 182 | for (unsigned int nbr : nhood4(idx, *costmap_)) { 183 | if (map_[nbr] == FREE_SPACE) { 184 | return true; 185 | } 186 | } 187 | 188 | return false; 189 | } 190 | 191 | double FrontierSearch::frontierCost(const Frontier& frontier) 192 | { 193 | return (potential_scale_ * frontier.min_distance * 194 | costmap_->getResolution()) - 195 | (gain_scale_ * frontier.size * costmap_->getResolution()); 196 | } 197 | } 198 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package multirobot_map_merge 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.1.4 (2021-01-07) 6 | ------------------ 7 | * use C++14 8 | * support both OpenCV 3 and OpenCV 4 (support Debian Buster) 9 | * Contributors: Jiri Horner 10 | 11 | 2.1.3 (2021-01-03) 12 | ------------------ 13 | * add missing dependencies to catkin_package calls 14 | * update map_merge for OpenCV 4 15 | * Contributors: Jiri Horner 16 | 17 | 2.1.2 (2021-01-02) 18 | ------------------ 19 | * support for ROS Melodic 20 | * bugfix: zero resolution of the merged grid for known initial posiiton 21 | * bugfix: estimation_confidence parameter had no effect 22 | * map_merge: set origin of merged grid in its centre 23 | * map_merge: add new launch file 24 | * map_merge with 2 maps served by map_server 25 | * bugfix: ensure that we never output map with 0 resolution 26 | * bugfix: nullptr derefence while setting resolution of output grid 27 | * Contributors: Jiri Horner 28 | 29 | 2.1.1 (2017-12-16) 30 | ------------------ 31 | * fix bugs in CMakeLists.txt: install nodes in packages, so they get shipped in debian packages. fixes `#11 `_ 32 | * map_merge: add bibtex to wiki page 33 | * Contributors: Jiri Horner 34 | 35 | 2.1.0 (2017-10-30) 36 | ------------------ 37 | * no major changes. Released together with explore_lite. 38 | 39 | 2.0.0 (2017-03-26) 40 | ------------------ 41 | * map_merge: upgrade to package format 2 42 | * node completely rewritten based on my work included in opencv 43 | * uses more reliable features by default -> more robust merging 44 | * known_init_poses is now by default false to make it easy to start for new users 45 | * Contributors: Jiri Horner 46 | 47 | 1.0.1 (2017-03-25) 48 | ------------------ 49 | * map_merge: use inverted tranform 50 | * transform needs to be inverted before using 51 | * map_merge: change package description 52 | * we support merging with unknown initial positions 53 | * Contributors: Jiri Horner 54 | 55 | 1.0.0 (2016-05-11) 56 | ------------------ 57 | * initial release 58 | * Contributors: Jiri Horner 59 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(multirobot_map_merge) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED COMPONENTS 6 | geometry_msgs 7 | image_geometry 8 | map_msgs 9 | nav_msgs 10 | roscpp 11 | tf2_geometry_msgs 12 | ) 13 | 14 | find_package(Boost REQUIRED COMPONENTS thread) 15 | 16 | # OpenCV is required for merging without initial positions 17 | find_package(OpenCV REQUIRED) 18 | if("${OpenCV_VERSION}" VERSION_LESS "3.0") 19 | message(FATAL_ERROR "This package needs OpenCV >= 3.0") 20 | endif() 21 | if("${OpenCV_VERSION}" VERSION_LESS "4.0") 22 | message(WARNING "This package supports OpenCV 3, but some features may not be " 23 | "available. Upgrade to OpenCV 4 to take advantage of all features.") 24 | endif() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | # we don't have any 30 | 31 | ################################### 32 | ## catkin specific configuration ## 33 | ################################### 34 | catkin_package( 35 | CATKIN_DEPENDS 36 | geometry_msgs 37 | map_msgs 38 | nav_msgs 39 | roscpp 40 | tf2_geometry_msgs 41 | DEPENDS 42 | OpenCV 43 | ) 44 | 45 | ########### 46 | ## Build ## 47 | ########### 48 | set(CMAKE_CXX_STANDARD 14) 49 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 50 | set(CMAKE_CXX_EXTENSIONS OFF) 51 | 52 | ## Specify additional locations of header files 53 | include_directories( 54 | ${catkin_INCLUDE_DIRS} 55 | ${Boost_INCLUDE_DIRS} 56 | ${OpenCV_INCLUDE_DIRS} 57 | include 58 | ) 59 | 60 | # we want static linking for now 61 | add_library(combine_grids STATIC 62 | src/combine_grids/grid_compositor.cpp 63 | src/combine_grids/grid_warper.cpp 64 | src/combine_grids/merging_pipeline.cpp 65 | ) 66 | add_dependencies(combine_grids ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 67 | target_link_libraries(combine_grids ${OpenCV_LIBRARIES}) 68 | 69 | add_executable(map_merge 70 | src/map_merge.cpp 71 | ) 72 | add_dependencies(map_merge ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 73 | target_link_libraries(map_merge combine_grids ${catkin_LIBRARIES}) 74 | 75 | ############# 76 | ## Install ## 77 | ############# 78 | 79 | # install nodes, installing combine_grids should not be necessary, 80 | # but lets make catkin_lint happy 81 | install(TARGETS combine_grids map_merge 82 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 83 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 84 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 85 | ) 86 | 87 | # install roslaunch files 88 | install(DIRECTORY launch/ 89 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 90 | ) 91 | 92 | ############# 93 | ## Testing ## 94 | ############# 95 | if(CATKIN_ENABLE_TESTING) 96 | find_package(roslaunch REQUIRED) 97 | 98 | # download test data 99 | set(base_url https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge) 100 | catkin_download_test_data(${PROJECT_NAME}_map00.pgm ${base_url}/hector_maps/map00.pgm MD5 915609a85793ec1375f310d44f2daf87) 101 | catkin_download_test_data(${PROJECT_NAME}_map05.pgm ${base_url}/hector_maps/map05.pgm MD5 cb9154c9fa3d97e5e992592daca9853a) 102 | catkin_download_test_data(${PROJECT_NAME}_2011-08-09-12-22-52.pgm ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm MD5 3c2c38e7dec2b7a67f41069ab58badaa) 103 | catkin_download_test_data(${PROJECT_NAME}_2012-01-28-11-12-01.pgm ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm MD5 681e704044889c95e47b0c3aadd81f1e) 104 | 105 | catkin_add_gtest(test_merging_pipeline test/test_merging_pipeline.cpp) 106 | # ensure that test data are downloaded before we run tests 107 | add_dependencies(test_merging_pipeline ${PROJECT_NAME}_map00.pgm ${PROJECT_NAME}_map05.pgm ${PROJECT_NAME}_2011-08-09-12-22-52.pgm ${PROJECT_NAME}_2012-01-28-11-12-01.pgm) 108 | target_link_libraries(test_merging_pipeline combine_grids ${catkin_LIBRARIES}) 109 | 110 | # test all launch files 111 | # do not test from_map_server.launch as we don't want to add dependency on map_server and this 112 | # launchfile is not critical 113 | roslaunch_add_file_check(launch/map_merge.launch) 114 | roslaunch_add_file_check(launch/experiments) 115 | endif() 116 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/doc/architecture.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/scybd/Sweeping-Robot/3c35cece54b061cbc3345794e1664cd715e1c429/src/m-explore/map_merge/doc/architecture.dia -------------------------------------------------------------------------------- /src/m-explore/map_merge/doc/screenshot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/scybd/Sweeping-Robot/3c35cece54b061cbc3345794e1664cd715e1c429/src/m-explore/map_merge/doc/screenshot.jpg -------------------------------------------------------------------------------- /src/m-explore/map_merge/doc/wiki_doc.txt: -------------------------------------------------------------------------------- 1 | <> 2 | 3 | <> 4 | 5 | <> 6 | 7 | == Overview == 8 | This package provides global map for multiple robots. It can merge maps from arbitrary number of robots. It expects maps from individual robots as ROS topics. If your run multiple robots under the same ROS master then {{{multirobot_map_merge}}} will probably work for you out-of-the-box. It is also very easy to setup an simulation experiment. 9 | 10 | {{attachment:screenshot.jpg||width="755px"}} 11 | 12 | If your run your robots under multiple ROS masters you need to run your own way of communication between robots and provide maps from robots on local topics (under the same master). Also if you want to distribute merged map back to robots your communication must take care of it. 13 | 14 | <> 15 | 16 | {{{multirobot_map_merge}}} does not depend on any particular communication between robots. 17 | 18 | == Architecture == 19 | 20 | {{{multirobot_map_merge}}} finds robot maps dynamically and new robots can be added to system at any time. 21 | 22 | {{attachment:architecture.svg||width="755px"}} 23 | 24 | To make this dynamic behaviour possible there are some constrains placed on robots. First all robots must publish map under `/map`, where topic name (`map`) is configurable, but must be same for all robots. For each robot `` will be of cause different. 25 | 26 | This node support merging maps with known initial positions of the robots or without. See below for details. 27 | 28 | == Merging modes == 29 | 30 | Two merging modes are currently supported as orthogonal options. If you know initial positions of robots you may preferably use the first mode and get exact results (rigid transformation will be computed according to initial positions). If you don't know robot's starting points you are still able to use the second mode where transformation between grids will be determined using heuristic algorithm. You can choose between these two modes using the `known_init_poses` parameter. 31 | 32 | === merging with known initial positions === 33 | 34 | This is preferred mode whenever you are able to determine exact starting point for each robot. You need to provide initial position for each robot. You need to provide set of `/map_merge/init_pose` parameters. These positions should be in `world_frame`. See [[#ROS API]]. 35 | 36 | In this merging these parameters are mandatory. If any of the required parameters is missing robot won't be considered for merging (you will get warning with name of affected robot). 37 | 38 | === merging without known initial positions === 39 | 40 | If you can't provide initial poses for robots this mode has minimal configuration requirements. You need to provide only map topic for each robot. Transformation between grids is estimated by feature-matching algorithm and therefore requires grids to have sufficient amount of overlapping space to make a high-probability match. If grids don't have enough overlapping space to make a solid match, merged map can differ greatly from physical situation. 41 | 42 | Estimating transforms between grids is cpu-intesive so you might want to tune `estimation_rate` parameter to run re-estimation less often if it causes any troubles. 43 | 44 | == ROS API == 45 | {{{ 46 | #!clearsilver CS/NodeAPI 47 | 48 | name = map_merge 49 | desc = Provides map merging services offered by this package. Dynamically looks for new robots in the system and merges their maps. 50 | 51 | pub { 52 | 0.name = map 53 | 0.type = nav_msgs/OccupancyGrid 54 | 0.desc = Merged map from all robots in the system. 55 | } 56 | sub { 57 | 0.name = /map 58 | 0.type = nav_msgs/OccupancyGrid 59 | 0.desc = Local map for specific robot. 60 | 61 | 1.name = /map_updates 62 | 1.type = map_msgs/OccupancyGridUpdate 63 | 1.desc = Local map updates for specific robot. Most of the <> sources (mapping algorithms) provides incremental map updates via this topic so they don't need to send always full map. This topic is optional. If your mapping algorithm does not provide this topic it is safe to ignore this topic. However if your mapping algorithm does provide this topic, it is preferable to subscribe to this topic. Otherwise map updates will be slow as all partial updates will be missed and map will be able to update only on full map updates. 64 | } 65 | 66 | param { 67 | group.0 { 68 | name = ROBOT PARAMETERS 69 | desc = Parameters that should be defined in the namespace of each robot if you want to use merging with known initial poses of robots (`known_init_poses` is `true`). Without these parameters robots won't be considered for merging. If you can't provide these parameters use merging without known initial poses. See [[#Merging modes]] 70 | 71 | 0.name = /map_merge/init_pose_x 72 | 0.default = `` 73 | 0.type = double 74 | 0.desc = `x` coordinate of robot initial position in `world_frame`. Should be in meters. It does not matter which frame you will consider global (preferably it should be different from all robots frames), but relative positions of robots in this frame must be correct. 75 | 76 | 1.name = /map_merge/init_pose_y 77 | 1.default = `` 78 | 1.type = double 79 | 1.desc = `y` coordinate of robot initial position in `world_frame`. 80 | 81 | 2.name = /map_merge/init_pose_z 82 | 2.default = `` 83 | 2.type = double 84 | 2.desc = `z` coordinate of robot initial position in `world_frame`. 85 | 86 | 3.name = /map_merge/init_pose_yaw 87 | 3.default = `` 88 | 3.type = double 89 | 3.desc = `yaw` component of robot initial position in `world_frame`. Represents robot rotation in radians. 90 | } 91 | 92 | group.1 { 93 | name = NODE PARAMETERS 94 | desc = Parameters that should be defined in the namespace of this node. 95 | 96 | 0.name = ~robot_map_topic 97 | 0.default = `map` 98 | 0.type = string 99 | 0.desc = Name of robot map topic without namespaces (last component of topic name). Only topics with this name will be considered when looking for new maps to merge. This topics may be subject to further filtering (see below). 100 | 101 | 1.name = ~robot_map_updates_topic 102 | 1.default = `map_updates` 103 | 1.type = string 104 | 1.desc = Name of robot map updates topic of <> without namespaces (last component of topic name). This topic will be always subscribed in the same namespace as `robot_map_topic`. You'll likely need to change this only when you changed `robot_map_topic`. These topics are never considered when searching for new robots. 105 | 106 | 2.name = ~robot_namespace 107 | 2.default = `` 108 | 2.type = string 109 | 2.desc = Fixed part of robot map topic. You can employ this parameter to further limit which topics will be considered during dynamic lookup for robots. Only topics which contain (anywhere) this string will be considered for lookup. Unlike `robot_map_topic` you are not limited by namespace logic. Topics will be filtered using text-based search. Therefore `robot_namespace` does not need to be ROS namespace, but can contain slashes etc. This must be common part of all robots map topics name (all robots for which you want to merge map). 110 | 111 | 3.name = ~known_init_poses 112 | 3.default = `true` 113 | 3.type = bool 114 | 3.desc = Selects between merging modes. `true` if merging with known initial positions. See [[#Merging modes]] 115 | 116 | 4.name = ~merged_map_topic 117 | 4.default = `map` 118 | 4.type = string 119 | 4.desc = Topic name where merged map will be published. 120 | 121 | 5.name = ~world_frame 122 | 5.default = `world` 123 | 5.type = string 124 | 5.desc = Frame id (in [[tf]] tree) which will be assigned to published merged map. This should be frame where you specified robot initial positions. 125 | 126 | 6.name = ~merging_rate 127 | 6.default = `4.0` 128 | 6.type = double 129 | 6.desc = Rate in Hz. Basic frequency on which this node discovers merges robots maps and publish merged map. Increase this value if you want faster updates. 130 | 131 | 7.name = ~discovery_rate 132 | 7.default = `0.05` 133 | 7.type = double 134 | 7.desc = Rate in Hz. Frequency on which this node discovers new robots. Increase this value if you need more agile behaviour when adding new robots. Robots will be discovered sooner. 135 | 136 | 8.name = ~estimation_rate 137 | 8.default = `0.5` 138 | 8.type = double 139 | 8.desc = Rate in Hz. This parameter is relevant only when merging without known positions, see [[#Merging modes]]. Frequency on which this node re-estimates transformation between grids. Estimation is cpu-intensive, so you may wish to lower this value. 140 | 141 | 9.name = ~estimation_confidence 142 | 9.default = `1.0` 143 | 9.type = double 144 | 9.desc = This parameter is relevant only when merging without known positions, see [[#Merging modes]]. Confidence according to probabilistic model for initial positions estimation. Default value 1.0 is suitable for most applications, increase this value for more confident estimations. Number of maps included in the merge may decrease with increasing confidence. Generally larger overlaps between maps will be required for map to be included in merge. Good range for tuning is [1.0, 2.0]. 145 | } 146 | } 147 | }}} 148 | 149 | == Acknowledgements == 150 | 151 | This package was developed as part of my bachelor thesis at [[http://www.mff.cuni.cz/to.en/|Charles University]] in Prague. 152 | 153 | {{{ 154 | @masterthesis{Hörner2016, 155 | author = {Jiří Hörner}, 156 | title = {Map-merging for multi-robot system}, 157 | address = {Prague}, 158 | year = {2016}, 159 | school = {Charles University in Prague, Faculty of Mathematics and Physics}, 160 | type = {Bachelor's thesis}, 161 | URL = {https://is.cuni.cz/webapps/zzp/detail/174125/}, 162 | } 163 | }}} 164 | 165 | Idea for dynamic robot discovery is from [[map_merging]] package from Zhi Yan. Merging algorithm and configuration are different. 166 | 167 | ## AUTOGENERATED DON'T DELETE 168 | ## CategoryPackage 169 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/include/combine_grids/grid_compositor.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #ifndef GRID_COMPOSITOR_H_ 38 | #define GRID_COMPOSITOR_H_ 39 | 40 | #include 41 | 42 | #include 43 | 44 | namespace combine_grids 45 | { 46 | namespace internal 47 | { 48 | class GridCompositor 49 | { 50 | public: 51 | nav_msgs::OccupancyGrid::Ptr compose(const std::vector& grids, 52 | const std::vector& rois); 53 | }; 54 | 55 | } // namespace internal 56 | 57 | } // namespace combine_grids 58 | 59 | #endif // GRID_COMPOSITOR_H_ 60 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/include/combine_grids/grid_warper.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #ifndef GRID_WARPER_H_ 38 | #define GRID_WARPER_H_ 39 | 40 | #include 41 | 42 | namespace combine_grids 43 | { 44 | namespace internal 45 | { 46 | class GridWarper 47 | { 48 | public: 49 | cv::Rect warp(const cv::Mat& grid, const cv::Mat& transform, 50 | cv::Mat& warped_grid); 51 | 52 | private: 53 | cv::Rect warpRoi(const cv::Mat& grid, const cv::Mat& transform); 54 | }; 55 | 56 | } // namespace internal 57 | 58 | } // namespace combine_grids 59 | 60 | #endif // GRID_WARPER_H_ 61 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/include/combine_grids/merging_pipeline.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #ifndef MERGING_PIPELINE_H_ 38 | #define MERGING_PIPELINE_H_ 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | namespace combine_grids 48 | { 49 | enum class FeatureType { AKAZE, ORB, SURF }; 50 | 51 | /** 52 | * @brief Pipeline for merging overlapping occupancy grids 53 | * @details Pipeline works on internally stored grids. 54 | */ 55 | class MergingPipeline 56 | { 57 | public: 58 | template 59 | void feed(InputIt grids_begin, InputIt grids_end); 60 | bool estimateTransforms(FeatureType feature = FeatureType::AKAZE, 61 | double confidence = 1.0); 62 | nav_msgs::OccupancyGrid::Ptr composeGrids(); 63 | 64 | std::vector getTransforms() const; 65 | template 66 | bool setTransforms(InputIt transforms_begin, InputIt transforms_end); 67 | 68 | private: 69 | std::vector grids_; 70 | std::vector images_; 71 | std::vector transforms_; 72 | }; 73 | 74 | template 75 | void MergingPipeline::feed(InputIt grids_begin, InputIt grids_end) 76 | { 77 | static_assert(std::is_assignable::value, 79 | "grids_begin must point to nav_msgs::OccupancyGrid::ConstPtr " 80 | "data"); 81 | 82 | // we can't reserve anything, because we want to support just InputIt and 83 | // their guarantee validity for only single-pass algos 84 | images_.clear(); 85 | grids_.clear(); 86 | for (InputIt it = grids_begin; it != grids_end; ++it) { 87 | if (*it && !(*it)->data.empty()) { 88 | grids_.push_back(*it); 89 | /* convert to opencv images. it creates only a view for opencv and does 90 | * not copy or own actual data. */ 91 | images_.emplace_back((*it)->info.height, (*it)->info.width, CV_8UC1, 92 | const_cast((*it)->data.data())); 93 | } else { 94 | grids_.emplace_back(); 95 | images_.emplace_back(); 96 | } 97 | } 98 | } 99 | 100 | template 101 | bool MergingPipeline::setTransforms(InputIt transforms_begin, 102 | InputIt transforms_end) 103 | { 104 | static_assert(std::is_assignable::value, 106 | "transforms_begin must point to geometry_msgs::Transform " 107 | "data"); 108 | 109 | decltype(transforms_) transforms_buf; 110 | for (InputIt it = transforms_begin; it != transforms_end; ++it) { 111 | const geometry_msgs::Quaternion& q = it->rotation; 112 | if ((q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) < 113 | std::numeric_limits::epsilon()) { 114 | // represents invalid transform 115 | transforms_buf.emplace_back(); 116 | continue; 117 | } 118 | double s = 2.0 / (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); 119 | double a = 1 - q.y * q.y * s - q.z * q.z * s; 120 | double b = q.x * q.y * s + q.z * q.w * s; 121 | double tx = it->translation.x; 122 | double ty = it->translation.y; 123 | cv::Mat transform = cv::Mat::eye(3, 3, CV_64F); 124 | transform.at(0, 0) = transform.at(1, 1) = a; 125 | transform.at(1, 0) = b; 126 | transform.at(0, 1) = -b; 127 | transform.at(0, 2) = tx; 128 | transform.at(1, 2) = ty; 129 | 130 | transforms_buf.emplace_back(std::move(transform)); 131 | } 132 | 133 | if (transforms_buf.size() != images_.size()) { 134 | return false; 135 | } 136 | std::swap(transforms_, transforms_buf); 137 | 138 | return true; 139 | } 140 | 141 | } // namespace combine_grids 142 | 143 | #endif // MERGING_PIPELINE_H_ 144 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/include/map_merge/map_merge.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2014, Zhi Yan. 6 | * Copyright (c) 2015-2016, Jiri Horner. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #ifndef MAP_MERGE_H_ 39 | #define MAP_MERGE_H_ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | namespace map_merge 54 | { 55 | struct MapSubscription { 56 | // protects consistency of writable_map and readonly_map 57 | // also protects reads and writes of shared_ptrs 58 | std::mutex mutex; 59 | 60 | geometry_msgs::Transform initial_pose; 61 | nav_msgs::OccupancyGrid::Ptr writable_map; 62 | nav_msgs::OccupancyGrid::ConstPtr readonly_map; 63 | 64 | ros::Subscriber map_sub; 65 | ros::Subscriber map_updates_sub; 66 | }; 67 | 68 | class MapMerge 69 | { 70 | private: 71 | ros::NodeHandle node_; 72 | 73 | /* parameters */ 74 | double merging_rate_; 75 | double discovery_rate_; 76 | double estimation_rate_; 77 | double confidence_threshold_; 78 | std::string robot_map_topic_; 79 | std::string robot_map_updates_topic_; 80 | std::string robot_namespace_; 81 | std::string world_frame_; 82 | bool have_initial_poses_; 83 | 84 | // publishing 85 | ros::Publisher merged_map_publisher_; 86 | // maps robots namespaces to maps. does not own 87 | std::unordered_map robots_; 88 | // owns maps -- iterator safe 89 | std::forward_list subscriptions_; 90 | size_t subscriptions_size_; 91 | boost::shared_mutex subscriptions_mutex_; 92 | combine_grids::MergingPipeline pipeline_; 93 | std::mutex pipeline_mutex_; 94 | 95 | std::string robotNameFromTopic(const std::string& topic); 96 | bool isRobotMapTopic(const ros::master::TopicInfo& topic); 97 | bool getInitPose(const std::string& name, geometry_msgs::Transform& pose); 98 | 99 | void fullMapUpdate(const nav_msgs::OccupancyGrid::ConstPtr& msg, 100 | MapSubscription& map); 101 | void partialMapUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr& msg, 102 | MapSubscription& map); 103 | 104 | public: 105 | MapMerge(); 106 | 107 | void spin(); 108 | void executetopicSubscribing(); 109 | void executemapMerging(); 110 | void executeposeEstimation(); 111 | 112 | void topicSubscribing(); 113 | void mapMerging(); 114 | /** 115 | * @brief Estimates initial positions of grids 116 | * @details Relevant only if initial poses are not known 117 | */ 118 | void poseEstimation(); 119 | }; 120 | 121 | } // namespace map_merge 122 | 123 | #endif /* MAP_MERGE_H_ */ 124 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/launch/experiments/3-robots.ttt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/launch/experiments/demo-scene-1.ttt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/launch/experiments/demo-scene-orientation-exp.ttt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/launch/from_map_server.launch: -------------------------------------------------------------------------------- 1 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/launch/map_merge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/launch/map_merge.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Map1 10 | Splitter Ratio: 0.5 11 | Tree Height: 735 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 0.699999988079071 53 | Class: rviz/Map 54 | Color Scheme: map 55 | Draw Behind: false 56 | Enabled: true 57 | Name: Map 58 | Topic: /map_merge/map 59 | Unreliable: false 60 | Use Timestamp: false 61 | Value: true 62 | Enabled: true 63 | Global Options: 64 | Background Color: 48; 48; 48 65 | Default Light: true 66 | Fixed Frame: world 67 | Frame Rate: 30 68 | Name: root 69 | Tools: 70 | - Class: rviz/Interact 71 | Hide Inactive Objects: true 72 | - Class: rviz/MoveCamera 73 | - Class: rviz/Select 74 | - Class: rviz/FocusCamera 75 | - Class: rviz/Measure 76 | - Class: rviz/SetInitialPose 77 | Topic: /initialpose 78 | - Class: rviz/SetGoal 79 | Topic: /move_base_simple/goal 80 | - Class: rviz/PublishPoint 81 | Single click: true 82 | Topic: /clicked_point 83 | Value: true 84 | Views: 85 | Current: 86 | Angle: 0 87 | Class: rviz/TopDownOrtho 88 | Enable Stereo Rendering: 89 | Stereo Eye Separation: 0.05999999865889549 90 | Stereo Focal Distance: 1 91 | Swap Stereo Eyes: false 92 | Value: false 93 | Invert Z Axis: false 94 | Name: Current View 95 | Near Clip Distance: 0.009999999776482582 96 | Scale: 38.159217834472656 97 | Target Frame: 98 | Value: TopDownOrtho (rviz) 99 | X: 0 100 | Y: 0 101 | Saved: ~ 102 | Window Geometry: 103 | Displays: 104 | collapsed: true 105 | Height: 1027 106 | Hide Left Dock: true 107 | Hide Right Dock: true 108 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000368fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003c00000368000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000368fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003c00000368000000a100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003460000003efc0100000002fb0000000800540069006d00650100000000000003460000025900fffffffb0000000800540069006d00650100000000000004500000000000000000000003460000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 109 | Selection: 110 | collapsed: false 111 | Time: 112 | collapsed: false 113 | Tool Properties: 114 | collapsed: false 115 | Views: 116 | collapsed: true 117 | Width: 838 118 | X: 0 119 | Y: 21 120 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multirobot_map_merge 4 | 2.1.4 5 | 6 | Merging multiple maps without knowledge of initial 7 | positions of robots. 8 | 9 | Jiri Horner 10 | Jiri Horner 11 | BSD 12 | http://wiki.ros.org/multirobot_map_merge 13 | 14 | catkin 15 | 16 | roscpp 17 | geometry_msgs 18 | nav_msgs 19 | map_msgs 20 | tf2_geometry_msgs 21 | 22 | image_geometry 23 | 24 | roslaunch 25 | rosunit 26 | 27 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/src/combine_grids/estimation_internal.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #ifndef ESTIMATION_INTERNAL_H_ 38 | #define ESTIMATION_INTERNAL_H_ 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | #ifdef HAVE_OPENCV_XFEATURES2D 50 | #include 51 | #endif 52 | 53 | namespace combine_grids 54 | { 55 | namespace internal 56 | { 57 | #if CV_VERSION_MAJOR >= 4 58 | 59 | static inline cv::Ptr chooseFeatureFinder(FeatureType type) 60 | { 61 | switch (type) { 62 | case FeatureType::AKAZE: 63 | return cv::AKAZE::create(); 64 | case FeatureType::ORB: 65 | return cv::ORB::create(); 66 | case FeatureType::SURF: 67 | #ifdef HAVE_OPENCV_XFEATURES2D 68 | return xfeatures2d::SURF::create(); 69 | #else 70 | return cv::AKAZE::create(); 71 | #endif 72 | } 73 | 74 | assert(false); 75 | return {}; 76 | } 77 | 78 | #else // (CV_VERSION_MAJOR < 4) 79 | 80 | static inline cv::Ptr 81 | chooseFeatureFinder(FeatureType type) 82 | { 83 | switch (type) { 84 | case FeatureType::AKAZE: 85 | return cv::makePtr(); 86 | case FeatureType::ORB: 87 | return cv::makePtr(); 88 | case FeatureType::SURF: 89 | return cv::makePtr(); 90 | } 91 | 92 | assert(false); 93 | return {}; 94 | } 95 | 96 | #endif // CV_VERSION_MAJOR >= 4 97 | 98 | static inline void writeDebugMatchingInfo( 99 | const std::vector& images, 100 | const std::vector& image_features, 101 | const std::vector& pairwise_matches) 102 | { 103 | for (auto& match_info : pairwise_matches) { 104 | if (match_info.H.empty() || 105 | match_info.src_img_idx >= match_info.dst_img_idx) { 106 | continue; 107 | } 108 | std::cout << match_info.src_img_idx << " " << match_info.dst_img_idx 109 | << std::endl 110 | << "features: " 111 | << image_features[size_t(match_info.src_img_idx)].keypoints.size() 112 | << " " 113 | << image_features[size_t(match_info.dst_img_idx)].keypoints.size() 114 | << std::endl 115 | << "matches: " << match_info.matches.size() << std::endl 116 | << "inliers: " << match_info.num_inliers << std::endl 117 | << "inliers/matches ratio: " 118 | << match_info.num_inliers / double(match_info.matches.size()) 119 | << std::endl 120 | << "confidence: " << match_info.confidence << std::endl 121 | << match_info.H << std::endl; 122 | cv::Mat img; 123 | // draw all matches 124 | cv::drawMatches(images[size_t(match_info.src_img_idx)], 125 | image_features[size_t(match_info.src_img_idx)].keypoints, 126 | images[size_t(match_info.dst_img_idx)], 127 | image_features[size_t(match_info.dst_img_idx)].keypoints, 128 | match_info.matches, img); 129 | cv::imwrite(std::to_string(match_info.src_img_idx) + "_" + 130 | std::to_string(match_info.dst_img_idx) + "_matches.png", 131 | img); 132 | // draw inliers only 133 | cv::drawMatches( 134 | images[size_t(match_info.src_img_idx)], 135 | image_features[size_t(match_info.src_img_idx)].keypoints, 136 | images[size_t(match_info.dst_img_idx)], 137 | image_features[size_t(match_info.dst_img_idx)].keypoints, 138 | match_info.matches, img, cv::Scalar::all(-1), cv::Scalar::all(-1), 139 | *reinterpret_cast*>(&match_info.inliers_mask)); 140 | cv::imwrite(std::to_string(match_info.src_img_idx) + "_" + 141 | std::to_string(match_info.dst_img_idx) + 142 | "_matches_inliers.png", 143 | img); 144 | } 145 | } 146 | 147 | } // namespace internal 148 | } // namespace combine_grids 149 | 150 | #endif // ESTIMATION_INTERNAL_H_ 151 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/src/combine_grids/grid_compositor.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | 43 | namespace combine_grids 44 | { 45 | namespace internal 46 | { 47 | nav_msgs::OccupancyGrid::Ptr GridCompositor::compose( 48 | const std::vector& grids, const std::vector& rois) 49 | { 50 | ROS_ASSERT(grids.size() == rois.size()); 51 | 52 | nav_msgs::OccupancyGrid::Ptr result_grid(new nav_msgs::OccupancyGrid()); 53 | 54 | std::vector corners; 55 | corners.reserve(grids.size()); 56 | std::vector sizes; 57 | sizes.reserve(grids.size()); 58 | for (auto& roi : rois) { 59 | corners.push_back(roi.tl()); 60 | sizes.push_back(roi.size()); 61 | } 62 | cv::Rect dst_roi = cv::detail::resultRoi(corners, sizes); 63 | 64 | result_grid->info.width = static_cast(dst_roi.width); 65 | result_grid->info.height = static_cast(dst_roi.height); 66 | result_grid->data.resize(static_cast(dst_roi.area()), -1); 67 | // create view for opencv pointing to newly allocated grid 68 | cv::Mat result(dst_roi.size(), CV_8S, result_grid->data.data()); 69 | 70 | for (size_t i = 0; i < grids.size(); ++i) { 71 | // we need to compensate global offset 72 | cv::Rect roi = cv::Rect(corners[i] - dst_roi.tl(), sizes[i]); 73 | cv::Mat result_roi(result, roi); 74 | // reinterpret warped matrix as signed 75 | // we will not change this matrix, but opencv does not support const matrices 76 | cv::Mat warped_signed (grids[i].size(), CV_8S, const_cast(grids[i].ptr())); 77 | // compose img into result matrix 78 | cv::max(result_roi, warped_signed, result_roi); 79 | } 80 | 81 | return result_grid; 82 | } 83 | 84 | } // namespace internal 85 | 86 | } // namespace combine_grids 87 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/src/combine_grids/grid_warper.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | 43 | namespace combine_grids 44 | { 45 | namespace internal 46 | { 47 | cv::Rect GridWarper::warp(const cv::Mat& grid, const cv::Mat& transform, 48 | cv::Mat& warped_grid) 49 | { 50 | ROS_ASSERT(transform.type() == CV_64F); 51 | cv::Mat H; 52 | invertAffineTransform(transform.rowRange(0, 2), H); 53 | cv::Rect roi = warpRoi(grid, H); 54 | // shift top left corner for warp affine (otherwise the image is cropped) 55 | H.at(0, 2) -= roi.tl().x; 56 | H.at(1, 2) -= roi.tl().y; 57 | warpAffine(grid, warped_grid, H, roi.size(), cv::INTER_NEAREST, 58 | cv::BORDER_CONSTANT, 59 | cv::Scalar::all(255) /* this is -1 for signed char */); 60 | ROS_ASSERT(roi.size() == warped_grid.size()); 61 | 62 | return roi; 63 | } 64 | 65 | cv::Rect GridWarper::warpRoi(const cv::Mat& grid, const cv::Mat& transform) 66 | { 67 | cv::Ptr warper = 68 | cv::makePtr(); 69 | cv::Mat H; 70 | transform.convertTo(H, CV_32F); 71 | 72 | // separate rotation and translation for plane warper 73 | // 3D translation 74 | cv::Mat T = cv::Mat::zeros(3, 1, CV_32F); 75 | H.colRange(2, 3).rowRange(0, 2).copyTo(T.rowRange(0, 2)); 76 | // 3D rotation 77 | cv::Mat R = cv::Mat::eye(3, 3, CV_32F); 78 | H.colRange(0, 2).copyTo(R.rowRange(0, 2).colRange(0, 2)); 79 | 80 | return warper->warpRoi(grid.size(), cv::Mat::eye(3, 3, CV_32F), R, T); 81 | } 82 | 83 | } // namespace internal 84 | 85 | } // namespace combine_grids 86 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/src/combine_grids/merging_pipeline.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | #include "estimation_internal.h" 47 | 48 | namespace combine_grids 49 | { 50 | bool MergingPipeline::estimateTransforms(FeatureType feature_type, 51 | double confidence) 52 | { 53 | std::vector image_features; 54 | std::vector pairwise_matches; 55 | std::vector transforms; 56 | std::vector good_indices; 57 | // TODO investigate value translation effect on features 58 | auto finder = internal::chooseFeatureFinder(feature_type); 59 | cv::Ptr matcher = 60 | cv::makePtr(); 61 | cv::Ptr estimator = 62 | cv::makePtr(); 63 | cv::Ptr adjuster = 64 | cv::makePtr(); 65 | 66 | if (images_.empty()) { 67 | return true; 68 | } 69 | 70 | /* find features in images */ 71 | ROS_DEBUG("computing features"); 72 | image_features.reserve(images_.size()); 73 | for (const cv::Mat& image : images_) { 74 | image_features.emplace_back(); 75 | if (!image.empty()) { 76 | #if CV_VERSION_MAJOR >= 4 77 | cv::detail::computeImageFeatures(finder, image, image_features.back()); 78 | #else 79 | (*finder)(image, image_features.back()); 80 | #endif 81 | } 82 | } 83 | finder = {}; 84 | 85 | /* find corespondent features */ 86 | ROS_DEBUG("pairwise matching features"); 87 | (*matcher)(image_features, pairwise_matches); 88 | matcher = {}; 89 | 90 | #ifndef NDEBUG 91 | internal::writeDebugMatchingInfo(images_, image_features, pairwise_matches); 92 | #endif 93 | 94 | /* use only matches that has enough confidence. leave out matches that are not 95 | * connected (small components) */ 96 | good_indices = cv::detail::leaveBiggestComponent( 97 | image_features, pairwise_matches, static_cast(confidence)); 98 | 99 | // no match found. try set first non-empty grid as reference frame. we try to 100 | // avoid setting empty grid as reference frame, in case some maps never 101 | // arrive. If all is empty just set null transforms. 102 | if (good_indices.size() == 1) { 103 | transforms_.clear(); 104 | transforms_.resize(images_.size()); 105 | for (size_t i = 0; i < images_.size(); ++i) { 106 | if (!images_[i].empty()) { 107 | // set identity 108 | transforms_[i] = cv::Mat::eye(3, 3, CV_64F); 109 | break; 110 | } 111 | } 112 | return true; 113 | } 114 | 115 | /* estimate transform */ 116 | ROS_DEBUG("calculating transforms in global reference frame"); 117 | // note: currently used estimator never fails 118 | if (!(*estimator)(image_features, pairwise_matches, transforms)) { 119 | return false; 120 | } 121 | 122 | /* levmarq optimization */ 123 | // openCV just accepts float transforms 124 | for (auto& transform : transforms) { 125 | transform.R.convertTo(transform.R, CV_32F); 126 | } 127 | ROS_DEBUG("optimizing global transforms"); 128 | adjuster->setConfThresh(confidence); 129 | if (!(*adjuster)(image_features, pairwise_matches, transforms)) { 130 | ROS_WARN("Bundle adjusting failed. Could not estimate transforms."); 131 | return false; 132 | } 133 | 134 | transforms_.clear(); 135 | transforms_.resize(images_.size()); 136 | size_t i = 0; 137 | for (auto& j : good_indices) { 138 | // we want to work with transforms as doubles 139 | transforms[i].R.convertTo(transforms_[static_cast(j)], CV_64F); 140 | ++i; 141 | } 142 | 143 | return true; 144 | } 145 | 146 | // checks whether given matrix is an identity, i.e. exactly appropriate Mat::eye 147 | static inline bool isIdentity(const cv::Mat& matrix) 148 | { 149 | if (matrix.empty()) { 150 | return false; 151 | } 152 | cv::MatExpr diff = matrix != cv::Mat::eye(matrix.size(), matrix.type()); 153 | return cv::countNonZero(diff) == 0; 154 | } 155 | 156 | nav_msgs::OccupancyGrid::Ptr MergingPipeline::composeGrids() 157 | { 158 | ROS_ASSERT(images_.size() == transforms_.size()); 159 | ROS_ASSERT(images_.size() == grids_.size()); 160 | 161 | if (images_.empty()) { 162 | return nullptr; 163 | } 164 | 165 | ROS_DEBUG("warping grids"); 166 | internal::GridWarper warper; 167 | std::vector imgs_warped; 168 | imgs_warped.reserve(images_.size()); 169 | std::vector rois; 170 | rois.reserve(images_.size()); 171 | 172 | for (size_t i = 0; i < images_.size(); ++i) { 173 | if (!transforms_[i].empty() && !images_[i].empty()) { 174 | imgs_warped.emplace_back(); 175 | rois.emplace_back( 176 | warper.warp(images_[i], transforms_[i], imgs_warped.back())); 177 | } 178 | } 179 | 180 | if (imgs_warped.empty()) { 181 | return nullptr; 182 | } 183 | 184 | ROS_DEBUG("compositing result grid"); 185 | nav_msgs::OccupancyGrid::Ptr result; 186 | internal::GridCompositor compositor; 187 | result = compositor.compose(imgs_warped, rois); 188 | 189 | // set correct resolution to output grid. use resolution of identity (works 190 | // for estimated trasforms), or any resolution (works for know_init_positions) 191 | // - in that case all resolutions should be the same. 192 | float any_resolution = 0.0; 193 | for (size_t i = 0; i < transforms_.size(); ++i) { 194 | // check if this transform is the reference frame 195 | if (isIdentity(transforms_[i])) { 196 | result->info.resolution = grids_[i]->info.resolution; 197 | break; 198 | } 199 | if (grids_[i]) { 200 | any_resolution = grids_[i]->info.resolution; 201 | } 202 | } 203 | if (result->info.resolution <= 0.f) { 204 | result->info.resolution = any_resolution; 205 | } 206 | 207 | // set grid origin to its centre 208 | result->info.origin.position.x = 209 | -(result->info.width / 2.0) * double(result->info.resolution); 210 | result->info.origin.position.y = 211 | -(result->info.height / 2.0) * double(result->info.resolution); 212 | result->info.origin.orientation.w = 1.0; 213 | 214 | return result; 215 | } 216 | 217 | std::vector MergingPipeline::getTransforms() const 218 | { 219 | std::vector result; 220 | result.reserve(transforms_.size()); 221 | 222 | for (auto& transform : transforms_) { 223 | if (transform.empty()) { 224 | result.emplace_back(); 225 | continue; 226 | } 227 | 228 | ROS_ASSERT(transform.type() == CV_64F); 229 | geometry_msgs::Transform ros_transform; 230 | ros_transform.translation.x = transform.at(0, 2); 231 | ros_transform.translation.y = transform.at(1, 2); 232 | ros_transform.translation.z = 0.; 233 | 234 | // our rotation is in fact only 2D, thus quaternion can be simplified 235 | double a = transform.at(0, 0); 236 | double b = transform.at(1, 0); 237 | ros_transform.rotation.w = std::sqrt(2. + 2. * a) * 0.5; 238 | ros_transform.rotation.x = 0.; 239 | ros_transform.rotation.y = 0.; 240 | ros_transform.rotation.z = std::copysign(std::sqrt(2. - 2. * a) * 0.5, b); 241 | 242 | result.push_back(ros_transform); 243 | } 244 | 245 | return result; 246 | } 247 | 248 | } // namespace combine_grids 249 | -------------------------------------------------------------------------------- /src/m-explore/map_merge/test/testing_helpers.h: -------------------------------------------------------------------------------- 1 | #ifndef TESTING_HELPERS_H_ 2 | #define TESTING_HELPERS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | const float resolution = 0.05f; 11 | 12 | nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename); 13 | void saveMap(const std::string& filename, 14 | const nav_msgs::OccupancyGridConstPtr& map); 15 | std::tuple randomAngleTxTy(); 16 | geometry_msgs::Transform randomTransform(); 17 | cv::Mat randomTransformMatrix(); 18 | 19 | /* map_server is really bad. until there is no replacement I will implement it 20 | * by myself */ 21 | template 22 | std::vector loadMaps(InputIt filenames_begin, 23 | InputIt filenames_end) 24 | { 25 | std::vector result; 26 | 27 | for (InputIt it = filenames_begin; it != filenames_end; ++it) { 28 | result.emplace_back(loadMap(*it)); 29 | } 30 | return result; 31 | } 32 | 33 | nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename) 34 | { 35 | cv::Mat lookUpTable(1, 256, CV_8S); 36 | signed char* p = lookUpTable.ptr(); 37 | p[254] = 0; 38 | p[205] = -1; 39 | p[0] = 100; 40 | 41 | cv::Mat img = cv::imread(filename, cv::IMREAD_GRAYSCALE); 42 | if (img.empty()) { 43 | throw std::runtime_error("could not load map"); 44 | } 45 | nav_msgs::OccupancyGridPtr grid{new nav_msgs::OccupancyGrid()}; 46 | grid->info.width = static_cast(img.size().width); 47 | grid->info.height = static_cast(img.size().height); 48 | grid->info.resolution = resolution; 49 | grid->data.resize(static_cast(img.size().area())); 50 | cv::Mat grid_view(img.size(), CV_8S, 51 | const_cast(grid->data.data())); 52 | cv::LUT(img, lookUpTable, grid_view); 53 | 54 | return grid; 55 | } 56 | 57 | void saveMap(const std::string& filename, 58 | const nav_msgs::OccupancyGridConstPtr& map) 59 | { 60 | cv::Mat lookUpTable(1, 256, CV_8U); 61 | uchar* p = lookUpTable.ptr(); 62 | for (int i = 0; i < 255; ++i) { 63 | if (i >= 0 && i < 10) 64 | p[i] = 254; 65 | else 66 | p[i] = 0; 67 | } 68 | p[255] = 205; 69 | 70 | cv::Mat img(map->info.height, map->info.width, CV_8S, 71 | const_cast(map->data.data())); 72 | cv::Mat out_img; 73 | cv::LUT(img, lookUpTable, out_img); 74 | cv::imwrite(filename, out_img); 75 | } 76 | 77 | std::tuple randomAngleTxTy() 78 | { 79 | static std::mt19937_64 g(156468754 /*magic*/); 80 | std::uniform_real_distribution rotation_dis(0., 2 * std::acos(-1)); 81 | std::uniform_real_distribution translation_dis(-1000, 1000); 82 | 83 | return std::tuple(rotation_dis(g), translation_dis(g), 84 | translation_dis(g)); 85 | } 86 | 87 | geometry_msgs::Transform randomTransform() 88 | { 89 | double angle, tx, ty; 90 | std::tie(angle, tx, ty) = randomAngleTxTy(); 91 | tf2::Transform transform; 92 | tf2::Quaternion rotation; 93 | rotation.setEuler(0., 0., angle); 94 | rotation.normalize(); 95 | transform.setRotation(rotation); 96 | transform.setOrigin(tf2::Vector3(tx, ty, 0.)); 97 | 98 | auto msg = toMsg(transform); 99 | // normalize quaternion such that w > 0 (q and -q represents the same 100 | // transformation) 101 | if (msg.rotation.w < 0.) { 102 | msg.rotation.x *= -1.; 103 | msg.rotation.y *= -1.; 104 | msg.rotation.z *= -1.; 105 | msg.rotation.w *= -1.; 106 | } 107 | 108 | return msg; 109 | } 110 | 111 | cv::Mat randomTransformMatrix() 112 | { 113 | double angle, tx, ty; 114 | std::tie(angle, tx, ty) = randomAngleTxTy(); 115 | cv::Mat transform = 116 | (cv::Mat_(3, 3) << std::cos(angle), -std::sin(angle), tx, 117 | std::sin(angle), std::cos(angle), ty, 0., 0., 1.); 118 | 119 | return transform; 120 | } 121 | 122 | static inline bool isIdentity(const geometry_msgs::Transform& transform) 123 | { 124 | tf2::Transform t; 125 | tf2::fromMsg(transform, t); 126 | return tf2::Transform::getIdentity() == t; 127 | } 128 | 129 | static inline bool isIdentity(const geometry_msgs::Quaternion& rotation) 130 | { 131 | tf2::Quaternion q; 132 | tf2::fromMsg(rotation, q); 133 | return tf2::Quaternion::getIdentity() == q; 134 | } 135 | 136 | // data size is consistent with height and width 137 | static inline bool consistentData(const nav_msgs::OccupancyGrid& grid) 138 | { 139 | return grid.info.width * grid.info.height == grid.data.size(); 140 | } 141 | 142 | // ignores header, map_load_time and origin 143 | static inline bool operator==(const nav_msgs::OccupancyGrid& grid1, 144 | const nav_msgs::OccupancyGrid& grid2) 145 | { 146 | bool equal = true; 147 | equal &= grid1.info.width == grid2.info.width; 148 | equal &= grid1.info.height == grid2.info.height; 149 | equal &= std::abs(grid1.info.resolution - grid2.info.resolution) < 150 | std::numeric_limits::epsilon(); 151 | equal &= grid1.data.size() == grid2.data.size(); 152 | for (size_t i = 0; i < grid1.data.size(); ++i) { 153 | equal &= grid1.data[i] == grid2.data[i]; 154 | } 155 | return equal; 156 | } 157 | 158 | #endif // TESTING_HELPERS_H_ 159 | -------------------------------------------------------------------------------- /src/manual_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(manual_nav) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | amcl 12 | gmapping 13 | map_server 14 | move_base 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a exec_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | # add_message_files( 52 | # FILES 53 | # Message1.msg 54 | # Message2.msg 55 | # ) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | # Service1.srv 61 | # Service2.srv 62 | # ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | # generate_messages( 73 | # DEPENDENCIES 74 | # std_msgs # Or other packages containing msgs 75 | # ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if your package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES nav 109 | # CATKIN_DEPENDS amcl gmapping map_server move_base 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | # include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/nav.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | # add_executable(${PROJECT_NAME}_node src/nav_node.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(${PROJECT_NAME}_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # catkin_install_python(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables for installation 169 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 170 | # install(TARGETS ${PROJECT_NAME}_node 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark libraries for installation 175 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 176 | # install(TARGETS ${PROJECT_NAME} 177 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 180 | # ) 181 | 182 | ## Mark cpp header files for installation 183 | # install(DIRECTORY include/${PROJECT_NAME}/ 184 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 185 | # FILES_MATCHING PATTERN "*.h" 186 | # PATTERN ".svn" EXCLUDE 187 | # ) 188 | 189 | ## Mark other files for installation (e.g. launch and bag files, etc.) 190 | # install(FILES 191 | # # myfile1 192 | # # myfile2 193 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 194 | # ) 195 | 196 | ############# 197 | ## Testing ## 198 | ############# 199 | 200 | ## Add gtest based cpp test target and link libraries 201 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_nav.cpp) 202 | # if(TARGET ${PROJECT_NAME}-test) 203 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 204 | # endif() 205 | 206 | ## Add folders to be run by python nosetests 207 | # catkin_add_nosetests(test) 208 | -------------------------------------------------------------------------------- /src/manual_nav/config/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | 3 | # Robot Configuration Parameters 4 | max_vel_x: 0.5 # X 方向最大速度 5 | min_vel_x: 0.1 # X 方向最小速速 6 | 7 | max_vel_theta: 1.0 # 8 | min_vel_theta: -1.0 9 | min_in_place_vel_theta: 1.0 10 | 11 | acc_lim_x: 1.0 # X 加速限制 12 | acc_lim_y: 0.0 # Y 加速限制 13 | acc_lim_theta: 0.6 # 角速度加速限制 14 | 15 | # Goal Tolerance Parameters，目标公差 16 | xy_goal_tolerance: 0.10 17 | yaw_goal_tolerance: 0.05 18 | 19 | # Differential-drive robot configuration 20 | # 是否是全向移动机器人 21 | holonomic_robot: false 22 | 23 | # Forward Simulation Parameters，前进模拟参数 24 | sim_time: 0.8 25 | vx_samples: 18 26 | vtheta_samples: 20 27 | sim_granularity: 0.05 28 | -------------------------------------------------------------------------------- /src/manual_nav/config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | #机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint 2 | robot_radius: 0.12 #圆形 3 | # footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状 4 | 5 | obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图 6 | raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物 7 | 8 | 9 | #膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物 10 | inflation_radius: 0.2 11 | #代价比例系数，越大则代价值越小 12 | cost_scaling_factor: 3.0 13 | 14 | #地图类型 15 | map_type: costmap 16 | #导航包所需要的传感器 17 | observation_sources: scan 18 | #对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。 19 | scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true} 20 | -------------------------------------------------------------------------------- /src/manual_nav/config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map #地图坐标系 3 | robot_base_frame: base_footprint #机器人坐标系 4 | # 以此实现坐标变换 5 | 6 | update_frequency: 1.0 #代价地图更新频率 7 | publish_frequency: 1.0 #代价地图的发布频率 8 | transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间 9 | 10 | static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false. 11 | -------------------------------------------------------------------------------- /src/manual_nav/config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom #里程计坐标系 3 | robot_base_frame: base_footprint #机器人坐标系 4 | 5 | update_frequency: 10.0 #代价地图更新频率 6 | publish_frequency: 10.0 #代价地图的发布频率 7 | transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间 8 | 9 | static_map: false #不需要静态地图，可以提升导航效果 10 | rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化 11 | width: 3 # 局部地图宽度 单位是 m 12 | height: 3 # 局部地图高度 单位是 m 13 | resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致 14 | -------------------------------------------------------------------------------- /src/manual_nav/launch/amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /src/manual_nav/launch/move.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /src/manual_nav/launch/nav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /src/manual_nav/launch/save_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/manual_nav/launch/slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /src/manual_nav/map/home_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/scybd/Sweeping-Robot/3c35cece54b061cbc3345794e1664cd715e1c429/src/manual_nav/map/home_map.pgm -------------------------------------------------------------------------------- /src/manual_nav/map/home_map.yaml: -------------------------------------------------------------------------------- 1 | image: /home/thy/MyROS/SweepingRobot/src/manual_nav/map/home_map.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /src/manual_nav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | manual_nav 4 | 0.0.0 5 | The manual_nav package 6 | 7 | 8 | 9 | 10 | thy 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | amcl 53 | gmapping 54 | map_server 55 | move_base 56 | amcl 57 | gmapping 58 | map_server 59 | move_base 60 | amcl 61 | gmapping 62 | map_server 63 | move_base 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /src/robot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(robot) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES robot 108 | # CATKIN_DEPENDS roscpp rospy std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/robot.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/robot_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # catkin_install_python(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_robot.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /src/robot/config/robot.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Odometry1/Shape1 9 | Splitter Ratio: 0.5 10 | Tree Height: 355 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: LaserScan 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/RobotModel 56 | Collision Enabled: false 57 | Enabled: true 58 | Links: 59 | All Links Enabled: true 60 | Expand Joint Details: false 61 | Expand Link Details: false 62 | Expand Tree: false 63 | Link Tree Style: Links in Alphabetic Order 64 | back_wheel: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | base_footprint: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | base_link: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | camera: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | front_wheel: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | laser: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | left_wheel: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | right_wheel: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | support: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | Name: RobotModel 110 | Robot Description: robot_description 111 | TF Prefix: "" 112 | Update Interval: 0 113 | Value: true 114 | Visual Enabled: true 115 | - Angle Tolerance: 0.10000000149011612 116 | Class: rviz/Odometry 117 | Covariance: 118 | Orientation: 119 | Alpha: 0.5 120 | Color: 255; 255; 127 121 | Color Style: Unique 122 | Frame: Local 123 | Offset: 1 124 | Scale: 1 125 | Value: true 126 | Position: 127 | Alpha: 0.30000001192092896 128 | Color: 204; 51; 204 129 | Scale: 1 130 | Value: true 131 | Value: false 132 | Enabled: true 133 | Keep: 100 134 | Name: Odometry 135 | Position Tolerance: 0.10000000149011612 136 | Queue Size: 10 137 | Shape: 138 | Alpha: 0.5 139 | Axes Length: 1 140 | Axes Radius: 0.10000000149011612 141 | Color: 138; 226; 52 142 | Head Length: 0.30000001192092896 143 | Head Radius: 0.10000000149011612 144 | Shaft Length: 0.5 145 | Shaft Radius: 0.05000000074505806 146 | Value: Arrow 147 | Topic: /odom 148 | Unreliable: false 149 | Value: true 150 | - Alpha: 1 151 | Autocompute Intensity Bounds: true 152 | Autocompute Value Bounds: 153 | Max Value: 10 154 | Min Value: -10 155 | Value: true 156 | Axis: Z 157 | Channel Name: intensity 158 | Class: rviz/LaserScan 159 | Color: 255; 255; 255 160 | Color Transformer: Intensity 161 | Decay Time: 0 162 | Enabled: true 163 | Invert Rainbow: false 164 | Max Color: 255; 255; 255 165 | Min Color: 0; 0; 0 166 | Name: LaserScan 167 | Position Transformer: XYZ 168 | Queue Size: 10 169 | Selectable: true 170 | Size (Pixels): 3 171 | Size (m): 0.029999999329447746 172 | Style: Flat Squares 173 | Topic: /scan 174 | Unreliable: false 175 | Use Fixed Frame: true 176 | Use rainbow: true 177 | Value: true 178 | - Class: rviz/Camera 179 | Enabled: true 180 | Image Rendering: background and overlay 181 | Image Topic: /camera/image_raw 182 | Name: Camera 183 | Overlay Alpha: 0.5 184 | Queue Size: 2 185 | Transport Hint: raw 186 | Unreliable: false 187 | Value: true 188 | Visibility: 189 | Grid: true 190 | LaserScan: true 191 | Odometry: true 192 | RobotModel: true 193 | Value: true 194 | Zoom Factor: 1 195 | Enabled: true 196 | Global Options: 197 | Background Color: 48; 48; 48 198 | Default Light: true 199 | Fixed Frame: odom 200 | Frame Rate: 30 201 | Name: root 202 | Tools: 203 | - Class: rviz/Interact 204 | Hide Inactive Objects: true 205 | - Class: rviz/MoveCamera 206 | - Class: rviz/Select 207 | - Class: rviz/FocusCamera 208 | - Class: rviz/Measure 209 | - Class: rviz/SetInitialPose 210 | Theta std deviation: 0.2617993950843811 211 | Topic: /initialpose 212 | X std deviation: 0.5 213 | Y std deviation: 0.5 214 | - Class: rviz/SetGoal 215 | Topic: /move_base_simple/goal 216 | - Class: rviz/PublishPoint 217 | Single click: true 218 | Topic: /clicked_point 219 | Value: true 220 | Views: 221 | Current: 222 | Class: rviz/Orbit 223 | Distance: 10.361392974853516 224 | Enable Stereo Rendering: 225 | Stereo Eye Separation: 0.05999999865889549 226 | Stereo Focal Distance: 1 227 | Swap Stereo Eyes: false 228 | Value: false 229 | Field of View: 0.7853981852531433 230 | Focal Point: 231 | X: 0.31483107805252075 232 | Y: -1.1162339448928833 233 | Z: -2.3101916313171387 234 | Focal Shape Fixed Size: true 235 | Focal Shape Size: 0.05000000074505806 236 | Invert Z Axis: false 237 | Name: Current View 238 | Near Clip Distance: 0.009999999776482582 239 | Pitch: 1.2253975868225098 240 | Target Frame: 241 | Yaw: 3.12357759475708 242 | Saved: ~ 243 | Window Geometry: 244 | Camera: 245 | collapsed: false 246 | Displays: 247 | collapsed: false 248 | Height: 846 249 | Hide Left Dock: false 250 | Hide Right Dock: true 251 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ee000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000231000000bc0000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 252 | Selection: 253 | collapsed: false 254 | Time: 255 | collapsed: false 256 | Tool Properties: 257 | collapsed: false 258 | Views: 259 | collapsed: true 260 | Width: 1200 261 | X: 606 262 | Y: 100 263 | -------------------------------------------------------------------------------- /src/robot/launch/robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/robot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot 4 | 0.0.0 5 | The robot package 6 | 7 | 8 | 9 | 10 | thy 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /src/robot/urdf/car.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/robot/urdf/car_base.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | Gazebo/Yellow 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | Gazebo/Red 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | Gazebo/Red 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | -------------------------------------------------------------------------------- /src/robot/urdf/car_camera.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | Gazebo/Blue 37 | 38 | 39 | 40 | 41 | 42 | 43 | 30.0 44 | 45 | 46 | 1.3962634 47 | 48 | 1280 49 | 720 50 | R8G8B8 51 | 52 | 53 | 0.02 54 | 300 55 | 56 | 57 | gaussian 58 | 0.0 59 | 0.007 60 | 61 | 62 | 63 | 64 | true 65 | 0.0 66 | /camera 67 | image_raw 68 | camera_info 69 | camera 70 | 0.07 71 | 0.0 72 | 0.0 73 | 0.0 74 | 0.0 75 | 0.0 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /src/robot/urdf/car_laser.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | Gazebo/White 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | Gazebo/Black 78 | 79 | 80 | 81 | 82 | 83 | 84 | 0 0 0 0 0 0 85 | true 86 | 10 87 | 88 | 89 | 90 | 360 91 | 1 92 | -3.14159265 93 | 3.14159265 94 | 95 | 96 | 97 | 0.1 98 | 10.0 99 | 0.01 100 | 101 | 102 | gaussian 103 | 0.0 104 | 0.01 105 | 106 | 107 | 108 | /scan 109 | laser 110 | 111 | 112 | 113 | 114 | 115 | -------------------------------------------------------------------------------- /src/robot/urdf/car_move.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | transmission_interface/SimpleTransmission 8 | 9 | hardware_interface/VelocityJointInterface 10 | 11 | 12 | hardware_interface/VelocityJointInterface 13 | 1 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | Debug 26 | true 27 | / 28 | 1 29 | true 30 | true 31 | 100.0 32 | true 33 | left_wheel2base_link 34 | right_wheel2base_link 35 | ${base_link_radius * 2} 36 | ${wheel_radius * 2} 37 | 1 38 | 50 39 | 1.8 40 | cmd_vel 41 | odom 42 | odom 43 | base_footprint 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /src/robot/urdf/head.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /基于ROS系统的移动扫地机器人.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/scybd/Sweeping-Robot/3c35cece54b061cbc3345794e1664cd715e1c429/基于ROS系统的移动扫地机器人.pdf --------------------------------------------------------------------------------