zcy
/
turtlebot3_cleaner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit

master
zhangrelay 2022-03-14 20:24:35 +08:00
commit 11fff5f14a
57 changed files with 5806 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored Normal file
View File

@ -0,0 +1,4 @@
/CMakeLists.txt
*.pyc
*.o
.vscode

105
README.md Normal file
View File

@ -0,0 +1,105 @@
# 自主清洁机器人仿真
适用于**Noetic**，版权属于原作者，此处引用供学生毕业设计参考。
## 效果展示视频
[【bilibili】](https://www.bilibili.com/video/BV17z4y1D7Wj)
全覆盖路径规划
[![result](img/result.png)](https://www.bilibili.com/video/BV17z4y1D7Wj)
自主探索建图
[![exploration](img/exploration.png)](https://www.bilibili.com/video/BV17z4y1D7Wj)
## 安装依赖
```bash
sudo apt install ros-${ROS_DISTRO}-turtlebot3 ros-${ROS_DISTRO}-navigation ros-${ROS_DISTRO}-dwa-local-planner ros-${ROS_DISTRO}-slam-karto
```
noetic:
```
sudo apt install ros-noetic-turtlebot3 ros-noetic-navigation ros-noetic-dwa-local-planner ros-noetic-slam-karto
```
## 使用方法
### 自主探索
**Ubuntu 18.04+melodic 上测试通过。Ubuntu 16.04请参看[master](https://github.com/mywisdomfly/Clean-robot-turtlebot3/tree/master)分支**
补充：Ubuntu 20.04+noetic上完全可以。
explore中为自动探索的包，这里用的karto slam，可以搭配其他的slam算法
自主探索建图示例启动方法
```bash
export TURTLEBOT3_MODEL=burger
roslaunch clean_robot auto_slam.launch
```
### 自主探索
清扫启动方法
```bash
export TURTLEBOT3_MODEL=burger
roslaunch clean_robot clean_work.launch
```
### 手动导航建图
```bash
export TURTLEBOT3_MODEL=burger
roslaunch clean_robot nav_slam.launch
```
## 系统结构
![structure](img/structure.png)
## 文件结构
`clean_robot`包中
`CleaningPathPlanner.cpp/h`为路径规划核心程序，
`next_goal.cpp`为发布下一个目标点的源程序，
`PathPlanningNode.cpp`为对路径规划的封装。
整个包会形成两个节点`path_planning_node`与`next_goal`分表示全覆盖路径规划器与目标点发送程序。
Launch文件中
`clean_work.launch`表示启动清洁工作，
`auto_slam.launch `表示全自主探索建图（全自动不需要人干预）
`nav_slam.launch`表示导航建图（也可以使用键盘控制建图），
`gazebo.launch`表示启动仿真环境，
`move_base.launch`表示机器人的导航栈配置，
`amcl.launch`表示粒子滤波定位系统，
`turtlebot3_navigation.launch`，表示机器人导航系统
`Config` 为路径规划所使用参数的yaml文件，
`param`为move_base所采用的参数文件，
`maps`表示地图文件，worlds为仿真环境。
## 算法思想
### 全覆盖路径规划
将原有的栅格地图无障碍物区域按照机器人大小进行分割，分割后直接对所有分割区域进行遍历即可。（按照一定规则直接进行遍历，不能保证路径最优，可以实现相对较优，完成基本功能）。
1. 如果机器人前方已经被覆盖过了或者是障碍,那么左转或者右转旋转180度接着走，实现S型走，如图5所示。
2. 如果机器人的周围都被覆盖过了，以当前机器人的位置为起点，未清洁区域作为目标点，用A*算法找出路径。重复1，2
3. 如果没有未清洁区域，算法结束
![fcpp](img/fcpp.png)
### exploration
考虑到真正的清洁机器人不可能让用户操作建图，因此尝试使用自主探索建图。采用开源的`explore_lite`包实现自主探索建图，其思路为找到可以达到的地图中的位置区域的边界中点作为导航的目标点。该方法在封闭环境中十分迅速！
## 参考资源
[CleaningRobot](https://github.com/peterWon/CleaningRobot)
[SLAM-Clean-Robot-Path-Coverage-in-ROS](https://github.com/hjr553199215/SLAM-Clean-Robot-Path-Coverage-in-ROS)
[polygon_coverage_planning](https://github.com/ethz-asl/polygon_coverage_planning)
[full_coverage_path_planner](https://github.com/nobleo/full_coverage_path_planner)

228
clean_robot/CMakeLists.txt Normal file
View File

@ -0,0 +1,228 @@
cmake_minimum_required(VERSION 3.0.2)
project(clean_robot)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
actionlib
costmap_2d
geometry_msgs
move_base_msgs
nav_msgs
roscpp
tf
)
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
find_package(catkin REQUIRED
COMPONENTS
angles
costmap_2d
geometry_msgs
nav_msgs
roscpp
tf
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# geometry_msgs# move_base_msgs# nav_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES clean_robot
CATKIN_DEPENDS actionlib costmap_2d geometry_msgs move_base_msgs nav_msgs roscpp tf
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/clean_robot.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/clean_robot_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_clean_robot.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
add_executable(path_planning_node src/PathPlanningNode.cpp src/CleaningPathPlanner.cpp )
target_link_libraries(path_planning_node ${catkin_LIBRARIES} ${OpenCV_LIBS})
add_executable(next_goal src/next_goal.cpp)
target_link_libraries(next_goal
${catkin_LIBRARIES}
)

15
clean_robot/config/cleaning_costmap_params.yaml Normal file
View File

@ -0,0 +1,15 @@
cleaning_costmap:
global_frame: map
robot_base_frame: base_footprint
update_frequency: 1.0
publish_frequency: 1.0
static_map: true
rolling_window: false
resolution: 0.05
transform_tolerance: 1.0
inflation_radius: 0.15
map_type: costmap
cleaning_plan_nodehandle:
size_of_cell: 3
grid_covered_value: 200

13
clean_robot/config/costmap_common_params.yaml Normal file
View File

@ -0,0 +1,13 @@
obstacle_range: 2.5 #2.5
raytrace_range: 3.0 #3.0
#footprint: [[0.175, 0.175], [0.175, -0.175], [-0.175, -0.175], [-0.175, 0.175]]
#footprint_inflation: 0.01
robot_radius: 0.175 #0.175
max_obstacle_height: 0.6
min_obstacle_height: 0.0
observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
# if set to false,then obstacle layer take unknow space to be free.
obstacle_layer:
track_unknown_space: true

98
clean_robot/include/CleaningPathPlanner.h Normal file
View File

@ -0,0 +1,98 @@
/***
* @brief: cleaning robot path planning
* @author: Wang
* @date: 20170702
***/
#ifndef CLEANINGPATHPLANNING_H
#define CLEANINGPATHPLANNING_H
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <vector>
#include <string>
#include "tf/tf.h"
#include "tf/transform_listener.h"
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
using namespace cv;
using namespace std;
constexpr double PI =3.14159;
struct cellIndex
{
int row;
int col;
double theta; //{0, 45,90,135,180,225,270,315} 角度信息 hjr 注
};
/*************************************************
*
* 读取栅格地图并根据占据信息获取其对应的空闲（可行走）空间，
* 按照遍历算法规划行走路线。
*
* **********************************************/
class CleaningPathPlanning
{
public:
//CleaningPathPlanning() = delete;
CleaningPathPlanning(costmap_2d::Costmap2DROS *costmap2d_ros);
vector<geometry_msgs::PoseStamped> GetPathInROS();
vector<geometry_msgs::PoseStamped> GetBorderTrackingPathInROS();
void SetCoveredGrid(double wx,double wy);
int GetSizeOfCell(){return this->SIZE_OF_CELL;}
//for visualization
void PublishCoveragePath();
void PublishGrid();
private:
//helper functions.
bool initializeMats();
bool initializeCoveredGrid();
void getCellMatAndFreeSpace(Mat srcImg, Mat &cellMat,vector<cellIndex> &freeSpaceVec);
void initializeNeuralMat(Mat cellMat, Mat neuralizedMat);
void writeResult(Mat resultmat,vector<cellIndex> pathVec);
void writeResult(cv::Mat resultmat,std::vector<cv::Point2i> pathVec);
void mainPlanningLoop();
double distance(Point2i pta,Point2i ptb);
bool findElement(vector<cv::Point2i> pointsVec,cv::Point2i pt, int&index);
void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
bool cellContainsPoint(cv::Point2i pt,cellIndex cell);
void GetBorderTrackingPathInCV(vector<cv::Point2i>&resultVec);
vector<cellIndex> GetPathInCV();
bool initialized_;
Mat srcMap_;
Mat cellMat_;
Mat neuralizedMat_;
vector<cellIndex> freeSpaceVec_;
vector<cellIndex> pathVec_;
vector<geometry_msgs::PoseStamped> pathVecInROS_;
double resolution_;
ros::Publisher plan_pub_;
ros::Publisher grid_pub_;
nav_msgs::OccupancyGrid covered_path_grid_;
//tf::TransformListener &tf_;
tf::Stamped<tf::Pose> initPose_;
costmap_2d::Costmap2D* costmap2d_;
costmap_2d::Costmap2DROS* costmap2d_ros_;
int SIZE_OF_CELL; //must be odd number.
int GRID_COVERED_VALUE;
};
#endif // CLEANINGPATHPLANNING_H

46
clean_robot/launch/amcl.launch Normal file
View File

@ -0,0 +1,46 @@
<launch>
<!-- Arguments -->
<arg name="scan_topic" default="scan"/>
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>
<!-- AMCL -->
<node pkg="amcl" type="amcl" name="amcl">
<param name="min_particles" value="500"/>
<param name="max_particles" value="3000"/>
<param name="kld_err" value="0.02"/>
<param name="update_min_d" value="0.20"/>
<param name="update_min_a" value="0.20"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="1"/>
<param name="recovery_alpha_slow" value="0.00"/>
<param name="recovery_alpha_fast" value="0.00"/>
<param name="initial_pose_x" value="$(arg initial_pose_x)"/>
<param name="initial_pose_y" value="$(arg initial_pose_y)"/>
<param name="initial_pose_a" value="$(arg initial_pose_a)"/>
<param name="gui_publish_rate" value="50.0"/>
<remap from="scan" to="$(arg scan_topic)"/>
<param name="laser_max_range" value="3.5"/>
<param name="laser_max_beams" value="180"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha1" value="0.1"/>
<param name="odom_alpha2" value="0.1"/>
<param name="odom_alpha3" value="0.1"/>
<param name="odom_alpha4" value="0.1"/>
<param name="odom_frame_id" value="odom"/>
<param name="base_frame_id" value="base_footprint"/>
</node>
</launch>

45
clean_robot/launch/auto_slam.launch Normal file
View File

@ -0,0 +1,45 @@
<?xml version="1.0"?>
<launch>
<!-- <arg name="slam_methods" default="karto" doc="slam type [gmapping, cartographer, hector, karto, frontier_exploration]"/> -->
<arg name="model" value="burger" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="configuration_basename" default="turtlebot3_lds_2d.lua"/>
<arg name="open_rviz" default="true"/>
<include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
<arg name="model" value="$(arg model)" />
</include>
<!-- 启动仿真环境 -->
<include file="$(find clean_robot)/launch/gazebo.launch"/>
<!-- SLAM: Gmapping, Cartographer, Hector, Karto, Frontier_exploration, RTAB-Map -->
<include file="$(find turtlebot3_slam)/launch/turtlebot3_karto.launch">
<arg name="model" value="$(arg model)"/>
<arg name="configuration_basename" value="$(arg configuration_basename)"/>
</include>
<!-- 启动 rviz 的标签 -->
<group if="$(arg open_rviz)">
<node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find clean_robot)/rviz/turtlebot3_karto.rviz"/>
</group>
<!-- 通过导航来实现自动建图 -->
<include file="$(find clean_robot)/launch/move_base.launch">
<arg name="model" value="$(arg model)" />
</include>
<!-- 自动探索 -->
<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
<param name="robot_base_frame" value="base_link"/>
<param name="costmap_topic" value="map"/>
<param name="costmap_updates_topic" value="map_updates"/>
<param name="visualize" value="true"/>
<param name="planner_frequency" value="0.33"/>
<param name="progress_timeout" value="50.0"/>
<param name="potential_scale" value="3.0"/>
<param name="orientation_scale" value="0.0"/>
<param name="gain_scale" value="1.0"/>
<param name="transform_tolerance" value="0.3"/>
<param name="min_frontier_size" value="0.6"/>
</node>
<!-- 键盘操作 -->
<!-- <node pkg="turtlebot3_teleop" type="turtlebot3_teleop_key" name="turtlebot3_teleop_key" launch-prefix="xterm -e" output="screen"/> -->
</launch>

24
clean_robot/launch/clean_work.launch Normal file
View File

@ -0,0 +1,24 @@
<?xml version="1.0"?>
<launch>
<arg name="model" value="burger" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="configuration_basename" default="turtlebot3_lds_2d.lua"/>
<arg name="open_rviz" default="true"/>
<!-- 启动仿真环境 -->
<include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
<arg name="model" value="$(arg model)" />
</include>
<include file="$(find clean_robot)/launch/gazebo.launch"/>
<!-- 启动导航系统，用作目标点路径规划，实现躲避小障碍物-->
<include file="$(find clean_robot)/launch/turtlebot3_navigation.launch">
<arg name="model" value="$(arg model)" />
</include>
<!-- 清扫路径规划器 -->
<node pkg="clean_robot" type="path_planning_node" respawn="false" name="path_planning_node" output="screen" clear_params="true">
<rosparam file="$(find clean_robot)/config/costmap_common_params.yaml" command="load" ns="cleaning_costmap" />
<rosparam file="$(find clean_robot)/config/cleaning_costmap_params.yaml" command="load" />
</node>
<!-- 根据清扫的路径向导航系统发送目标点位 -->
<!-- 设定距离当前目标点多进时候发布下一个目标点 -->
<param name="/NextGoal/tolerance_goal" value="0.25" />
<node pkg="clean_robot" type="next_goal" respawn="true" name="next_goal" output="screen" />
</launch>

19
clean_robot/launch/gazebo.launch Normal file
View File

@ -0,0 +1,19 @@
<launch>
<arg name="model" default="burger" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="x_pos" default="0.0"/>
<arg name="y_pos" default="0.0"/>
<arg name="z_pos" default="0.0"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find clean_robot)/worlds/clean_room.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
</include>
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
<node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
</launch>

21
clean_robot/launch/move_base.launch Normal file
View File

@ -0,0 +1,21 @@
<launch>
<!-- Arguments -->
<arg name="model" default="burger" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="cmd_vel_topic" default="/cmd_vel" />
<arg name="odom_topic" default="odom" />
<arg name="move_forward_only" default="false"/>
<!-- move_base -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
<rosparam file="$(find clean_robot)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
<rosparam file="$(find clean_robot)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
<rosparam file="$(find clean_robot)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find clean_robot)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find clean_robot)/param/move_base_params.yaml" command="load" />
<rosparam file="$(find clean_robot)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
<remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
</node>
</launch>

30
clean_robot/launch/nav_slam.launch Normal file
View File

@ -0,0 +1,30 @@
<?xml version="1.0"?>
<launch>
<!-- <arg name="slam_methods" default="karto" doc="slam type [gmapping, cartographer, hector, karto, frontier_exploration]"/> -->
<arg name="model" value="burger" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="configuration_basename" default="turtlebot3_lds_2d.lua"/>
<arg name="open_rviz" default="true"/>
<include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
<arg name="model" value="$(arg model)" />
</include>
<!-- 启动仿真环境 -->
<include file="$(find clean_robot)/launch/gazebo.launch"/>
<!-- SLAM: Gmapping, Cartographer, Hector, Karto, Frontier_exploration, RTAB-Map -->
<include file="$(find turtlebot3_slam)/launch/turtlebot3_karto.launch">
<arg name="model" value="$(arg model)"/>
<arg name="configuration_basename" value="$(arg configuration_basename)"/>
</include>
<!-- 启动 rviz 的标签 -->
<group if="$(arg open_rviz)">
<node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find clean_robot)/rviz/turtlebot3_karto.rviz"/>
</group>
<!-- 通过导航来实现自动建图 -->
<include file="$(find clean_robot)/launch/move_base.launch">
<arg name="model" value="$(arg model)" />
</include>
<node pkg="turtlebot3_teleop" type="turtlebot3_teleop_key" name="turtlebot3_teleop_key" launch-prefix="xterm -e" output="screen"/>
</launch>

26
clean_robot/launch/turtlebot3_navigation.launch Normal file
View File

@ -0,0 +1,26 @@
<launch>
<!-- Arguments -->
<arg name="model" default="burger" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="map_file" default="$(find clean_robot)/maps/clean_room.yaml"/>
<arg name="open_rviz" default="true"/>
<arg name="move_forward_only" default="false"/>
<!-- Map server -->
<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
<!-- AMCL -->
<include file="$(find clean_robot)/launch/amcl.launch"/>
<!-- move_base -->
<include file="$(find clean_robot)/launch/move_base.launch">
<arg name="model" value="$(arg model)" />
<arg name="move_forward_only" value="$(arg move_forward_only)"/>
</include>
<!-- rviz -->
<group if="$(arg open_rviz)">
<node pkg="rviz" type="rviz" name="rviz" required="true"
args="-d $(find clean_robot)/rviz/turtlebot3_navigation.rviz"/>
</group>
</launch>

BIN
clean_robot/maps/clean_room.pgm Normal file
View File

Binary file not shown.

7
clean_robot/maps/clean_room.yaml Normal file
View File

@ -0,0 +1,7 @@
image: clean_room.pgm
resolution: 0.050000
origin: [-5.159059, -3.144864, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

BIN
clean_robot/maps/clean_room_new.pgm Normal file
View File

Binary file not shown.

7
clean_robot/maps/clean_room_new.yaml Normal file
View File

@ -0,0 +1,7 @@
image: clean_room_new.pgm
resolution: 0.050000
origin: [-5.169793, -3.148376, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

80
clean_robot/package.xml Normal file
View File

@ -0,0 +1,80 @@
<?xml version="1.0"?>
<package format="2">
<name>clean_robot</name>
<version>0.0.0</version>
<description>The clean_robot package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="derek@todo.todo">derek</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/clean_robot</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>actionlib</build_depend>
<build_depend>costmap_2d</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>move_base_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>tf</build_depend>
<build_export_depend>actionlib</build_export_depend>
<build_export_depend>costmap_2d</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>move_base_msgs</build_export_depend>
<build_export_depend>nav_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>tf</build_export_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>costmap_2d</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>move_base_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>tf</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

26
clean_robot/param/base_local_planner_params.yaml Normal file
View File

@ -0,0 +1,26 @@
TrajectoryPlannerROS:
# Robot Configuration Parameters
max_vel_x: 0.18
min_vel_x: 0.08
max_vel_theta: 1.0
min_vel_theta: -1.0
min_in_place_vel_theta: 1.0
acc_lim_x: 1.0
acc_lim_y: 0.0
acc_lim_theta: 0.6
# Goal Tolerance Parameters
xy_goal_tolerance: 0.10
yaw_goal_tolerance: 0.05
# Differential-drive robot configuration
holonomic_robot: false
# Forward Simulation Parameters
sim_time: 0.8
vx_samples: 18
vtheta_samples: 20
sim_granularity: 0.05

12
clean_robot/param/costmap_common_params_burger.yaml Normal file
View File

@ -0,0 +1,12 @@
obstacle_range: 3.0
raytrace_range: 3.5
footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]]
#robot_radius: 0.105
inflation_radius: 1.0
cost_scaling_factor: 4.0
map_type: costmap
observation_sources: scan
scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

12
clean_robot/param/costmap_common_params_waffle.yaml Normal file
View File

@ -0,0 +1,12 @@
obstacle_range: 3.0
raytrace_range: 3.5
footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]]
#robot_radius: 0.17
inflation_radius: 1.0
cost_scaling_factor: 3.0
map_type: costmap
observation_sources: scan
scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

12
clean_robot/param/costmap_common_params_waffle_pi.yaml Normal file
View File

@ -0,0 +1,12 @@
obstacle_range: 3.0
raytrace_range: 3.5
footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]]
#robot_radius: 0.17
inflation_radius: 1.0
cost_scaling_factor: 3.0
map_type: costmap
observation_sources: scan
scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

47
clean_robot/param/dwa_local_planner_params_burger.yaml Normal file
View File

@ -0,0 +1,47 @@
DWAPlannerROS:
# Robot Configuration Parameters
max_vel_x: 0.22
min_vel_x: -0.22
max_vel_y: 0.0
min_vel_y: 0.0
# The velocity when robot is moving in a straight line
max_vel_trans: 0.22
min_vel_trans: 0.11
max_vel_theta: 2.75
min_vel_theta: 1.37
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
# Goal Tolerance Parametes
xy_goal_tolerance: 0.05
yaw_goal_tolerance: 0.17
latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 1.5
vx_samples: 20
vy_samples: 0
vth_samples: 40
controller_frequency: 10.0
# Trajectory Scoring Parameters
path_distance_bias: 32.0
goal_distance_bias: 20.0
occdist_scale: 0.02
forward_point_distance: 0.325
stop_time_buffer: 0.2
scaling_speed: 0.25
max_scaling_factor: 0.2
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.05
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true

47
clean_robot/param/dwa_local_planner_params_waffle.yaml Normal file
View File

@ -0,0 +1,47 @@
DWAPlannerROS:
# Robot Configuration Parameters
max_vel_x: 0.26
min_vel_x: -0.26
max_vel_y: 0.0
min_vel_y: 0.0
# The velocity when robot is moving in a straight line
max_vel_trans: 0.26
min_vel_trans: 0.13
max_vel_theta: 1.82
min_vel_theta: 0.9
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
# Goal Tolerance Parametes
xy_goal_tolerance: 0.05
yaw_goal_tolerance: 0.17
latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 2.0
vx_samples: 20
vy_samples: 0
vth_samples: 40
controller_frequency: 10.0
# Trajectory Scoring Parameters
path_distance_bias: 32.0
goal_distance_bias: 20.0
occdist_scale: 0.02
forward_point_distance: 0.325
stop_time_buffer: 0.2
scaling_speed: 0.25
max_scaling_factor: 0.2
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.05
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true

47
clean_robot/param/dwa_local_planner_params_waffle_pi.yaml Normal file
View File

@ -0,0 +1,47 @@
DWAPlannerROS:
# Robot Configuration Parameters
max_vel_x: 0.26
min_vel_x: -0.26
max_vel_y: 0.0
min_vel_y: 0.0
# The velocity when robot is moving in a straight line
max_vel_trans: 0.26
min_vel_trans: 0.13
max_vel_theta: 1.82
min_vel_theta: 0.9
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
# Goal Tolerance Parametes
xy_goal_tolerance: 0.05
yaw_goal_tolerance: 0.17
latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 2.0
vx_samples: 20
vy_samples: 0
vth_samples: 40
controller_frequency: 10.0
# Trajectory Scoring Parameters
path_distance_bias: 32.0
goal_distance_bias: 20.0
occdist_scale: 0.02
forward_point_distance: 0.325
stop_time_buffer: 0.2
scaling_speed: 0.25
max_scaling_factor: 0.2
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.05
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true

10
clean_robot/param/global_costmap_params.yaml Normal file
View File

@ -0,0 +1,10 @@
global_costmap:
global_frame: map
robot_base_frame: base_footprint
update_frequency: 10.0
publish_frequency: 10.0
transform_tolerance: 0.5
static_map: true

14
clean_robot/param/local_costmap_params.yaml Normal file
View File

@ -0,0 +1,14 @@
local_costmap:
global_frame: odom
robot_base_frame: base_footprint
update_frequency: 10.0
publish_frequency: 10.0
transform_tolerance: 0.5
static_map: false
rolling_window: true
width: 3
height: 3
resolution: 0.05

9
clean_robot/param/move_base_params.yaml Normal file
View File

@ -0,0 +1,9 @@
shutdown_costmaps: false
controller_frequency: 10.0
planner_patience: 5.0
controller_patience: 15.0
conservative_reset_dist: 3.0
planner_frequency: 5.0
oscillation_timeout: 10.0
oscillation_distance: 0.2

295
clean_robot/rviz/turtlebot3_karto.rviz Normal file
View File

@ -0,0 +1,295 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /TF1/Frames1
- /Path2
- /Map2
- /Map3
Splitter Ratio: 0.594406009
Tree Height: 776
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: LaserScan
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 100
Reference Frame: map
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_scan:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
caster_back_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
wheel_left_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheel_right_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 0; 255; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 11799
Min Color: 0; 0; 0
Min Intensity: 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.0300000012
Style: Flat Squares
Topic: /scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/Image
Enabled: false
Image Topic: /raspicam_node/image
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: compressed
Unreliable: false
Value: false
- Alpha: 0.699999988
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Billboards
Line Width: 0.0299999993
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /move_base/NavfnROS/plan
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 85; 0; 255
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Billboards
Line Width: 0.0299999993
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /move_base/DWAPlannerROS/local_plan
Unreliable: false
Value: true
- Alpha: 0.699999988
Class: rviz/Map
Color Scheme: costmap
Draw Behind: false
Enabled: false
Name: Map
Topic: /move_base/global_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: false
- Alpha: 0.699999988
Class: rviz/Map
Color Scheme: costmap
Draw Behind: false
Enabled: false
Name: Map
Topic: /move_base/local_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Angle: -1.57079637
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Scale: 136.475739
Target Frame: map
Value: TopDownOrtho (rviz)
X: -0.542091846
Y: 0.0558035932
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: true
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000020f000001900000001600ffffff000000010000010f0000035cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000430000035c000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1855
X: 65
Y: 24

432
clean_robot/rviz/turtlebot3_navigation.rviz Normal file
View File

@ -0,0 +1,432 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /TF1/Frames1
- /TF1/Tree1
- /Global Map1/Costmap1
- /Local Map1/Polygon1
- /Local Map1/Costmap1
- /Local Map1/Planner1
- /Path3
Splitter Ratio: 0.5
Tree Height: 844
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: LaserScan
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 20
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_scan:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
caster_back_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
wheel_left_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheel_right_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: false
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
{}
Update Interval: 0
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 0; 255; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 13069
Min Color: 0; 0; 0
Min Intensity: 28
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.0300000012
Style: Flat Squares
Topic: /scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/Image
Enabled: false
Image Topic: /raspicam_node/image
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: compressed
Unreliable: false
Value: false
- Alpha: 0.699999988
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 0; 0; 0
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Planner Plan
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /move_base/NavfnROS/plan
Unreliable: false
Value: true
- Class: rviz/Group
Displays:
- Alpha: 0.699999988
Class: rviz/Map
Color Scheme: costmap
Draw Behind: true
Enabled: true
Name: Costmap
Topic: /move_base/global_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 0; 0
Enabled: false
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Planner
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /move_base/NavfnROS/plan
Unreliable: false
Value: false
Enabled: true
Name: Global Map
- Class: rviz/Group
Displays:
- Alpha: 1
Class: rviz/Polygon
Color: 0; 0; 0
Enabled: true
Name: Polygon
Topic: /move_base/local_costmap/footprint
Unreliable: false
Value: true
- Alpha: 0.699999988
Class: rviz/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Costmap
Topic: /move_base/local_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 255; 0
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Planner
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /move_base/DWAPlannerROS/local_plan
Unreliable: false
Value: true
Enabled: false
Name: Local Map
- Alpha: 1
Arrow Length: 0.0500000007
Axes Length: 0.300000012
Axes Radius: 0.00999999978
Class: rviz/PoseArray
Color: 0; 192; 0
Enabled: false
Head Length: 0.0700000003
Head Radius: 0.0299999993
Name: Amcl Particles
Shaft Length: 0.230000004
Shaft Radius: 0.00999999978
Shape: Arrow (Flat)
Topic: /particlecloud
Unreliable: false
Value: false
- Alpha: 1
Axes Length: 1
Axes Radius: 0.100000001
Class: rviz/Pose
Color: 255; 25; 0
Enabled: true
Head Length: 0.0500000007
Head Radius: 0.100000001
Name: Goal
Shaft Length: 0.0500000007
Shaft Radius: 0.0500000007
Shape: Arrow
Topic: /move_base_simple/goal
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Billboards
Line Width: 0.0299999993
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /path_planning_node/cleaning_plan_nodehandle/cleaning_path
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 170; 0; 0
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Billboards
Line Width: 0.349999994
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /clean_robot/passed_path
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 255; 127
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /move_base/DWAPlannerROS/local_plan
Unreliable: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/MoveCamera
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/Select
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/Measure
Value: true
Views:
Current:
Angle: -1.57079637
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Scale: 156.50177
Target Frame: <Fixed Frame>
Value: TopDownOrtho (rviz)
X: -2.20532393
Y: -1.97824216
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: true
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a000003dafc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003da000000d600fffffffb0000000a0049006d0061006700650000000317000000cc0000001600fffffffb0000000a0049006d0061006700650000000330000000ce0000000000000000000000010000010f000003a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003a0000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf000003da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1855
X: 65
Y: 24

724
clean_robot/src/CleaningPathPlanner.cpp Normal file
View File

@ -0,0 +1,724 @@
#include "CleaningPathPlanner.h"
#include <costmap_2d/cost_values.h>
CleaningPathPlanning::CleaningPathPlanning(costmap_2d::Costmap2DROS *costmap2d_ros)
{
//temp solution.
costmap2d_ros_ = costmap2d_ros;
//costmap2d_ros_->updateMap();
costmap2d_ = costmap2d_ros->getCostmap();
ros::NodeHandle private_nh("~/cleaning_plan_nodehandle");
plan_pub_ = private_nh.advertise<nav_msgs::Path>("cleaning_path", 1);
grid_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("covered_grid", 1);
string sizeOfCellString, coveredValueStr;
SIZE_OF_CELL = 3;
if (private_nh.searchParam("size_of_cell", sizeOfCellString)) //搜索参数,根据名称"size of cell"搜索参数，将对应名称下的参数值赋给sizeOfCellString.
private_nh.param("size_of_cell", SIZE_OF_CELL, 3); //设置机器人占据n*n的栅格，决定规划的稀疏
GRID_COVERED_VALUE = 0;
if (private_nh.searchParam("grid_covered_value", coveredValueStr))
private_nh.param("grid_covered_value", GRID_COVERED_VALUE, 0);
int sizex = costmap2d_->getSizeInCellsX(); //获取地图尺寸
int sizey = costmap2d_->getSizeInCellsY();
cout << "The size of map is " << sizex << " " << sizey << endl;
resolution_ = costmap2d_->getResolution(); //分辨率
srcMap_ = Mat(sizey, sizex, CV_8U);
for (int r = 0; r < sizey; r++)
{
for (int c = 0; c < sizex; c++)
{
srcMap_.at<uchar>(r, c) = costmap2d_->getCost(c, sizey - r - 1); //??sizey-r-1 caution: costmap's origin is at left bottom ,while opencv's pic's origin is at left-top.
//getCost（）:获取代价值
}
}
initializeMats();
initializeCoveredGrid();
//imshow("debugMapImage",srcMap_);
//imshow("debugCellMatImage",cellMat_);
//waitKey(0);
//imwrite("debug_srcmap.jpg",srcMap_);
if (!srcMap_.empty())
initialized_ = true; //这句话説明srcMap_里面得有东西才能说明初始化成功。
else
initialized_ = false;
}
//规划路径
vector<geometry_msgs::PoseStamped> CleaningPathPlanning::GetPathInROS()
{
// vector<geometry_msgs::PoseStamped> resultVec;
if (!pathVecInROS_.empty())
pathVecInROS_.clear(); //清空操作
geometry_msgs::PoseStamped posestamped;
geometry_msgs::Pose pose;
vector<cellIndex> cellvec;
cellvec = GetPathInCV();
/*trasnsform*/
vector<cellIndex>::iterator iter; //cellIndex里面存放的是行，列以及角度信息。
int sizey = cellMat_.rows;
for (iter = cellvec.begin(); iter != cellvec.end(); iter++)
{
costmap2d_->mapToWorld((*iter).col * SIZE_OF_CELL + SIZE_OF_CELL / 2, (sizey - (*iter).row - 1) * SIZE_OF_CELL + SIZE_OF_CELL / 2, pose.position.x, pose.position.y);
pose.orientation.w = cos((*iter).theta * PI / 180 / 2); //(sizey-(*iter).row-1)
pose.orientation.x = 0;
pose.orientation.y = 0;
pose.orientation.z = sin((*iter).theta * PI / 180 / 2);
posestamped.header.stamp = ros::Time::now();
posestamped.header.frame_id = "map";
posestamped.pose = pose;
pathVecInROS_.push_back(posestamped);
}
publishPlan(pathVecInROS_);
cout << "The path size is " << pathVecInROS_.size() << endl;
return pathVecInROS_;
}
vector<geometry_msgs::PoseStamped> CleaningPathPlanning::GetBorderTrackingPathInROS()
{
//vector<geometry_msgs::PoseStamped> resultPathInROS;
if (!pathVecInROS_.empty())
pathVecInROS_.clear();
geometry_msgs::PoseStamped posestamped;
geometry_msgs::Pose pose;
vector<cv::Point2i> resultCV;
GetBorderTrackingPathInCV(resultCV);
vector<bool> visitedVec;
cv::Point2i lastPoint = resultCV[0], curPoint;
double theta = 0.0;
visitedVec.assign(resultCV.size(), false);
int i, j, k, index;
for (i = 0; i < resultCV.size(); i++)
{
if (!visitedVec[i])
{
for (k = -SIZE_OF_CELL / 2; k < SIZE_OF_CELL / 2; k++)
{
for (j = -SIZE_OF_CELL / 2; j < SIZE_OF_CELL / 2; j++)
{
if (findElement(resultCV, Point2i(resultCV[i].x + j, resultCV[i].y + k), index))
{
visitedVec[index] = true;
curPoint = resultCV[index];
if (curPoint.x == lastPoint.x)
{
if (curPoint.y > lastPoint.y)
theta = PI / 2;
else
theta = -PI / 2;
}
else
theta = atan((curPoint.y - lastPoint.x) / (curPoint.x - lastPoint.x));
//srcMap_'s shape is same with costmap2d_,ie.. they have same resolution and size.
costmap2d_->mapToWorld(resultCV[index].x, srcMap_.rows - resultCV[index].y - 1, pose.position.x, pose.position.y);
pose.orientation.w = cos(theta * PI / 180 / 2);
pose.orientation.x = 0;
pose.orientation.y = 0;
pose.orientation.z = sin(theta * PI / 180 / 2);
posestamped.header.stamp = ros::Time::now();
posestamped.header.frame_id = "map";
posestamped.pose = pose;
pathVecInROS_.push_back(posestamped);
}
}
}
}
}
publishPlan(pathVecInROS_);
return pathVecInROS_;
}
//First make border path planning in original resolution using opencv tools.Then transform the result
//in ROS format and take the robot's shape into account.
void CleaningPathPlanning::GetBorderTrackingPathInCV(vector<cv::Point2i> &resultVec)
{
std::vector<cv::Point2i> borderPointsIndexVec; //todo:make point2i corrresponding to cellindex.
resultVec.clear();
if (srcMap_.empty())
return; //srcMap 是个啥？
cv::Point2i temppoint;
int r, c, i, j;
for (r = 0; r < srcMap_.rows; r++)
{
for (c = 0; c < srcMap_.cols; c++)
{
//ROS_INFO("The value at row %d, col %d is %d",r,c,srcMap_.at<uchar>(r,c));
if (srcMap_.at<uchar>(r, c) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
{
ROS_INFO("entered once!");
if (srcMap_.at<uchar>(r, c - 1) != srcMap_.at<uchar>(r, c + 1) && (srcMap_.at<uchar>(r, c - 1) == costmap_2d::FREE_SPACE || srcMap_.at<uchar>(r, c + 1) == costmap_2d::FREE_SPACE))
{
temppoint.x = c;
temppoint.y = r;
borderPointsIndexVec.push_back(temppoint);
}
else if (srcMap_.at<uchar>(r - 1, c) != srcMap_.at<uchar>(r + 1, c) && (srcMap_.at<uchar>(r - 1, c) == costmap_2d::FREE_SPACE || srcMap_.at<uchar>(r + 1, c) == costmap_2d::FREE_SPACE))
{
temppoint.x = c;
temppoint.y = r;
borderPointsIndexVec.push_back(temppoint);
}
}
}
}
std::cout << "The border path length is:" << borderPointsIndexVec.size() << std::endl;
std::vector<bool> visitedVec;
if (borderPointsIndexVec.empty())
return;
int minDistIndex = -1, loopCounter = 0;
double dist, minDist;
visitedVec.assign(borderPointsIndexVec.size(), false); //vector assign是vector容器下的赋值函数。
Point2i curPoint = borderPointsIndexVec[0];
visitedVec[0] = true;
while (loopCounter < borderPointsIndexVec.size())
{
minDist = 1e6;
resultVec.push_back(curPoint);
for (j = 1; j < borderPointsIndexVec.size(); j++)
{
if (!visitedVec[j])
{
dist = distance(curPoint, borderPointsIndexVec[j]);
if (dist < minDist)
{
minDist = dist;
minDistIndex = j;
}
}
}
curPoint = borderPointsIndexVec[minDistIndex];
visitedVec[minDistIndex] = true;
loopCounter++;
}
//Mat resultmat=Mat(srcMap_.rows,srcMap_.cols, CV_8UC3);;
//writeResult(resultmat,resultVec);
}
void CleaningPathPlanning::SetCoveredGrid(double wx, double wy)
{
unsigned int mx, my, index;
bool isok = costmap2d_->worldToMap(wx, wy, mx, my);
if (!isok)
{
return;
}
for (int dx = -SIZE_OF_CELL / 2; dx < SIZE_OF_CELL / 2 + 1; dx++)
{
for (int dy = -SIZE_OF_CELL / 2; dy < SIZE_OF_CELL / 2 + 1; dy++)
{
index = costmap2d_->getIndex(mx + dx, my + dy);
covered_path_grid_.data[index] = GRID_COVERED_VALUE;
}
}
}
void CleaningPathPlanning::PublishGrid()
{
if (!initialized_)
initializeCoveredGrid();
grid_pub_.publish(covered_path_grid_);
}
vector<cellIndex> CleaningPathPlanning::GetPathInCV()
{
mainPlanningLoop();
return this->pathVec_;
}
void CleaningPathPlanning::PublishCoveragePath()
{
publishPlan(this->pathVecInROS_);
}
void CleaningPathPlanning::publishPlan(const std::vector<geometry_msgs::PoseStamped> &path)
{
if (!initialized_)
{
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return;
}
//create a message for the plan
nav_msgs::Path gui_path;
gui_path.poses.resize(path.size());
gui_path.header.frame_id = "map";
gui_path.header.stamp = ros::Time::now();
// Extract the plan in world co-ordinates, we assume the path is all in the same frame
for (unsigned int i = 0; i < path.size(); i++)
{
gui_path.poses[i] = path[i];
}
plan_pub_.publish(gui_path);
}
bool CleaningPathPlanning::cellContainsPoint(Point2i pt, cellIndex cell)
{
return pt.y > cell.row * SIZE_OF_CELL && pt.y < (cell.row + 1) * SIZE_OF_CELL && pt.x > cell.col * SIZE_OF_CELL && pt.x < (cell.col + 1) * SIZE_OF_CELL;
}
bool CleaningPathPlanning::initializeMats()
{
//initialize the member variables.
if (srcMap_.empty())
return false;
getCellMatAndFreeSpace(srcMap_, cellMat_, freeSpaceVec_);
neuralizedMat_ = Mat(cellMat_.rows, cellMat_.cols, CV_32F);
//Astarmap = Mat(cellMat_.rows, cellMat_.cols, CV_32F);
initializeNeuralMat(cellMat_, neuralizedMat_);
//Astarmap = neuralizedMat_;
return true;
}
void CleaningPathPlanning::getCellMatAndFreeSpace(Mat srcImg, Mat &cellMat, vector<cellIndex> &freeSpaceVec)
{
cellMat = Mat(srcImg.rows / SIZE_OF_CELL, srcImg.cols / SIZE_OF_CELL, srcImg.type()); //cellMat是以之前规定的cell为最小单位重新划分的矩阵
freeSpaceVec.clear();
bool isFree = true;
int r = 0, c = 0, i = 0, j = 0;
for (r = 0; r < cellMat.rows; r++)
{
for (c = 0; c < cellMat.cols; c++)
{
isFree = true;
for (i = 0; i < SIZE_OF_CELL; i++)
{
for (j = 0; j < SIZE_OF_CELL; j++)
{
if (srcImg.at<uchar>(r * SIZE_OF_CELL + i, c * SIZE_OF_CELL + j) != costmap_2d::FREE_SPACE)
{
isFree = false;
i = SIZE_OF_CELL;
break;
}
}
}
if (isFree)
{
cellIndex ci;
ci.row = r;
ci.col = c;
ci.theta = 0;
freeSpaceVec.push_back(ci);
cellMat.at<uchar>(r, c) = costmap_2d::FREE_SPACE; //0
}
else
{
cellMat.at<uchar>(r, c) = costmap_2d::LETHAL_OBSTACLE;
} //254
}
}
cout << "freespace size:" << freeSpaceVec.size() << endl;
//imwrite("cellMat.jpg",cellMat);
return;
}
void CleaningPathPlanning::initializeNeuralMat(Mat cellMat, Mat neuralizedMat)
{
int i = 0, j = 0;
for (i = 0; i < neuralizedMat.rows; i++)
{
for (j = 0; j < neuralizedMat.cols; j++)
{
if (cellMat.at<uchar>(i, j) == costmap_2d::LETHAL_OBSTACLE)
neuralizedMat.at<float>(i, j) = -100000.0;
else
neuralizedMat.at<float>(i, j) = 50.0 / j; //这里的1.0/j的用意是什么？这里有没有考虑到j=0时刻的问题呢？ hjr注
}
}
return;
}
void CleaningPathPlanning::writeResult(Mat resultmat, vector<cellIndex> pathVec)
{
int i = 0, j = 0;
Point initpoint = Point(pathVec[0].col * SIZE_OF_CELL + SIZE_OF_CELL / 2, pathVec[0].row * SIZE_OF_CELL + SIZE_OF_CELL / 2); //这里是取中心位置的意思吗？
for (i = 1; i < pathVec.size(); i++)
{
Point cupoint = Point(pathVec[i].col * SIZE_OF_CELL + SIZE_OF_CELL / 2, pathVec[i].row * SIZE_OF_CELL + SIZE_OF_CELL / 2);
if (sqrt((initpoint.x - cupoint.x) * (initpoint.x - cupoint.x) + (initpoint.y - cupoint.y) * (initpoint.y - cupoint.y)) > 2)
{
line(resultmat, initpoint, cupoint, Scalar(0, 255, 0), 1, 8); //line就是划线函数
}
else
line(resultmat, initpoint, cupoint, Scalar(0, 0, 255), 1);
initpoint = cupoint;
cout << "The point of step " << i << " is: " << pathVec[i].row << " " << pathVec[i].col << endl;
/*resultMat.at<Vec3b>(pathVec[i].row*SIZE_OF_CELL,pathVec[i].col*SIZE_OF_CELL)[0] = 0;
resultMat.at<Vec3b>(pathVec[i].row*SIZE_OF_CELL,pathVec[i].col*SIZE_OF_CELL)[1] = 0;
resultMat.at<Vec3b>(pathVec[i].row*SIZE_OF_CELL,pathVec[i].col*SIZE_OF_CELL)[2] = 255;*/
}
// imshow("resultMat",resultmat);
// waitKey(0);
imwrite("reaultMat.jpg", resultmat);
}
void CleaningPathPlanning::writeResult(Mat resultmat, vector<cv::Point2i> pathVec)
{
int i = 0, j = 0;
Point initpoint = pathVec[0];
for (i = 1; i < pathVec.size(); i++)
{
Point cupoint = pathVec[i];
line(resultmat, initpoint, cupoint, Scalar(0, 0, 255), 1);
initpoint = cupoint;
//std::cout<<"X: "<<cupoint.x<<","<<"Y:"<<cupoint<<std::endl;
}
// imshow("resultMat",resultmat);
// waitKey(0);
}
void CleaningPathPlanning::mainPlanningLoop()
{
cellIndex initPoint, nextPoint, currentPoint;
// initPoint.row = cellMat_.rows/2; //initPoint to be made interface.
// initPoint.col = cellMat_.cols/2;
initPoint.theta = 90;
bool isok = costmap2d_ros_->getRobotPose(initPose_);
if (!isok)
{
ROS_INFO("Failed to get robot location! Please check where goes wrong!");
return;
}
//initPoint.row = initPose_.getOrigin().y()
unsigned int mx, my;
double wx = initPose_.pose.position.x; //获取原点的x坐标
double wy = initPose_.pose.position.y;
//geometry_msgs::PoseStamped current_position;
//tf::poseStampedTFToMsg(global_pose, current_position);
bool getmapcoor = costmap2d_->worldToMap(wx, wy, mx, my);
if (!getmapcoor)
{
ROS_INFO("Failed to get robot location in map! Please check where goes wrong!");
return;
}
initPoint.row = cellMat_.rows - my / SIZE_OF_CELL - 1; //再研究一下这个行列之间的转换问题。
initPoint.col = mx / SIZE_OF_CELL; //这里貌似不光有行列转换
currentPoint = initPoint; //将初始化的点赋予给当前点。
pathVec_.clear();
pathVec_.push_back(initPoint);
float initTheta = initPoint.theta; //initial orientation
const float c_0 = 50; //这貌似是后面算法中要用到的参数。0.001
float e = 0.0, v = 0.0, v_1 = 0.0, deltaTheta = 0.0, lasttheta = initTheta, PI = 3.14159;
int fx;
vector<float> thetaVec = {0, 45, 90, 135, 180, 225, 270, 315};
//vector<float> thetaVec = {0,315,270,225,180,135,90,45};
//the main planning loop
//while(freeSpaceVec_.size()>0)
for (int loop = 0; loop < 9000; loop++)
{
//erase current point from free space first.
vector<cellIndex>::iterator it;
/*
for(it=freeSpaceVec_.begin();it!=freeSpaceVec_.end();)
{
if((*it).row==nextPoint.row && (*it).col==nextPoint.col)
{it = freeSpaceVec_.erase(it);continue;}
it++;
}
*/
//compute neiborhood's activities
//hjr注：目前我认为这里进行的是有关方向上的抉择。
int maxIndex = 0; //目前尚不清楚这两个参数最后是干啥的。
float max_v = -300;
neuralizedMat_.at<float>(currentPoint.row, currentPoint.col) = -250.0;
lasttheta = currentPoint.theta;
for (int id = 0; id < 8; id++)
{
deltaTheta = max(thetaVec[id], lasttheta) - min(thetaVec[id], lasttheta);
if (deltaTheta > 180)
deltaTheta = 360 - deltaTheta;
e = 1 - abs(deltaTheta) / 180; //角度参数？
switch (id)
{
case 0:
if (currentPoint.col == neuralizedMat_.cols - 1)
{
v = -100000;
break;
} //处于边界？
v = neuralizedMat_.at<float>(currentPoint.row, currentPoint.col + 1) + c_0 * e;
break;
case 1:
if (currentPoint.col == neuralizedMat_.cols - 1 || currentPoint.row == 0)
{
v = -100000;
break;
}
v = neuralizedMat_.at<float>(currentPoint.row - 1, currentPoint.col + 1) + c_0 * e - 200;
break;
case 2:
if (currentPoint.row == 0)
{
v = -100000;
break;
}
v = neuralizedMat_.at<float>(currentPoint.row - 1, currentPoint.col) + c_0 * e;
break;
case 3:
if (currentPoint.col == 0 || currentPoint.row == 0)
{
v = -100000;
break;
}
v = neuralizedMat_.at<float>(currentPoint.row - 1, currentPoint.col - 1) + c_0 * e - 200;
break;
case 4:
if (currentPoint.col == 0)
{
v = -100000;
break;
}
v = neuralizedMat_.at<float>(currentPoint.row, currentPoint.col - 1) + c_0 * e;
break;
case 5:
if (currentPoint.col == 0 || currentPoint.row == neuralizedMat_.rows - 1)
{
v = -100000;
break;
}
v = neuralizedMat_.at<float>(currentPoint.row + 1, currentPoint.col - 1) + c_0 * e - 200;
break;
case 6:
if (currentPoint.row == neuralizedMat_.rows - 1)
{
v = -100000;
break;
}
v = neuralizedMat_.at<float>(currentPoint.row + 1, currentPoint.col) + c_0 * e;
break;
case 7:
if (currentPoint.col == neuralizedMat_.cols - 1 || currentPoint.row == neuralizedMat_.rows - 1)
{
v = -100000;
break;
}
v = neuralizedMat_.at<float>(currentPoint.row + 1, currentPoint.col + 1) + c_0 * e - 200;
break;
default:
break;
}
if (v > max_v)
{
max_v = v;
maxIndex = id;
}
if (v == max_v && id > maxIndex)
{
max_v = v;
maxIndex = id;
}
}
if (max_v <= 0) //接下来应该是在处理距离上的关系。
{
float dist = 0.0, min_dist = 100000000;
//vector<cellIndex>::iterator min_iter;
int ii = 0, min_index = -1;
for (it = freeSpaceVec_.begin(); it != freeSpaceVec_.end(); it++)
{
if (neuralizedMat_.at<float>((*it).row, (*it).col) > 0)
{
if (Boundingjudge((*it).row, (*it).col)) //周围是否存在-250的点
{
dist = sqrt((currentPoint.row - (*it).row) * (currentPoint.row - (*it).row) + (currentPoint.col - (*it).col) * (currentPoint.col - (*it).col));
if (dist < min_dist)
{
min_dist = dist;
min_index = ii;
}
}
}
ii++;
}
//if(min_dist==0 || min_index == -1)
//{break;}
//else
if (min_index != -1 && min_dist != 100000000)
{
cout << "next point index: " << min_index << endl;
cout << "distance: " << min_dist << endl;
nextPoint = freeSpaceVec_[min_index];
currentPoint = nextPoint;
pathVec_.push_back(nextPoint);
continue;
}
else //产生了自锁现象
{
ROS_INFO("The program has been dead because of the self-locking");
ROS_INFO("The program has gone through %ld steps", pathVec_.size());
break;
}
}
//hjr自己编写部分------------------------------------------------------------------------------
//next point.
switch (maxIndex)
{
case 0:
nextPoint.row = currentPoint.row;
nextPoint.col = currentPoint.col + 1;
break;
case 1:
nextPoint.row = currentPoint.row - 1;
nextPoint.col = currentPoint.col + 1;
break;
case 2:
nextPoint.row = currentPoint.row - 1;
nextPoint.col = currentPoint.col;
break;
case 3:
nextPoint.row = currentPoint.row - 1;
nextPoint.col = currentPoint.col - 1;
break;
case 4:
nextPoint.row = currentPoint.row;
nextPoint.col = currentPoint.col - 1;
break;
case 5:
nextPoint.row = currentPoint.row + 1;
nextPoint.col = currentPoint.col - 1;
break;
case 6:
nextPoint.row = currentPoint.row + 1;
nextPoint.col = currentPoint.col;
break;
case 7:
nextPoint.row = currentPoint.row + 1;
nextPoint.col = currentPoint.col + 1;
break;
default:
break;
}
nextPoint.theta = thetaVec[maxIndex];
currentPoint = nextPoint;
pathVec_.push_back(nextPoint);
//for(it=freeSpaceVec_.begin();it!=freeSpaceVec_.end();)
// {
// if((*it).row==nextPoint.row && (*it).col==nextPoint.col)
// {it = freeSpaceVec_.erase(it);continue;}
// it++;
// }
}
// Mat resultMat = Mat(srcMap_.rows, srcMap_.cols, CV_8UC3);
// writeResult(resultMat, pathVec_);
}
double CleaningPathPlanning::distance(Point2i pta, Point2i ptb)
{
return sqrt((pta.x - ptb.x) * (pta.x - ptb.x) + (pta.y - ptb.y) * (pta.y - ptb.y));
}
bool CleaningPathPlanning::findElement(vector<Point2i> pointsVec, Point2i pt, int &index)
{
for (int i = 0; i < pointsVec.size(); i++)
{
if (pointsVec[i].x == pt.x && pointsVec[i].y == pt.y)
{
index = i;
return true;
}
}
index = -1;
return false;
}
bool CleaningPathPlanning::initializeCoveredGrid() //在这里我对CoverGrid的理解为覆盖栅格。
{
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap2d_->getMutex()));
double resolution = costmap2d_->getResolution(); //分辨率
covered_path_grid_.header.frame_id = "map"; //covered_path_grid_是costmap库中的占据栅格地图消息。
covered_path_grid_.header.stamp = ros::Time::now();
covered_path_grid_.info.resolution = resolution;
covered_path_grid_.info.width = costmap2d_->getSizeInCellsX();
covered_path_grid_.info.height = costmap2d_->getSizeInCellsY();
double wx, wy;
costmap2d_->mapToWorld(0, 0, wx, wy); //从地图坐标系转换至世界坐标系。
covered_path_grid_.info.origin.position.x = wx - resolution / 2;
covered_path_grid_.info.origin.position.y = wy - resolution / 2;
covered_path_grid_.info.origin.position.z = 0.0;
covered_path_grid_.info.origin.orientation.w = 1.0;
covered_path_grid_.data.resize(covered_path_grid_.info.width * covered_path_grid_.info.height); //这里可以理解为一共有多少个栅格，所以长×宽
unsigned char *data = costmap2d_->getCharMap(); //返回一个指针，指向一个底层无符号字符数组，数组中存储着代价值，这个数组貌似还可以用来作为代价地图。
for (unsigned int i = 0; i < covered_path_grid_.data.size(); i++)
{
/*if(data[i]==costmap_2d::FREE_SPACE)
covered_path_grid_.data[i] = costmap_2d::FREE_SPACE;
else
covered_path_grid_.data[i] = 0;*/
covered_path_grid_.data[i] = data[i]; //这里我理解为将代价值赋予到栅格地图的每个对应栅格当中去。
}
return true;
}
//hjr---------------------------------------------------------
bool CleaningPathPlanning::Boundingjudge(int a, int b)
{
int num = 0;
for (int i = -1; i <= 1; i++)
{
for (int m = -1; m <= 1; m++)
{
if (i == 0 && m == 0)
{
continue;
}
if (neuralizedMat_.at<float>((a + i), (b + m)) == -250.0)
num++;
}
}
if (num != 0)
return true;
else
return false;
}

100
clean_robot/src/CleaningPathPlanner.h Normal file
View File

@ -0,0 +1,100 @@
/***
* @brief: cleaning robot path planning
* @author: Wang
* @date: 20170702
***/
#ifndef CLEANINGPATHPLANNING_H
#define CLEANINGPATHPLANNING_H
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <vector>
#include <string>
#include "tf/tf.h"
#include "tf/transform_listener.h"
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
using namespace cv;
using namespace std;
constexpr double PI = 3.14159;
struct cellIndex
{
int row;
int col;
double theta; //{0, 45,90,135,180,225,270,315} 角度信息 hjr 注
};
/*************************************************
*
* 读取栅格地图并根据占据信息获取其对应的空闲（可行走）空间，
* 按照遍历算法规划行走路线。
*
* **********************************************/
class CleaningPathPlanning
{
public:
//CleaningPathPlanning() = delete;
CleaningPathPlanning(costmap_2d::Costmap2DROS *costmap2d_ros);
vector<geometry_msgs::PoseStamped> GetPathInROS();
vector<geometry_msgs::PoseStamped> GetBorderTrackingPathInROS();
void SetCoveredGrid(double wx, double wy);
int GetSizeOfCell() { return this->SIZE_OF_CELL; }
bool Boundingjudge(int a, int b);
//for visualization
void PublishCoveragePath();
void PublishGrid();
private:
//helper functions.
bool initializeMats();
bool initializeCoveredGrid();
void getCellMatAndFreeSpace(Mat srcImg, Mat &cellMat, vector<cellIndex> &freeSpaceVec);
void initializeNeuralMat(Mat cellMat, Mat neuralizedMat);
void writeResult(Mat resultmat, vector<cellIndex> pathVec);
void writeResult(cv::Mat resultmat, std::vector<cv::Point2i> pathVec);
void mainPlanningLoop();
double distance(Point2i pta, Point2i ptb);
bool findElement(vector<cv::Point2i> pointsVec, cv::Point2i pt, int &index);
void publishPlan(const std::vector<geometry_msgs::PoseStamped> &path);
bool cellContainsPoint(cv::Point2i pt, cellIndex cell);
void GetBorderTrackingPathInCV(vector<cv::Point2i> &resultVec);
vector<cellIndex> GetPathInCV();
bool initialized_;
Mat srcMap_;
Mat cellMat_;
Mat neuralizedMat_;
vector<cellIndex> freeSpaceVec_;
vector<cellIndex> pathVec_;
vector<geometry_msgs::PoseStamped> pathVecInROS_;
double resolution_;
ros::Publisher plan_pub_;
ros::Publisher grid_pub_;
nav_msgs::OccupancyGrid covered_path_grid_;
//tf::TransformListener &tf_;
geometry_msgs::PoseStamped initPose_;
costmap_2d::Costmap2D *costmap2d_;
costmap_2d::Costmap2DROS *costmap2d_ros_;
int SIZE_OF_CELL; //must be odd number.
int GRID_COVERED_VALUE;
};
#endif // CLEANINGPATHPLANNING_H

37
clean_robot/src/PathPlanningNode.cpp Normal file
View File

@ -0,0 +1,37 @@
#include "CleaningPathPlanner.h"
#include <boost/shared_ptr.hpp>
#include <costmap_2d/costmap_2d_ros.h>
namespace cm = costmap_2d;
namespace rm = geometry_msgs;
using std::vector;
using rm::PoseStamped;
using std::string;
using cm::Costmap2D;
using cm::Costmap2DROS;
int main(int argc, char** argv) {
ros::init(argc, argv, "path_planning_node");//hjr
tf2_ros::Buffer tf;
tf2_ros::TransformListener tf2_listener(tf);
costmap_2d::Costmap2DROS lcr("cleaning_costmap", tf);
//planner_costmap_ros_->pause();
ros::Duration(5).sleep();
CleaningPathPlanning clr(&lcr);
clr.GetPathInROS();
//clr.GetBorderTrackingPathInROS();
ros::Rate r(1);
while(ros::ok()){
clr.PublishCoveragePath();
ros::spinOnce();
r.sleep();
}
ros::shutdown();//关闭节点以及所有与之相关的发布，订阅，调用与服务。
return 0;
}

223
clean_robot/src/next_goal.cpp Normal file
View File

@ -0,0 +1,223 @@
/*This code is used to plan the trajectory of the robot
*/
#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "nav_msgs/Odometry.h"
#include "nav_msgs/Path.h"
#include "geometry_msgs/PoseStamped.h"
#include <tf/transform_listener.h>
#include <iostream>
#include <fstream>
using namespace std;
using namespace tf;
float x_current;
float y_current;
float normeNextGoal;
class quaternion_ros
{
public:
float w;
float x;
float y;
float z;
quaternion_ros();
void toQuaternion(float pitch, float roll, float yaw);
};
void quaternion_ros::toQuaternion(float pitch, float roll, float yaw)
{
float cy = cos(yaw * 0.5);
float sy = sin(yaw * 0.5);
float cr = cos(roll * 0.5);
float sr = sin(roll * 0.5);
float cp = cos(pitch * 0.5);
float sp = sin(pitch * 0.5);
w = cy * cr * cp + sy * sr * sp;
x = cy * sr * cp - sy * cr * sp;
y = cy * cr * sp + sy * sr * cp;
z = sy * cr * cp - cy * sr * sp;
}
quaternion_ros::quaternion_ros()
{
w = 1;
x = 0;
y = 0;
z = 0;
}
class Path_planned
{
public:
struct Goal
{
float x;
float y;
bool visited;
};
vector<Goal> Path;
Path_planned();
//Path_planned(float x, float y, bool visited);
void addGoal(float X, float Y, bool visit);
};
Path_planned::Path_planned()
{
}
//Path_planned(float x, float y, bool visited)
void Path_planned::addGoal(float X, float Y, bool visit)
{
Path_planned::Goal newGoal;
newGoal.x = X;
newGoal.y = Y;
newGoal.visited = visit;
Path.push_back(newGoal);
}
Path_planned planned_path;
nav_msgs::Path passed_path;
ros::Publisher pub_passed_path;
void pose_callback(const nav_msgs::Odometry &poses)
{ //里程计回调函数,用来计算当前机器人位置与前面目标点的距离,判断是否要发新的幕摆点
x_current = poses.pose.pose.position.x;
y_current = poses.pose.pose.position.y;
passed_path.header = poses.header;
geometry_msgs::PoseStamped p;
p.header = poses.header;
p.pose = poses.pose.pose;
passed_path.poses.emplace_back(p);
pub_passed_path.publish(passed_path);
}
int taille_last_path = 0;
bool new_path = false;
//接受规划的路径
void path_callback(const nav_msgs::Path &path)
{
//注意为了rviz显示方便 路径一直在发,但是这里只用接受一次就好,当规划的路径发生变化时候再重新装载
if ((planned_path.Path.size() == 0) || (path.poses.size() != taille_last_path))
{
planned_path.Path.clear();
new_path = true;
for (int i = 0; i < path.poses.size(); i++)
{
planned_path.addGoal(path.poses[i].pose.position.x, path.poses[i].pose.position.y, false);
cout << path.poses[i].pose.position.x << " " << path.poses[i].pose.position.y << endl;
}
cout << "Recv path size:" << path.poses.size() << endl;
taille_last_path = path.poses.size();
}
}
// int **count_antonin(char *)
int main(int argc, char *argv[])
{
srand(time(0));
ros::init(argc, argv, "next_goal");
ros::NodeHandle next_goal;
ros::Subscriber sub1 = next_goal.subscribe("/odom", 1000, pose_callback);
ros::Subscriber sub2 = next_goal.subscribe("/path_planning_node/cleaning_plan_nodehandle/cleaning_path", 1000, path_callback);
ros::Publisher pub1 = next_goal.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1000);
pub_passed_path = next_goal.advertise<nav_msgs::Path>("/clean_robot/passed_path", 1000);
ros::Rate loop_rate(10);
geometry_msgs::PoseStamped goal_msgs;
int count = 0;
double angle;
bool goal_reached = false;
//获取发送下一个点的阈值
if (!next_goal.getParam("/NextGoal/tolerance_goal", normeNextGoal))
{
ROS_ERROR("Please set your tolerance_goal");
return 0;
}
ROS_INFO("tolerance_goal=%f", normeNextGoal);
while (ros::ok())
{
ros::spinOnce();
if (new_path)
{
count = 0;
new_path = false;
}
//当前处理的点
cout << " count : " << count << endl;
if (!planned_path.Path.empty())
{
//当前距离达到了
if (sqrt(pow(x_current - planned_path.Path[count].x, 2) + pow(y_current - planned_path.Path[count].y, 2)) <= normeNextGoal)
{
count++;
goal_reached = false;
}
if (goal_reached == false)
{
goal_msgs.header.frame_id = "odom";
goal_msgs.header.stamp = ros::Time::now();
goal_msgs.pose.position.x = planned_path.Path[count].x;
goal_msgs.pose.position.y = planned_path.Path[count].y;
goal_msgs.pose.position.z = 0;
if (count < planned_path.Path.size())
{//计算发布的yaw，不过还有bug 但是不影响使用，yaw不会产生太大影响
angle = atan2(planned_path.Path[count + 1].y - planned_path.Path[count].y, planned_path.Path[count + 1].x - planned_path.Path[count].x);
}
else
{
angle = atan2(planned_path.Path[0].y - planned_path.Path[count].y, planned_path.Path[0].x - planned_path.Path[count].x);
}
cout << angle << endl;
quaternion_ros q;
q.toQuaternion(0, 0, float(angle));
goal_msgs.pose.orientation.w = q.w;
goal_msgs.pose.orientation.x = q.x;
goal_msgs.pose.orientation.y = q.y;
if (planned_path.Path[count].x < planned_path.Path[count + 1].x)
{
goal_msgs.pose.orientation.z = 0;
}
if (planned_path.Path[count].x > planned_path.Path[count + 1].x)
{
goal_msgs.pose.orientation.z = 2;
}
cout << " NEW GOAL " << endl;
cout << " x = " << planned_path.Path[count].x << " y = " << planned_path.Path[count].y << endl;
goal_reached = true;
pub1.publish(goal_msgs);
}
cout << x_current << " " << y_current << endl;
//当前
cout << planned_path.Path[count].x << " " << planned_path.Path[count].y << endl;
//目标
cout << " DISTANCE : " << sqrt((x_current - planned_path.Path[count].x) * (x_current - planned_path.Path[count].x) + (y_current - planned_path.Path[count].y) * (y_current - planned_path.Path[count].y)) << endl;
// 距离公式
}
loop_rate.sleep();
}
return 0;
}

592
clean_robot/worlds/clean_room.world Normal file
View File

@ -0,0 +1,592 @@
<sdf version='1.6'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose frame=''>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.5 -1</direction>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics name='default_physics' default='0' type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='clean_room'>
<pose frame=''>-0.225 -3.145 0 0 -0 0</pose>
<link name='Wall_0'>
<collision name='Wall_0_Collision'>
<geometry>
<box>
<size>6 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_0_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>6 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/CeilingTiled</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>0 1.925 0 0 -0 0</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<link name='Wall_1'>
<collision name='Wall_1_Collision'>
<geometry>
<box>
<size>4 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_1_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/CeilingTiled</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>2.925 0 0 0 0 -1.5708</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<link name='Wall_12'>
<collision name='Wall_12_Collision'>
<geometry>
<box>
<size>1.25 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_12_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>1.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Wood</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-0.032459 1.37074 0 0 0 -1.5708</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<link name='Wall_15'>
<collision name='Wall_15_Collision'>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_15_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Wood</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-0.033608 -1.2111 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<link name='Wall_2'>
<collision name='Wall_2_Collision'>
<geometry>
<box>
<size>6 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_2_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>6 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/CeilingTiled</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>0 -1.925 0 0 -0 3.14159</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<link name='Wall_3'>
<collision name='Wall_3_Collision'>
<geometry>
<box>
<size>4 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_3_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/CeilingTiled</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-2.925 0 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<static>1</static>
</model>
<model name='unit_box_0'>
<pose frame=''>-0.943901 -1.17332 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<state world_name='default'>
<sim_time>202 804000000</sim_time>
<real_time>203 218099251</real_time>
<wall_time>1592817139 630737498</wall_time>
<iterations>202804</iterations>
<model name='clean_room'>
<pose frame=''>-2.27449 -1.25584 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='Wall_0'>
<pose frame=''>-2.27449 0.669159 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_1'>
<pose frame=''>0.650514 -1.25584 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_12'>
<pose frame=''>-2.30694 0.114899 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_15'>
<pose frame=''>-2.30809 -2.46694 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_2'>
<pose frame=''>-2.27449 -3.18084 0 0 -0 3.14159</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_3'>
<pose frame=''>-5.19949 -1.25584 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='ground_plane'>
<pose frame=''>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='unit_box_0'>
<pose frame=''>-0.943901 -1.17332 0.862371 0 -0 0</pose>
<scale>1 1 1.65575</scale>
<link name='link'>
<pose frame=''>-0.943901 -1.17332 0.862371 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-0.004709 -9.78112 9.78158 0.712677 -0.009414 -4.3e-05</acceleration>
<wrench>-0.004709 -9.78112 9.78158 0 -0 0</wrench>
</link>
</model>
<model name='unit_box_1'>
<pose frame=''>-0.589858 -2.38679 0.5 0 -0 0</pose>
<scale>1 0.451115 1</scale>
<link name='link'>
<pose frame=''>-0.589858 -2.38679 0.5 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='unit_box_2'>
<pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
<scale>1 1.44086 1</scale>
<link name='link'>
<pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose frame=''>0 0 10 0 -0 0</pose>
</light>
</state>
<model name='unit_box_1'>
<pose frame=''>-0.221262 -2.44008 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<model name='unit_box_2'>
<pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 0.999995 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 0.999995 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose frame=''>-4.40017 -4.63981 10.4277 0.0395 1.41825 0.665003</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>

592
clean_robot/worlds/clean_room_new.world Normal file
View File

@ -0,0 +1,592 @@
<sdf version='1.6'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose frame=''>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.5 -1</direction>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics name='default_physics' default='0' type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='clean_room'>
<pose frame=''>-0.225 -3.145 0 0 -0 0</pose>
<link name='Wall_0'>
<collision name='Wall_0_Collision'>
<geometry>
<box>
<size>6 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_0_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>6 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/CeilingTiled</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>0 1.925 0 0 -0 0</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<link name='Wall_1'>
<collision name='Wall_1_Collision'>
<geometry>
<box>
<size>4 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_1_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/CeilingTiled</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>2.925 0 0 0 0 -1.5708</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<link name='Wall_12'>
<collision name='Wall_12_Collision'>
<geometry>
<box>
<size>1.25 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_12_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>1.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Wood</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-0.032459 1.37074 0 0 0 -1.5708</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<link name='Wall_15'>
<collision name='Wall_15_Collision'>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_15_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Wood</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-0.033608 -1.2111 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<link name='Wall_2'>
<collision name='Wall_2_Collision'>
<geometry>
<box>
<size>6 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_2_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>6 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/CeilingTiled</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>0 -1.925 0 0 -0 3.14159</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<link name='Wall_3'>
<collision name='Wall_3_Collision'>
<geometry>
<box>
<size>4 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_3_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/CeilingTiled</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-2.925 0 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
<static>1</static>
</model>
<model name='unit_box_0'>
<pose frame=''>-0.943901 -1.17332 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<state world_name='default'>
<sim_time>211 64000000</sim_time>
<real_time>8 289779519</real_time>
<wall_time>1593047453 346825405</wall_time>
<iterations>8260</iterations>
<model name='clean_room'>
<pose frame=''>-2.27449 -1.25584 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='Wall_0'>
<pose frame=''>-2.27449 0.669159 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_1'>
<pose frame=''>0.650514 -1.25584 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_12'>
<pose frame=''>-2.30694 0.114899 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_15'>
<pose frame=''>-2.30809 -2.46694 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_2'>
<pose frame=''>-2.27449 -3.18084 0 0 -0 3.14159</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_3'>
<pose frame=''>-5.19949 -1.25584 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='ground_plane'>
<pose frame=''>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='unit_box_0'>
<pose frame=''>-0.943902 -1.17416 0.827873 4e-06 -1e-06 3e-06</pose>
<scale>1 1 1.65575</scale>
<link name='link'>
<pose frame=''>-0.943902 -1.17416 0.827873 4e-06 -1e-06 3e-06</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>2.2913 -2.31936 -5.06986 -0.472089 0.332594 -3.05735</acceleration>
<wrench>2.2913 -2.31936 -5.06986 0 -0 0</wrench>
</link>
</model>
<model name='unit_box_1'>
<pose frame=''>-0.589861 -2.72216 0.499997 0 -7e-06 0</pose>
<scale>1 0.451115 1</scale>
<link name='link'>
<pose frame=''>-0.589861 -2.72216 0.499997 0 -7e-06 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-0.000647 -0.913095 0.411631 1.82618 -0.00123 0.0001</acceleration>
<wrench>-0.000647 -0.913095 0.411631 0 -0 0</wrench>
</link>
</model>
<model name='unit_box_2'>
<pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
<scale>1 1.44086 1</scale>
<link name='link'>
<pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>9.67937 0.027687 9.68609 -0.055112 0.511953 -7.7e-05</acceleration>
<wrench>9.67937 0.027687 9.68609 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose frame=''>0 0 10 0 -0 0</pose>
</light>
</state>
<model name='unit_box_1'>
<pose frame=''>-0.221262 -2.44008 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<model name='unit_box_2'>
<pose frame=''>-3.70594 -1.29835 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 0.999993 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 0.999993 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose frame=''>-1.88641 -2.99318 3.5742 -0 1.23425 0.953003</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>

44
explore/CHANGELOG.rst Normal file
View File

@ -0,0 +1,44 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package explore_lite
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2.1.1 (2017-12-16)
------------------
* explore: fix min_frontier_size bug
* min_frontier_size parameter was not used to at all
* adjust min_frontier size filtering according to map resolution
* fix bugs in CMakeLists.txt
* install nodes in packages, so they get shipped in debian packages. fixes `#11 <https://github.com/hrnr/m-explore/issues/11>`_
* explore: update documentation
* Contributors: Jiri Horner
2.1.0 (2017-10-30)
------------------
* explore: get rid of boost
* explore: rework launchfiles
* new visualisation of frontiers
* remove navfn library from dependencies
* use frontier seach algorithm from Paul Bovbel
* much better than previous version of search
* https://github.com/paulbovbel/frontier_exploration.git
* fix deadlock in explore
* reworked expore to use timer instead of separate thread for replanning
* fix deadlock occuring between makePlan and goal reached callback
* Contributors: Jiri Horner
2.0.0 (2017-03-26)
------------------
* explore: migrate to package format 2
* explore: remove internal version of navfn_ros
* my changes are included in the ros since kinetic
* Contributors: Jiri Horner
1.0.1 (2017-03-25)
------------------
* update documentation
* Contributors: Jiri Horner
1.0.0 (2016-05-11)
------------------
* initial release
* Contributors: Jiri Horner, duhadway-bosch, pitzer

79
explore/CMakeLists.txt Normal file
View File

@ -0,0 +1,79 @@
cmake_minimum_required(VERSION 3.1)
project(explore_lite)
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
actionlib
actionlib_msgs
costmap_2d
geometry_msgs
map_msgs
move_base_msgs
nav_msgs
roscpp
std_msgs
tf
visualization_msgs
)
###################################
## catkin specific configuration ##
###################################
catkin_package(
CATKIN_DEPENDS
actionlib_msgs
geometry_msgs
map_msgs
move_base_msgs
nav_msgs
std_msgs
visualization_msgs
)
###########
## Build ##
###########
# c++11 support required
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
## Specify additional locations of header files
include_directories(
${catkin_INCLUDE_DIRS}
include
)
add_executable(explore
src/costmap_client.cpp
src/explore.cpp
src/frontier_search.cpp
)
add_dependencies(explore ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(explore ${catkin_LIBRARIES})
#############
## Install ##
#############
# install nodes
install(TARGETS explore
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# install roslaunch files
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)
#############
## Testing ##
#############
if(CATKIN_ENABLE_TESTING)
find_package(roslaunch REQUIRED)
# test all launch files
roslaunch_add_file_check(launch)
endif()

BIN
explore/doc/architecture.dia Normal file
View File

Binary file not shown.

BIN
explore/doc/screenshot.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 24 KiB

148
explore/doc/wiki_doc.txt Normal file
View File

@ -0,0 +1,148 @@
<<PackageHeader(explore_lite)>>
<<GitHubIssues(hrnr/m-explore)>>
<<TOC(4)>>
== Overview ==
This package provides greedy frontier-based exploration. When node is running, robot will greedily explore its environment until no frontiers could be found. Movement commands will be send to [[move_base]].
{{attachment:screenshot.png||width="755px"}}
Unlike similar packages, {{{explore_lite}}} does not create its own costmap, which makes it easier to configure and more efficient (lighter on resources). Node simply subscribes to <<MsgLink(nav_msgs/OccupancyGrid)>> messages. Commands for robot movement are send to [[move_base]] node.
Node can do frontier filtering and can operate even on non-inflated maps. Goal blacklisting allows to deal with places inaccessible for robot.
<<Youtube(op0L0LyGNwY&rel=0)>>
== Architecture ==
{{{explore_lite}}} uses [[move_base]] for navigation. You need to run properly configured [[move_base]] node.
{{attachment:architecture.svg||width="755px"}}
{{{explore_lite}}} subscribes to a <<MsgLink(nav_msgs/OccupancyGrid)>> and <<MsgLink(map_msgs/OccupancyGridUpdate)>> messages to construct a map where it looks for frontiers. You can either use costmap published by [[move_base]] (ie. `<move_base>/global_costmap/costmap`) or you can use map constructed by mapping algorithm (SLAM).
Depending on your environment you may achieve better results with either SLAM map or costmap published by `move_base`. Advantage of `move_base` costmap is the inflation which helps to deal with some very small unexplorable frontiers. When you are using a raw map produced by SLAM you should set the `min_frontier_size` parameter to some reasonable number to deal with the small frontiers. For details on both setups check the `explore.launch` and `explore_costmap.launch` launch files.
== Setup ==
Before starting experimenting with {{{explore_lite}}} you need to have working [[move_base]] for navigation. You should be able to navigate with [[move_base]] manually through [[rviz]]. Please refer to [[navigation#Tutorials]] for setting up [[move_base]] and the rest of the navigation stack with your robot.
You should be also able to to navigate with [[move_base]] though unknown space in the map. If you set the goal to unknown place in the map, planning and navigating should work. With most planners this should work by default, refer to [[navfn#Parameters]] if you need to setup this for [[navfn]] planner (but should be enabled by default). Navigation through unknown space is required for {{{explore_lite}}}.
If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`.
If you have [[move_base]] configured correctly, you can start experimenting with {{{explore_lite}}}. Provided `explore.launch` should work out-of-the box in most cases, but as always you might need to adjust topic names and frame names according to your setup.
== ROS API ==
{{{
#!clearsilver CS/NodeAPI
name = explore
desc = Provides exploration services offered by this package. Exploration will start immediately after node initialization.
pub {
0.name = ~frontiers
0.type = visualization_msgs/MarkerArray
0.desc = Visualization of frontiers considered by exploring algorithm. Each frontier is visualized by frontier points in blue and with a small sphere, which visualize the cost of the frontiers (costlier frontiers will have smaller spheres).
}
sub {
0.name = costmap
0.type = nav_msgs/OccupancyGrid
0.desc = Map which will be used for exploration planning. Can be either costmap from [[move_base]] or map created by SLAM (see above). Occupancy grid must have got properly marked unknown space, mapping algorithms usually track unknown space by default. If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`.
1.name = costmap_updates
1.type = map_msgs/OccupancyGridUpdate
1.desc = Incremental updates on costmap. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic.
}
param {
0.name = ~robot_base_frame
0.default = `base_link`
0.type = string
0.desc = The name of the base frame of the robot. This is used for determining robot position on map. Mandatory.
1.name = ~costmap_topic
1.default = `costmap`
1.type = string
1.desc = Specifies topic of source <<MsgLink(nav_msgs/OccupancyGrid)>>. Mandatory.
3.name = ~costmap_updates_topic
3.default = `costmap_updates`
3.type = string
3.desc = Specifies topic of source <<MsgLink(map_msgs/OccupancyGridUpdate)>>. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic.
4.name = ~visualize
4.default = `false`
4.type = bool
4.desc = Specifies whether or not publish visualized frontiers.
6.name = ~planner_frequency
6.default = `1.0`
6.type = double
6.desc = Rate in Hz at which new frontiers will computed and goal reconsidered.
7.name = ~progress_timeout
7.default = `30.0`
7.type = double
7.desc = Time in seconds. When robot do not make any progress for `progress_timeout`, current goal will be abandoned.
8.name = ~potential_scale
8.default = `1e-3`
8.type = double
8.desc = Used for weighting frontiers. This multiplicative parameter affects frontier potential component of the frontier weight (distance to frontier).
9.name = ~orientation_scale
9.default = `0`
9.type = double
9.desc = Used for weighting frontiers. This multiplicative parameter affects frontier orientation component of the frontier weight. This parameter does currently nothing and is provided solely for forward compatibility.
10.name = ~gain_scale
10.default = `1.0`
10.type = double
10.desc = Used for weighting frontiers. This multiplicative parameter affects frontier gain component of the frontier weight (frontier size).
11.name = ~transform_tolerance
11.default = `0.3`
11.type = double
11.desc = Transform tolerance to use when transforming robot pose.
12.name = ~min_frontier_size
12.default = `0.5`
12.type = double
12.desc = Minimum size of the frontier to consider the frontier as the exploration goal. In meters.
}
req_tf {
0.from = global_frame
0.to = robot_base_frame
0.desc = This transformation is usually provided by mapping algorithm. Those frames are usually called `map` and `base_link`. For adjusting `robot_base_frame` name see respective parameter. You don't need to set `global_frame`. The name for `global_frame` will be sourced from `costmap_topic` automatically.
}
act_called {
0.name = move_base
0.type = move_base_msgs/MoveBaseAction
0.desc = [[move_base]] actionlib API for posting goals. See [[move_base#Action API]] for details. This expects [[move_base]] node in the same namespace as `explore_lite`, you may want to remap this node if this is not true.
}
}}}
== Acknowledgements ==
This package was developed as part of my bachelor thesis at [[http://www.mff.cuni.cz/to.en/|Charles University]] in Prague.
{{{
@masterthesis{Hörner2016,
author = {Jiří Hörner},
title = {Map-merging for multi-robot system},
address = {Prague},
year = {2016},
school = {Charles University in Prague, Faculty of Mathematics and Physics},
type = {Bachelor's thesis},
URL = {https://is.cuni.cz/webapps/zzp/detail/174125/},
}
}}}
This project was initially based on [[explore]] package by Charles !DuHadway. Most of the node has been rewritten since then. The current frontier search algorithm is based on [[frontier_exploration]] by Paul Bovbel.
## AUTOGENERATED DON'T DELETE
## CategoryPackage

131
explore/include/explore/costmap_client.h Normal file
View File

@ -0,0 +1,131 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Jiri Horner nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/
#ifndef COSTMAP_CLIENT_
#define COSTMAP_CLIENT_
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/Pose.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <nav_msgs/OccupancyGrid.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
namespace explore
{
class Costmap2DClient
{
public:
/**
* @brief Contructs client and start listening
* @details Constructor will block until first map update is received and
* map is ready to use, also will block before trasformation
* robot_base_frame <-> global_frame is available.
*
* @param param_nh node hadle to retrieve parameters from
* @param subscription_nh node hadle where topics will be subscribed
* @param tf_listener Will be used for transformation of robot pose.
*/
Costmap2DClient(ros::NodeHandle& param_nh, ros::NodeHandle& subscription_nh,
const tf::TransformListener* tf_listener);
/**
* @brief Get the pose of the robot in the global frame of the costmap
* @return pose of the robot in the global frame of the costmap
*/
geometry_msgs::Pose getRobotPose() const;
/**
* @brief Return a pointer to the "master" costmap which receives updates from
* all the layers.
*
* This pointer will stay the same for the lifetime of Costmap2DClient object.
*/
costmap_2d::Costmap2D* getCostmap()
{
return &costmap_;
}
/**
* @brief Return a pointer to the "master" costmap which receives updates from
* all the layers.
*
* This pointer will stay the same for the lifetime of Costmap2DClient object.
*/
const costmap_2d::Costmap2D* getCostmap() const
{
return &costmap_;
}
/**
* @brief Returns the global frame of the costmap
* @return The global frame of the costmap
*/
const std::string& getGlobalFrameID() const
{
return global_frame_;
}
/**
* @brief Returns the local frame of the costmap
* @return The local frame of the costmap
*/
const std::string& getBaseFrameID() const
{
return robot_base_frame_;
}
protected:
void updateFullMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
void updatePartialMap(const map_msgs::OccupancyGridUpdate::ConstPtr& msg);
costmap_2d::Costmap2D costmap_;
const tf::TransformListener* const tf_; ///< @brief Used for transforming
/// point clouds
std::string global_frame_; ///< @brief The global frame for the costmap
std::string robot_base_frame_; ///< @brief The frame_id of the robot base
double transform_tolerance_; ///< timeout before transform errors
private:
// will be unsubscribed at destruction
ros::Subscriber costmap_sub_;
ros::Subscriber costmap_updates_sub_;
};
} // namespace explore
#endif

133
explore/include/explore/costmap_tools.h Normal file
View File

@ -0,0 +1,133 @@
#ifndef COSTMAP_TOOLS_H_
#define COSTMAP_TOOLS_H_
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PolygonStamped.h>
#include <ros/ros.h>
namespace frontier_exploration
{
/**
* @brief Determine 4-connected neighbourhood of an input cell, checking for map
* edges
* @param idx input cell index
* @param costmap Reference to map data
* @return neighbour cell indexes
*/
std::vector<unsigned int> nhood4(unsigned int idx,
const costmap_2d::Costmap2D& costmap)
{
// get 4-connected neighbourhood indexes, check for edge of map
std::vector<unsigned int> out;
unsigned int size_x_ = costmap.getSizeInCellsX(),
size_y_ = costmap.getSizeInCellsY();
if (idx > size_x_ * size_y_ - 1) {
ROS_WARN("Evaluating nhood for offmap point");
return out;
}
if (idx % size_x_ > 0) {
out.push_back(idx - 1);
}
if (idx % size_x_ < size_x_ - 1) {
out.push_back(idx + 1);
}
if (idx >= size_x_) {
out.push_back(idx - size_x_);
}
if (idx < size_x_ * (size_y_ - 1)) {
out.push_back(idx + size_x_);
}
return out;
}
/**
* @brief Determine 8-connected neighbourhood of an input cell, checking for map
* edges
* @param idx input cell index
* @param costmap Reference to map data
* @return neighbour cell indexes
*/
std::vector<unsigned int> nhood8(unsigned int idx,
const costmap_2d::Costmap2D& costmap)
{
// get 8-connected neighbourhood indexes, check for edge of map
std::vector<unsigned int> out = nhood4(idx, costmap);
unsigned int size_x_ = costmap.getSizeInCellsX(),
size_y_ = costmap.getSizeInCellsY();
if (idx > size_x_ * size_y_ - 1) {
return out;
}
if (idx % size_x_ > 0 && idx >= size_x_) {
out.push_back(idx - 1 - size_x_);
}
if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) {
out.push_back(idx - 1 + size_x_);
}
if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) {
out.push_back(idx + 1 - size_x_);
}
if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) {
out.push_back(idx + 1 + size_x_);
}
return out;
}
/**
* @brief Find nearest cell of a specified value
* @param result Index of located cell
* @param start Index initial cell to search from
* @param val Specified value to search for
* @param costmap Reference to map data
* @return True if a cell with the requested value was found
*/
bool nearestCell(unsigned int& result, unsigned int start, unsigned char val,
const costmap_2d::Costmap2D& costmap)
{
const unsigned char* map = costmap.getCharMap();
const unsigned int size_x = costmap.getSizeInCellsX(),
size_y = costmap.getSizeInCellsY();
if (start >= size_x * size_y) {
return false;
}
// initialize breadth first search
std::queue<unsigned int> bfs;
std::vector<bool> visited_flag(size_x * size_y, false);
// push initial cell
bfs.push(start);
visited_flag[start] = true;
// search for neighbouring cell matching value
while (!bfs.empty()) {
unsigned int idx = bfs.front();
bfs.pop();
// return if cell of correct value is found
if (map[idx] == val) {
result = idx;
return true;
}
// iterate over all adjacent unvisited cells
for (unsigned nbr : nhood8(idx, costmap)) {
if (!visited_flag[nbr]) {
bfs.push(nbr);
visited_flag[nbr] = true;
}
}
}
return false;
}
}
#endif

114
explore/include/explore/explore.h Normal file
View File

@ -0,0 +1,114 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Robert Bosch LLC.
* Copyright (c) 2015-2016, Jiri Horner.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Jiri Horner nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/
#ifndef NAV_EXPLORE_H_
#define NAV_EXPLORE_H_
#include <memory>
#include <mutex>
#include <string>
#include <vector>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/PoseStamped.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <explore/costmap_client.h>
#include <explore/frontier_search.h>
namespace explore
{
/**
* @class Explore
* @brief A class adhering to the robot_actions::Action interface that moves the
* robot base to explore its environment.
*/
class Explore
{
public:
Explore();
~Explore();
void start();
void stop();
private:
/**
* @brief Make a global plan
*/
void makePlan();
/**
* @brief Publish a frontiers as markers
*/
void visualizeFrontiers(
const std::vector<frontier_exploration::Frontier>& frontiers);
void reachedGoal(const actionlib::SimpleClientGoalState& status,
const move_base_msgs::MoveBaseResultConstPtr& result,
const geometry_msgs::Point& frontier_goal);
bool goalOnBlacklist(const geometry_msgs::Point& goal);
ros::NodeHandle private_nh_;
ros::NodeHandle relative_nh_;
ros::Publisher marker_array_publisher_;
tf::TransformListener tf_listener_;
Costmap2DClient costmap_client_;
actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>
move_base_client_;
frontier_exploration::FrontierSearch search_;
ros::Timer exploring_timer_;
ros::Timer oneshot_;
std::vector<geometry_msgs::Point> frontier_blacklist_;
geometry_msgs::Point prev_goal_;
double prev_distance_;
ros::Time last_progress_;
size_t last_markers_count_;
// parameters
double planner_frequency_;
double potential_scale_, orientation_scale_, gain_scale_;
ros::Duration progress_timeout_;
bool visualize_;
};
}
#endif

88
explore/include/explore/frontier_search.h Normal file
View File

@ -0,0 +1,88 @@
#ifndef FRONTIER_SEARCH_H_
#define FRONTIER_SEARCH_H_
#include <costmap_2d/costmap_2d.h>
namespace frontier_exploration
{
/**
* @brief Represents a frontier
*
*/
struct Frontier {
std::uint32_t size;
double min_distance;
double cost;
geometry_msgs::Point initial;
geometry_msgs::Point centroid;
geometry_msgs::Point middle;
std::vector<geometry_msgs::Point> points;
};
/**
* @brief Thread-safe implementation of a frontier-search task for an input
* costmap.
*/
class FrontierSearch
{
public:
FrontierSearch()
{
}
/**
* @brief Constructor for search task
* @param costmap Reference to costmap data to search.
*/
FrontierSearch(costmap_2d::Costmap2D* costmap, double potential_scale,
double gain_scale, double min_frontier_size);
/**
* @brief Runs search implementation, outward from the start position
* @param position Initial position to search from
* @return List of frontiers, if any
*/
std::vector<Frontier> searchFrom(geometry_msgs::Point position);
protected:
/**
* @brief Starting from an initial cell, build a frontier from valid adjacent
* cells
* @param initial_cell Index of cell to start frontier building
* @param reference Reference index to calculate position from
* @param frontier_flag Flag vector indicating which cells are already marked
* as frontiers
* @return new frontier
*/
Frontier buildNewFrontier(unsigned int initial_cell, unsigned int reference,
std::vector<bool>& frontier_flag);
/**
* @brief isNewFrontierCell Evaluate if candidate cell is a valid candidate
* for a new frontier.
* @param idx Index of candidate cell
* @param frontier_flag Flag vector indicating which cells are already marked
* as frontiers
* @return true if the cell is frontier cell
*/
bool isNewFrontierCell(unsigned int idx,
const std::vector<bool>& frontier_flag);
/**
* @brief computes frontier cost
* @details cost function is defined by potential_scale and gain_scale
*
* @param frontier frontier for which compute the cost
* @return cost of the frontier
*/
double frontierCost(const Frontier& frontier);
private:
costmap_2d::Costmap2D* costmap_;
unsigned char* map_;
unsigned int size_x_, size_y_;
double potential_scale_, gain_scale_;
double min_frontier_size_;
};
}
#endif

25
explore/launch/explore.launch Normal file
View File

@ -0,0 +1,25 @@
<launch>
<!-- move_base -->
<include file="$(find turtlebot3_navigation)/launch/move_base.launch">
<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
</include>
<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
<param name="robot_base_frame" value="base_link"/>
<param name="costmap_topic" value="map"/>
<param name="costmap_updates_topic" value="map_updates"/>
<param name="visualize" value="true"/>
<param name="planner_frequency" value="0.33"/>
<param name="progress_timeout" value="50.0"/>
<param name="potential_scale" value="3.0"/>
<param name="orientation_scale" value="0.0"/>
<param name="gain_scale" value="1.0"/>
<param name="transform_tolerance" value="0.3"/>
<param name="min_frontier_size" value="0.6"/>
</node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
</node>
</launch>

15
explore/launch/explore_costmap.launch Normal file
View File

@ -0,0 +1,15 @@
<launch>
<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
<param name="robot_base_frame" value="base_link"/>
<param name="costmap_topic" value="move_base/global_costmap/costmap"/>
<param name="costmap_updates_topic" value="move_base/global_costmap/costmap_updates"/>
<param name="visualize" value="true"/>
<param name="planner_frequency" value="0.33"/>
<param name="progress_timeout" value="30.0"/>
<param name="potential_scale" value="3.0"/>
<param name="orientation_scale" value="0.0"/>
<param name="gain_scale" value="1.0"/>
<param name="transform_tolerance" value="0.3"/>
<param name="min_frontier_size" value="0.5"/>
</node>
</launch>

239
explore/launch/explore_multi.launch Normal file
View File

@ -0,0 +1,239 @@
<launch>
<!-- Argument -->
<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="move_forward_only" default="false"/>
<arg name="cmd_vel_topic" default="/cmd_vel" />
<arg name="odom_topic" default="odom" />
<group ns="tb3_0">
<!-- Turtlebot3 -->
<include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml">
<arg name="model" value="$(arg model)" />
</include>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
<param name="tf_prefix" value="tb3_0" />
</node>
<!-- move_base -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
<rosparam file="$(find explore_lite)/param/first_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_0/global_costmap" />
<rosparam file="$(find explore_lite)/param/first_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_0/local_costmap" />
<rosparam file="$(find explore_lite)/param/first_local_costmap_params.yaml" command="load" />
<rosparam file="$(find explore_lite)/param/first_global_costmap_params.yaml" command="load" />
<rosparam file="$(find explore_lite)/param/move_base_params.yaml" command="load" />
<rosparam file="$(find explore_lite)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
<remap from="cmd_vel" to="/tb3_0/cmd_vel"/>
<remap from="odom" to="tb3_0/odom"/>
<param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
</node>
<!-- SLAM -->
<node pkg="gmapping" type="slam_gmapping" name="turtlebot3_slam_gmapping" output="screen">
<param name="base_frame" value="/tb3_0/base_footprint"/>
<param name="odom_frame" value="/tb3_0/odom"/>
<param name="map_frame" value="/tb3_0/map"/>
<param name="map_update_interval" value="2.0"/>
<param name="maxUrange" value="3.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="minimumScore" value="50"/>
<param name="srr" value="0.1"/>
<param name="srt" value="0.2"/>
<param name="str" value="0.1"/>
<param name="stt" value="0.2"/>
<param name="linearUpdate" value="1.0"/>
<param name="angularUpdate" value="0.2"/>
<param name="temporalUpdate" value="0.5"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="100"/>
<param name="xmin" value="-10.0"/>
<param name="ymin" value="-10.0"/>
<param name="xmax" value="10.0"/>
<param name="ymax" value="10.0"/>
<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
</node>
<!-- Explore -->
<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
<param name="robot_base_frame" value="/tb3_0/base_footprint"/>
<param name="costmap_topic" value="/tb3_0/map"/>
<param name="costmap_updates_topic" value="/tb3_0/map_updates"/>
<param name="visualize" value="true"/>
<param name="planner_frequency" value="0.33"/>
<param name="progress_timeout" value="30.0"/>
<param name="potential_scale" value="3.0"/>
<param name="orientation_scale" value="0.0"/>
<param name="gain_scale" value="1.0"/>
<param name="transform_tolerance" value="0.3"/>
<param name="min_frontier_size" value="0.75"/>
</node>
</group>
<group ns="tb3_1">
<!-- Turtlebot3 -->
<include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml">
<arg name="model" value="$(arg model)" />
</include>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
<param name="tf_prefix" value="tb3_1" />
</node>
<!-- move_base -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
<rosparam file="$(find explore_lite)/param/second_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_1/global_costmap" />
<rosparam file="$(find explore_lite)/param/second_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_1/local_costmap" />
<rosparam file="$(find explore_lite)/param/second_local_costmap_params.yaml" command="load" />
<rosparam file="$(find explore_lite)/param/second_global_costmap_params.yaml" command="load" />
<rosparam file="$(find explore_lite)/param/move_base_params.yaml" command="load" />
<rosparam file="$(find explore_lite)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
<remap from="cmd_vel" to="/tb3_1/cmd_vel"/>
<remap from="odom" to="/tb3_1/odom"/>
<param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
</node>
<node pkg="gmapping" type="slam_gmapping" name="turtlebot3_slam_gmapping" output="screen">
<param name="base_frame" value="/tb3_1/base_footprint"/>
<param name="odom_frame" value="/tb3_1/odom"/>
<param name="map_frame" value="/tb3_1/map"/>
<param name="map_update_interval" value="2.0"/>
<param name="maxUrange" value="3.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="minimumScore" value="50"/>
<param name="srr" value="0.1"/>
<param name="srt" value="0.2"/>
<param name="str" value="0.1"/>
<param name="stt" value="0.2"/>
<param name="linearUpdate" value="1.0"/>
<param name="angularUpdate" value="0.2"/>
<param name="temporalUpdate" value="0.5"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="100"/>
<param name="xmin" value="-10.0"/>
<param name="ymin" value="-10.0"/>
<param name="xmax" value="10.0"/>
<param name="ymax" value="10.0"/>
<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
</node>
<!-- Explore -->
<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
<param name="robot_base_frame" value="/tb3_1/base_footprint"/>
<param name="costmap_topic" value="/tb3_1/map"/>
<param name="costmap_updates_topic" value="/tb3_1/map_updates"/>
<param name="visualize" value="true"/>
<param name="planner_frequency" value="0.33"/>
<param name="progress_timeout" value="30.0"/>
<param name="potential_scale" value="3.0"/>
<param name="orientation_scale" value="0.0"/>
<param name="gain_scale" value="1.0"/>
<param name="transform_tolerance" value="0.3"/>
<param name="min_frontier_size" value="0.75"/>
</node>
</group>
<group ns="tb3_2">
<!-- Turtlebot3 -->
<include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml">
<arg name="model" value="$(arg model)" />
</include>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
<param name="tf_prefix" value="tb3_2" />
</node>
<!-- move_base -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
<rosparam file="$(find explore_lite)/param/third_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_2/global_costmap" />
<rosparam file="$(find explore_lite)/param/third_costmap_common_params_$(arg model).yaml" command="load" ns="/tb3_2/local_costmap" />
<rosparam file="$(find explore_lite)/param/third_local_costmap_params.yaml" command="load" />
<rosparam file="$(find explore_lite)/param/third_global_costmap_params.yaml" command="load" />
<rosparam file="$(find explore_lite)/param/move_base_params.yaml" command="load" />
<rosparam file="$(find explore_lite)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
<remap from="cmd_vel" to="/tb3_2/cmd_vel"/>
<remap from="odom" to="/tb3_2/odom"/>
<param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
</node>
<!-- SLAM -->
<node pkg="gmapping" type="slam_gmapping" name="turtlebot3_slam_gmapping" output="screen">
<param name="base_frame" value="/tb3_2/base_footprint"/>
<param name="odom_frame" value="/tb3_2/odom"/>
<param name="map_frame" value="/tb3_2/map"/>
<param name="map_update_interval" value="2.0"/>
<param name="maxUrange" value="3.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="minimumScore" value="50"/>
<param name="srr" value="0.1"/>
<param name="srt" value="0.2"/>
<param name="str" value="0.1"/>
<param name="stt" value="0.2"/>
<param name="linearUpdate" value="1.0"/>
<param name="angularUpdate" value="0.2"/>
<param name="temporalUpdate" value="0.5"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="100"/>
<param name="xmin" value="-10.0"/>
<param name="ymin" value="-10.0"/>
<param name="xmax" value="10.0"/>
<param name="ymax" value="10.0"/>
<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
</node>
<!-- Explore -->
<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
<param name="robot_base_frame" value="/tb3_2/base_footprint"/>
<param name="costmap_topic" value="/tb3_2/map"/>
<param name="costmap_updates_topic" value="/tb3_2/map_updates"/>
<param name="visualize" value="true"/>
<param name="planner_frequency" value="0.33"/>
<param name="progress_timeout" value="30.0"/>
<param name="potential_scale" value="3.0"/>
<param name="orientation_scale" value="0.0"/>
<param name="gain_scale" value="1.0"/>
<param name="transform_tolerance" value="0.3"/>
<param name="min_frontier_size" value="0.75"/>
</node>
</group>
</launch>

34
explore/package.xml Normal file
View File

@ -0,0 +1,34 @@
<?xml version="1.0"?>
<package format="2">
<name>explore_lite</name>
<version>2.1.1</version>
<description>Lightweight frontier-based exploration.</description>
<author email="laeqten@gmail.com">Jiri Horner</author>
<maintainer email="laeqten@gmail.com">Jiri Horner</maintainer>
<license>BSD</license>
<url>http://wiki.ros.org/explore_lite</url>
<!-- acknowledgements
<author>Charles DuHadway</author>
<url>http://ros.org/wiki/explore</url>
<author>Paul Bovbel</author>
<url>https://github.com/paulbovbel/frontier_exploration</url>
-->
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>move_base_msgs</depend>
<depend>visualization_msgs</depend>
<depend>geometry_msgs</depend>
<depend>map_msgs</depend>
<depend>nav_msgs</depend>
<depend>actionlib_msgs</depend>
<depend>tf</depend>
<depend>costmap_2d</depend>
<depend>actionlib</depend>
<test_depend>roslaunch</test_depend>
</package>

252
explore/src/costmap_client.cpp Normal file
View File

@ -0,0 +1,252 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Jiri Horner nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/
#include <explore/costmap_client.h>
#include <functional>
#include <mutex>
#include <string>
namespace explore
{
// static translation table to speed things up
std::array<unsigned char, 256> init_translation_table();
static const std::array<unsigned char, 256> cost_translation_table__ =
init_translation_table();
Costmap2DClient::Costmap2DClient(ros::NodeHandle& param_nh,
ros::NodeHandle& subscription_nh,
const tf::TransformListener* tf)
: tf_(tf)
{
std::string costmap_topic;
std::string footprint_topic;
std::string costmap_updates_topic;
param_nh.param("costmap_topic", costmap_topic, std::string("costmap"));
param_nh.param("costmap_updates_topic", costmap_updates_topic,
std::string("costmap_updates"));
param_nh.param("robot_base_frame", robot_base_frame_,
std::string("base_link"));
// transform tolerance is used for all tf transforms here
param_nh.param("transform_tolerance", transform_tolerance_, 0.3);
/* initialize costmap */
costmap_sub_ = subscription_nh.subscribe<nav_msgs::OccupancyGrid>(
costmap_topic, 1000,
[this](const nav_msgs::OccupancyGrid::ConstPtr& msg) {
updateFullMap(msg);
});
ROS_INFO("Waiting for costmap to become available, topic: %s",
costmap_topic.c_str());
auto costmap_msg = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>(
costmap_topic, subscription_nh);
updateFullMap(costmap_msg);
/* subscribe to map updates */
costmap_updates_sub_ =
subscription_nh.subscribe<map_msgs::OccupancyGridUpdate>(
costmap_updates_topic, 1000,
[this](const map_msgs::OccupancyGridUpdate::ConstPtr& msg) {
updatePartialMap(msg);
});
/* resolve tf prefix for robot_base_frame */
std::string tf_prefix = tf::getPrefixParam(param_nh);
robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);
// we need to make sure that the transform between the robot base frame and
// the global frame is available
/* tf transform is necessary for getRobotPose */
ros::Time last_error = ros::Time::now();
std::string tf_error;
while (ros::ok() &&
!tf_->waitForTransform(global_frame_, robot_base_frame_, ros::Time(),
ros::Duration(0.1), ros::Duration(0.01),
&tf_error)) {
ros::spinOnce();
if (last_error + ros::Duration(5.0) < ros::Time::now()) {
ROS_WARN(
"Timed out waiting for transform from %s to %s to become available "
"before subscribing to costmap, tf error: %s",
robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
last_error = ros::Time::now();
}
// The error string will accumulate and errors will typically be the same,
// so the last
// will do for the warning above. Reset the string here to avoid
// accumulation.
tf_error.clear();
}
}
void Costmap2DClient::updateFullMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
global_frame_ = msg->header.frame_id;
unsigned int size_in_cells_x = msg->info.width;
unsigned int size_in_cells_y = msg->info.height;
double resolution = msg->info.resolution;
double origin_x = msg->info.origin.position.x;
double origin_y = msg->info.origin.position.y;
ROS_DEBUG("received full new map, resizing to: %d, %d", size_in_cells_x,
size_in_cells_y);
costmap_.resizeMap(size_in_cells_x, size_in_cells_y, resolution, origin_x,
origin_y);
// lock as we are accessing raw underlying map
auto* mutex = costmap_.getMutex();
std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
// fill map with data
unsigned char* costmap_data = costmap_.getCharMap();
size_t costmap_size = costmap_.getSizeInCellsX() * costmap_.getSizeInCellsY();
ROS_DEBUG("full map update, %lu values", costmap_size);
for (size_t i = 0; i < costmap_size && i < msg->data.size(); ++i) {
unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
costmap_data[i] = cost_translation_table__[cell_cost];
}
ROS_DEBUG("map updated, written %lu values", costmap_size);
}
void Costmap2DClient::updatePartialMap(
const map_msgs::OccupancyGridUpdate::ConstPtr& msg)
{
ROS_DEBUG("received partial map update");
global_frame_ = msg->header.frame_id;
if (msg->x < 0 || msg->y < 0) {
ROS_ERROR("negative coordinates, invalid update. x: %d, y: %d", msg->x,
msg->y);
return;
}
size_t x0 = static_cast<size_t>(msg->x);
size_t y0 = static_cast<size_t>(msg->y);
size_t xn = msg->width + x0;
size_t yn = msg->height + y0;
// lock as we are accessing raw underlying map
auto* mutex = costmap_.getMutex();
std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
size_t costmap_xn = costmap_.getSizeInCellsX();
size_t costmap_yn = costmap_.getSizeInCellsY();
if (xn > costmap_xn || x0 > costmap_xn || yn > costmap_yn ||
y0 > costmap_yn) {
ROS_WARN("received update doesn't fully fit into existing map, "
"only part will be copied. received: [%lu, %lu], [%lu, %lu] "
"map is: [0, %lu], [0, %lu]",
x0, xn, y0, yn, costmap_xn, costmap_yn);
}
// update map with data
unsigned char* costmap_data = costmap_.getCharMap();
size_t i = 0;
for (size_t y = y0; y < yn && y < costmap_yn; ++y) {
for (size_t x = x0; x < xn && x < costmap_xn; ++x) {
size_t idx = costmap_.getIndex(x, y);
unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
costmap_data[idx] = cost_translation_table__[cell_cost];
++i;
}
}
}
geometry_msgs::Pose Costmap2DClient::getRobotPose() const
{
tf::Stamped<tf::Pose> global_pose;
global_pose.setIdentity();
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
robot_pose.stamp_ = ros::Time();
ros::Time current_time =
ros::Time::now(); // save time for checking tf delay later
// get the global pose of the robot
try {
tf_->transformPose(global_frame_, robot_pose, global_pose);
} catch (tf::LookupException& ex) {
ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot "
"pose: %s\n",
ex.what());
return {};
} catch (tf::ConnectivityException& ex) {
ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n",
ex.what());
return {};
} catch (tf::ExtrapolationException& ex) {
ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n",
ex.what());
return {};
}
// check global_pose timeout
if (current_time.toSec() - global_pose.stamp_.toSec() >
transform_tolerance_) {
ROS_WARN_THROTTLE(1.0, "Costmap2DClient transform timeout. Current time: "
"%.4f, global_pose stamp: %.4f, tolerance: %.4f",
current_time.toSec(), global_pose.stamp_.toSec(),
transform_tolerance_);
return {};
}
geometry_msgs::PoseStamped msg;
tf::poseStampedTFToMsg(global_pose, msg);
return msg.pose;
}
std::array<unsigned char, 256> init_translation_table()
{
std::array<unsigned char, 256> cost_translation_table;
// lineary mapped from [0..100] to [0..255]
for (size_t i = 0; i < 256; ++i) {
cost_translation_table[i] =
static_cast<unsigned char>(1 + (251 * (i - 1)) / 97);
}
// special values:
cost_translation_table[0] = 0; // NO obstacle
cost_translation_table[99] = 253; // INSCRIBED obstacle
cost_translation_table[100] = 254; // LETHAL obstacle
cost_translation_table[static_cast<unsigned char>(-1)] = 255; // UNKNOWN
return cost_translation_table;
}
} // namespace explore

308
explore/src/explore.cpp Normal file
View File

@ -0,0 +1,308 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Robert Bosch LLC.
* Copyright (c) 2015-2016, Jiri Horner.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Jiri Horner nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/
#include <explore/explore.h>
#include <thread>
inline static bool operator==(const geometry_msgs::Point& one,
const geometry_msgs::Point& two)
{
double dx = one.x - two.x;
double dy = one.y - two.y;
double dist = sqrt(dx * dx + dy * dy);
return dist < 0.01;
}
namespace explore
{
Explore::Explore()
: private_nh_("~")
, tf_listener_(ros::Duration(10.0))
, costmap_client_(private_nh_, relative_nh_, &tf_listener_)
, move_base_client_("move_base")
, prev_distance_(0)
, last_markers_count_(0)
{
double timeout;
double min_frontier_size;
private_nh_.param("planner_frequency", planner_frequency_, 1.0);
private_nh_.param("progress_timeout", timeout, 30.0);
progress_timeout_ = ros::Duration(timeout);
private_nh_.param("visualize", visualize_, false);
private_nh_.param("potential_scale", potential_scale_, 1e-3);
private_nh_.param("orientation_scale", orientation_scale_, 0.0);
private_nh_.param("gain_scale", gain_scale_, 1.0);
private_nh_.param("min_frontier_size", min_frontier_size, 0.5);
search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
potential_scale_, gain_scale_,
min_frontier_size);
if (visualize_) {
marker_array_publisher_ =
private_nh_.advertise<visualization_msgs::MarkerArray>("frontiers", 10);
}
ROS_INFO("Waiting to connect to move_base server");
move_base_client_.waitForServer();
ROS_INFO("Connected to move_base server");
exploring_timer_ =
relative_nh_.createTimer(ros::Duration(1. / planner_frequency_),
[this](const ros::TimerEvent&) { makePlan(); });
}
Explore::~Explore()
{
stop();
}
void Explore::visualizeFrontiers(
const std::vector<frontier_exploration::Frontier>& frontiers)
{
std_msgs::ColorRGBA blue;
blue.r = 0;
blue.g = 0;
blue.b = 1.0;
blue.a = 1.0;
std_msgs::ColorRGBA red;
red.r = 1.0;
red.g = 0;
red.b = 0;
red.a = 1.0;
std_msgs::ColorRGBA green;
green.r = 0;
green.g = 1.0;
green.b = 0;
green.a = 1.0;
ROS_DEBUG("visualising %lu frontiers", frontiers.size());
visualization_msgs::MarkerArray markers_msg;
std::vector<visualization_msgs::Marker>& markers = markers_msg.markers;
visualization_msgs::Marker m;
m.header.frame_id = costmap_client_.getGlobalFrameID();
m.header.stamp = ros::Time::now();
m.ns = "frontiers";
m.scale.x = 1.0;
m.scale.y = 1.0;
m.scale.z = 1.0;
m.color.r = 0;
m.color.g = 0;
m.color.b = 255;
m.color.a = 255;
// lives forever
m.lifetime = ros::Duration(0);
m.frame_locked = true;
// weighted frontiers are always sorted
double min_cost = frontiers.empty() ? 0. : frontiers.front().cost;
m.action = visualization_msgs::Marker::ADD;
size_t id = 0;
for (auto& frontier : frontiers) {
m.type = visualization_msgs::Marker::POINTS;
m.id = int(id);
m.pose.position = {};
m.scale.x = 0.1;
m.scale.y = 0.1;
m.scale.z = 0.1;
m.points = frontier.points;
if (goalOnBlacklist(frontier.centroid)) {
m.color = red;
} else {
m.color = blue;
}
markers.push_back(m);
++id;
m.type = visualization_msgs::Marker::SPHERE;
m.id = int(id);
m.pose.position = frontier.initial;
// scale frontier according to its cost (costier frontiers will be smaller)
double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5);
m.scale.x = scale;
m.scale.y = scale;
m.scale.z = scale;
m.points = {};
m.color = green;
markers.push_back(m);
++id;
}
size_t current_markers_count = markers.size();
// delete previous markers, which are now unused
m.action = visualization_msgs::Marker::DELETE;
for (; id < last_markers_count_; ++id) {
m.id = int(id);
markers.push_back(m);
}
last_markers_count_ = current_markers_count;
marker_array_publisher_.publish(markers_msg);
}
void Explore::makePlan()
{
// find frontiers
auto pose = costmap_client_.getRobotPose();
// get frontiers sorted according to cost
auto frontiers = search_.searchFrom(pose.position);
ROS_DEBUG("found %lu frontiers", frontiers.size());
for (size_t i = 0; i < frontiers.size(); ++i) {
ROS_DEBUG("frontier %zd cost: %f", i, frontiers[i].cost);
}
if (frontiers.empty()) {
stop();
return;
}
// publish frontiers as visualization markers
if (visualize_) {
visualizeFrontiers(frontiers);
}
// find non blacklisted frontier
auto frontier =
std::find_if_not(frontiers.begin(), frontiers.end(),
[this](const frontier_exploration::Frontier& f) {
return goalOnBlacklist(f.centroid);
});
if (frontier == frontiers.end()) {
stop();
return;
}
geometry_msgs::Point target_position = frontier->centroid;
// time out if we are not making any progress
bool same_goal = prev_goal_ == target_position;
prev_goal_ = target_position;
if (!same_goal || prev_distance_ > frontier->min_distance) {
// we have different goal or we made some progress
last_progress_ = ros::Time::now();
prev_distance_ = frontier->min_distance;
}
// black list if we've made no progress for a long time
if (ros::Time::now() - last_progress_ > progress_timeout_) {
frontier_blacklist_.push_back(target_position);
ROS_DEBUG("Adding current goal to black list");
makePlan();
return;
}
// we don't need to do anything if we still pursuing the same goal
if (same_goal) {
return;
}
// send goal to move_base if we have something new to pursue
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position = target_position;
goal.target_pose.pose.orientation.w = 1.;
goal.target_pose.header.frame_id = costmap_client_.getGlobalFrameID();
goal.target_pose.header.stamp = ros::Time::now();
move_base_client_.sendGoal(
goal, [this, target_position](
const actionlib::SimpleClientGoalState& status,
const move_base_msgs::MoveBaseResultConstPtr& result) {
reachedGoal(status, result, target_position);
});
}
bool Explore::goalOnBlacklist(const geometry_msgs::Point& goal)
{
constexpr static size_t tolerace = 5;
costmap_2d::Costmap2D* costmap2d = costmap_client_.getCostmap();
// check if a goal is on the blacklist for goals that we're pursuing
for (auto& frontier_goal : frontier_blacklist_) {
double x_diff = fabs(goal.x - frontier_goal.x);
double y_diff = fabs(goal.y - frontier_goal.y);
if (x_diff < tolerace * costmap2d->getResolution() &&
y_diff < tolerace * costmap2d->getResolution())
return true;
}
return false;
}
void Explore::reachedGoal(const actionlib::SimpleClientGoalState& status,
const move_base_msgs::MoveBaseResultConstPtr&,
const geometry_msgs::Point& frontier_goal)
{
ROS_DEBUG("Reached goal with status: %s", status.toString().c_str());
if (status == actionlib::SimpleClientGoalState::ABORTED) {
frontier_blacklist_.push_back(frontier_goal);
ROS_DEBUG("Adding current goal to black list");
}
// find new goal immediatelly regardless of planning frequency.
// execute via timer to prevent dead lock in move_base_client (this is
// callback for sendGoal, which is called in makePlan). the timer must live
// until callback is executed.
oneshot_ = relative_nh_.createTimer(
ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); },
true);
}
void Explore::start()
{
exploring_timer_.start();
}
void Explore::stop()
{
move_base_client_.cancelAllGoals();
exploring_timer_.stop();
ROS_INFO("Exploration stopped.");
}
} // namespace explore
int main(int argc, char** argv)
{
ros::init(argc, argv, "explore");
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
ros::console::levels::Debug)) {
ros::console::notifyLoggerLevelsChanged();
}
explore::Explore explore;
ros::spin();
return 0;
}

197
explore/src/frontier_search.cpp Normal file
View File

@ -0,0 +1,197 @@
#include <explore/frontier_search.h>
#include <mutex>
#include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/Point.h>
#include <explore/costmap_tools.h>
namespace frontier_exploration
{
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;
using costmap_2d::FREE_SPACE;
FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap,
double potential_scale, double gain_scale,
double min_frontier_size)
: costmap_(costmap)
, potential_scale_(potential_scale)
, gain_scale_(gain_scale)
, min_frontier_size_(min_frontier_size)
{
}
std::vector<Frontier> FrontierSearch::searchFrom(geometry_msgs::Point position)
{
std::vector<Frontier> frontier_list;
// Sanity check that robot is inside costmap bounds before searching
unsigned int mx, my;
if (!costmap_->worldToMap(position.x, position.y, mx, my)) {
ROS_ERROR("Robot out of costmap bounds, cannot search for frontiers");
return frontier_list;
}
// make sure map is consistent and locked for duration of search
std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
map_ = costmap_->getCharMap();
size_x_ = costmap_->getSizeInCellsX();
size_y_ = costmap_->getSizeInCellsY();
// initialize flag arrays to keep track of visited and frontier cells
std::vector<bool> frontier_flag(size_x_ * size_y_, false);
std::vector<bool> visited_flag(size_x_ * size_y_, false);
// initialize breadth first search
std::queue<unsigned int> bfs;
// find closest clear cell to start search
unsigned int clear, pos = costmap_->getIndex(mx, my);
if (nearestCell(clear, pos, FREE_SPACE, *costmap_)) {
bfs.push(clear);
} else {
bfs.push(pos);
ROS_WARN("Could not find nearby clear cell to start search");
}
visited_flag[bfs.front()] = true;
while (!bfs.empty()) {
unsigned int idx = bfs.front();
bfs.pop();
// iterate over 4-connected neighbourhood
for (unsigned nbr : nhood4(idx, *costmap_)) {
// add to queue all free, unvisited cells, use descending search in case
// initialized on non-free cell
if (map_[nbr] <= map_[idx] && !visited_flag[nbr]) {
visited_flag[nbr] = true;
bfs.push(nbr);
// check if cell is new frontier cell (unvisited, NO_INFORMATION, free
// neighbour)
} else if (isNewFrontierCell(nbr, frontier_flag)) {
frontier_flag[nbr] = true;
Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag);
if (new_frontier.size * costmap_->getResolution() >=
min_frontier_size_) {
frontier_list.push_back(new_frontier);
}
}
}
}
// set costs of frontiers
for (auto& frontier : frontier_list) {
frontier.cost = frontierCost(frontier);
}
std::sort(
frontier_list.begin(), frontier_list.end(),
[](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; });
return frontier_list;
}
Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell,
unsigned int reference,
std::vector<bool>& frontier_flag)
{
// initialize frontier structure
Frontier output;
output.centroid.x = 0;
output.centroid.y = 0;
output.size = 1;
output.min_distance = std::numeric_limits<double>::infinity();
// record initial contact point for frontier
unsigned int ix, iy;
costmap_->indexToCells(initial_cell, ix, iy);
costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y);
// push initial gridcell onto queue
std::queue<unsigned int> bfs;
bfs.push(initial_cell);
// cache reference position in world coords
unsigned int rx, ry;
double reference_x, reference_y;
costmap_->indexToCells(reference, rx, ry);
costmap_->mapToWorld(rx, ry, reference_x, reference_y);
while (!bfs.empty()) {
unsigned int idx = bfs.front();
bfs.pop();
// try adding cells in 8-connected neighborhood to frontier
for (unsigned int nbr : nhood8(idx, *costmap_)) {
// check if neighbour is a potential frontier cell
if (isNewFrontierCell(nbr, frontier_flag)) {
// mark cell as frontier
frontier_flag[nbr] = true;
unsigned int mx, my;
double wx, wy;
costmap_->indexToCells(nbr, mx, my);
costmap_->mapToWorld(mx, my, wx, wy);
geometry_msgs::Point point;
point.x = wx;
point.y = wy;
output.points.push_back(point);
// update frontier size
output.size++;
// update centroid of frontier
output.centroid.x += wx;
output.centroid.y += wy;
// determine frontier's distance from robot, going by closest gridcell
// to robot
double distance = sqrt(pow((double(reference_x) - double(wx)), 2.0) +
pow((double(reference_y) - double(wy)), 2.0));
if (distance < output.min_distance) {
output.min_distance = distance;
output.middle.x = wx;
output.middle.y = wy;
}
// add to queue for breadth first search
bfs.push(nbr);
}
}
}
// average out frontier centroid
output.centroid.x /= output.size;
output.centroid.y /= output.size;
return output;
}
bool FrontierSearch::isNewFrontierCell(unsigned int idx,
const std::vector<bool>& frontier_flag)
{
// check that cell is unknown and not already marked as frontier
if (map_[idx] != NO_INFORMATION || frontier_flag[idx]) {
return false;
}
// frontier cells should have at least one cell in 4-connected neighbourhood
// that is free
for (unsigned int nbr : nhood4(idx, *costmap_)) {
if (map_[nbr] == FREE_SPACE) {
return true;
}
}
return false;
}
double FrontierSearch::frontierCost(const Frontier& frontier)
{
return (potential_scale_ * frontier.min_distance *
costmap_->getResolution()) -
(gain_scale_ * frontier.size * costmap_->getResolution());
}
}

BIN
img/exploration.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 646 KiB

BIN
img/fcpp.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 21 KiB

BIN
img/result.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 725 KiB

BIN
img/structure.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 63 KiB

BIN
ros-clean-robot.pdf Normal file
View File

Binary file not shown.