10. rrt_exploration mapping algorithm
10. rrt_exploration mapping algorithm10.1 Introduction 10.2 Use 10.2.1 Start 10.2.2 Mapping 10.2.3 Map save 10.2.4 Node View 10.2.5 View tf tree 10.3 global_rrt_frontier_detector 10.3.1. Introduction 10.3.2 Topics and Services 10.3.3 Configuration parameters 10.4 local_rrt_frontier_detector 10.4.1 Introduction 10.4.2 Topics and Services 10.4.3 Configuration parameters 10.5 frontier_opencv_detector 10.5.1 Introduction 10.5.2 Topics and Services 10.5.3 Configuration parameters 10.6 filter 10.6.1 Introduction 10.6.2 Topics and Services 10.6.3 Configuration parameters 10.7 Assign 10.7.1 Introduction 10.7.2 Topics and Services 10.7.3 Configuration parameters
rrt_exploration： http://wiki.ros.org/rrt_exploration
rrt_exploration/Tutorials： http://wiki.ros.org/rrt_exploration/Tutorials
map_server： https://wiki.ros.org/map_server
10.1 Introduction
RRT exploration is a search algorithm implemented based on the RRT path planning algorithm. The reason why the RRT algorithm is used is that RRT has a strong tendency to unknown areas. In RRT exploration, RRT is mainly used to generate boundary points, which is very beneficial for exploring boundary points. The so-called boundary point is the junction point between the explored and unknown areas. Here is a definition: for all areas, if they are explored, the area without obstacles is recorded as 0, and the area with obstacles is recorded as 1, the unknown area is recorded as -1, and at the beginning, the entire area is recorded as -1.
The framework of the Rapid Exploration Random Tree(RRT) algorithm is as follows:
- Global RRT frontier point detector node.
- Local RRT frontier point detector node.
- OpenCV-based frontier detector node.
- Filter node.
- Assign node.
There are 3 types of nodes: nodes used to detect boundary points in the occupancy raster map, nodes used to filter detected points, and nodes used to assign points to robots.
10.2 Use
Note: When building a map, the slower the speed, the better the effect(note that if the rotation speed is slower), the effect will be poor if the speed is too fast.
According to different models, you only need to set the purchased model in [.bashrc], X1(ordinary four-wheel drive) X3(Mike wheel) X3plus(Mike wheel mechanical arm) R2(Ackerman differential) and so on. Section takes X3 as an example
Open the [.bashrc] file
sudo vim .bashrc
Find the [ROBOT_TYPE] parameter and modify the corresponding model
xxxxxxxxxxexport ROBOT_TYPE=X3 # ROBOT_TYPE: X1 X3 X3plus R2 X7
10.2.1 Start
Start command(robot side), for the convenience of operation, this section takes [mono + laser + yahboomcar] as an example.
xxxxxxxxxxroslaunch yahboomcar_nav laser_bringup.launch # laser + yahboomcar roslaunch yahboomcar_nav laser_usb_bringup.launch # mono + laser + yahboomcar roslaunch yahboomcar_nav laser_astrapro_bringup.launch # Astra + laser + yahboomcar Mapping command(robot side)
xxxxxxxxxxroslaunch yahboomcar_nav rrt_exploration.launch use_rviz:=false
- [use_rviz] parameter: whether to enable rviz visualization.
Open the visual interface(virtual machine side)
xxxxxxxxxxroslaunch yahboomcar_nav view_rrt_map.launch
The gap at the back of the robot is due to the occlusion of the installation position of the display screen, so a certain range of Lidar data is shielded. The shielding range can be adjusted or not shielded according to the actual situation. For details, see [01. Lidar Basic Course].
10.2.2 Mapping
Click [Publish Point] on the [rviz] interface, and then click on the map. Each time you click a point on the map, you must first select [Publish Point]. the clockwise or counterclockwise last point is near the car. After selecting five points as required, the robot begins to explore and build a map.
10.2.3 Map save
After the construction is completed, the map is automatically saved and returned to zero. The map is saved to the folder ~/yahboomcar_ws/src/yahboomcar_nav/maps with the name rrt_map, which can be modified in the [simple.launch] file.
rrt_map.yaml
xxxxxxxxxximage map.pgm resolution 0.05 origin -15.4 -12.2 0.0 negate 0 occupied_thresh 0.65 free_thresh： 0.196 Parameter parsing:
- image: The path of the map file, which can be an absolute path or a relative path
- resolution: the resolution of the map, m/pixel
- origin: The 2D pose(x, y, yaw) of the lower left corner of the map, where yaw is rotated counterclockwise(yaw=0 means no rotation). Many parts of the system currently ignore the yaw value.
- negate: whether to reverse the meaning of white/black, free/occupy(the interpretation of the threshold is not affected)
- occupied_thresh: Pixels with an occupancy probability greater than this threshold will be considered fully occupied.
- free_thresh: Pixels with an occupancy probability less than this threshold will be considered completely free.
10.2.4 Node View
xxxxxxxxxxrqt_graph
10.2.5 View tf tree
xxxxxxxxxxrosrun rqt_tf_tree rqt_tf_tree
10.3 global_rrt_frontier_detector
10.3.1. Introduction
The global_rrt_frontier_detector node takes an occupancy grid and finds frontier points(i.e. exploration targets) in it. It publishes detected points so that filter nodes can process them. In a multi-robot configuration, only one instance of the node is running. If desired, running additional instances of the global boundary detector can increase the speed of boundary point detection.
10.3.2 Topics and Services
| Subscribe to topics | type | describe |
|---|---|---|
| map | nav_msgs/OccupancyGrid | Topic names are defined by the ~map_Topic parameter. It is the name of the topic that the node receives. |
| clicked_point | geometry_msgs/PointStamped | The area to be probed by the global_rrt_frontier_detector node. This topic is where the node receives the five points that define the area. The first four points are the four points that define the square area to be explored, and the last point is the starting point of the tree. After these five points are announced, the RRT will begin to probe the border points. |
| Post a topic | type | describe |
| detected_points | geometry_msgs/PointStamped | Post the topic of detected boundary points. |
| shapes | visualization_msgs/Marker | Post RRT hypothetical linetypes to view with Rviz. |
10.3.3 Configuration parameters
| parameter | type | Defaults | describe |
|---|---|---|---|
| ~map_topic | string | "/robot_1/map" | This node receives the map topic map name |
| ~ eta | float | 5.0 | This parameter controls the growth rate of the RRT used to detect boundary points, in meters. This parameter should be set according to the map size, a very large value will cause the tree to grow faster and thus detect border points faster, but a larger growth rate also means that the tree will lose small corner points in the map |
10.4 local_rrt_frontier_detector
10.4.1 Introduction
This node is similar to global_rrt_frontier_detector. However, it works differently because here the tree keeps getting reset every time a boundary point is detected. This node will run along the Global Boundary Detector node, which is responsible for quickly detecting boundary points located near the robot.
In a multi-robot configuration, each robot runs a local probe instance. So for a team of 3 robots, there will be 4 nodes for detecting boundary points; 3 local detectors and 1 global detector. If needed, running additional instances of the local boundary detector can increase the speed of boundary point detection. All detectors will publish detected boundary points("/detected_points") on the same topic.
10.4.2 Topics and Services
| Subscribe to topics | type | describe |
|---|---|---|
| map | nav_msgs/OccupancyGrid | The name of the map topic to which this node is subscribed. |
| clicked_point | geometry_msgs/PointStamped | Similar to global_rrt_frontier_detector node |
| Post a topic | type | describe |
| detected_points | geometry_msgs/PointStamped | Post the topic of detected boundary points. |
| shapes | visualization_msgs/Marker | Post RRT hypothetical linetypes to view with Rviz. |
10.4.3 Configuration parameters
| parameter | type | Defaults | describe |
|---|---|---|---|
| ~/robot_1/base_link | string | "/robot_1/base_link" | Connect to the frame of the robot. Every time the RRT tree is reset, it will start from the current robot position obtained in this coordinate system. |
| ~map_topic | string | "/robot_1/map" | This node receives the map topic map name |
| ~ eta | float | 5.0 | This parameter controls the growth rate of the RRT used to detect boundary points, in meters. This parameter should be set according to the map size, a very large value will cause the tree to grow faster and thus detect border points faster, but a larger growth rate also means that the tree will lose small corner points in the map |
10.5 frontier_opencv_detector
10.5.1 Introduction
This node is another edge detector, but it is not based on RRT. This node uses OpenCV tools to detect boundary points. It is designed to run alone, in a multi-bot configuration, only one instance should be running(running other instances of this node will not make any difference).
Originally, this node was implemented for comparison with RRT-based boundary detectors. Running this node along the RRT detectors(local and global) can increase the speed of boundary point detection.
Note: You can run any type and number of detectors, all of which will be published on the same topic that the filter node(explained in the next section) subscribes to. The filter, on the other hand, passes the filtered boundary points to the assignor in order to command the robot to explore these points.
10.5.2 Topics and Services
| Subscribe to topics | type | describe |
|---|---|---|
| map | nav_msgs/OccupancyGrid | The name of the map topic to which this node is subscribed. |
| Post a topic | type | describe |
| detected_points | geometry_msgs/PointStamped | Post the topic of detected boundary points. |
| shapes | visualization_msgs/Marker | Post RRT hypothetical linetypes to view with Rviz. |
10.5.3 Configuration parameters
| parameter | type | Defaults | describe |
|---|---|---|---|
| ~map_topic | string | "/robot_1/map" | This node receives the map topic map name |
10.6 filter
10.6.1 Introduction
This node node receives detected boundary points from all detectors, filters these points, and passes them to the assign node to command the robot. Filtering includes removing old and invalid points, as well as removing redundant points.
10.6.2 Topics and Services
| Subscribe to topics | type | describe |
|---|---|---|
| map | nav_msgs/OccupancyGrid | Topic names are defined by the ~map_Topic parameter. It is the name of the topic that the node receives. |
| robot_x/move_base_node /global_costmap/costmap | nav_msgs/OccupancyGrid | x is the number of the robot. This node subscribes to the topic of all costmaps for all robots, so costmaps are required. Normally, costmaps should be published by navigation(there will be one costmap per robot when navigation is turned on on a robot). For example, if n_robots=2, the nodes will subscribe to: robot_1/move_base_node/global_costmap/costmap and robot_2/move_base_node/global_costmap/costmap. The costmap is used to remove invalid points. Note: The namespace of all nodes corresponding to robots should start with robot_x. |
| detected_points | geometry_msgs/PointStamped | The topic name defined by ~goals_topic. It is the topic on which the filter node receives boundary detection points. |
| Post a topic | type | describe |
| frontiers | visualization_msgs/Marker | The filter node only publishes topics of filtered boundary points. |
| centroids | visualization_msgs/Marker | The filter node publishes the topic of the received boundary points. |
| filtered_points | MsgLink(msg/type) | All filtered points are sent to the assigner node of this topic as an array of points. |
10.6.3 Configuration parameters
| parameter | type | Defaults | describe |
|---|---|---|---|
| ~map_topic | string | "/robot_1/map" | This node receives the map topic map name |
| ~costmap_clearing_threshold | float | 70.0 | Costmap cleanup threshold |
| ~info_radius | float | 1.0 | The information radius used to calculate the information gain of the boundary point. |
| ~goals_topic | string | /detected_points | Defines the topic the node receives to detect boundary points |
| ~n_robots | float | 1.0 | Number of robots |
| ~namespace | string | Namespaces | |
| ~namespace_init_count | float | 1.0 | index of namespace |
| ~rate | float | 100.0 | Node cycle rate(in Hz). |
10.7 Assign
10.7.1 Introduction
This node receives target detection targets, i.e. filtered boundary points issued by the filtering node, and commands the robot accordingly. The evaluator node commands the robot via move_base_node. That's why navigation is initiated on the robot.
10.7.2 Topics and Services
| Subscribe to topics | type | describe |
|---|---|---|
| map | nav_msgs/OccupancyGrid | Topic names are defined by the ~map_Topic parameter. It is the name of the topic that the node receives. |
| frontiers_topic | nav_msgs/OccupancyGrid | The topic name is defined by the ~frontiers_topic parameter |
10.7.3 Configuration parameters
| parameter | type | Defaults | describe |
|---|---|---|---|
| ~map_topic | string | "/robot_1/map" | This node receives the map topic map name |
| ~info_radius | float | 1.0 | The information radius used to calculate the information gain of the boundary point. |
| ~info_multiplier | float | 3.0 | The unit is meters. This parameter is used to emphasize the importance of the information gain of the boundary point relative to the cost (expected travel distance to the boundary point). |
| ~hysteresis_radius | float | 3.0 | The unit is meters. This parameter defines the lag radius. |
| ~hysteresis_gain | float | 2.0 | The unit is meters. This parameter defines the hysteresis gain. |
| ~frontiers_topic | string | /filtered_points | This node receives topics for boundary points. |
| ~n_robots | float | 1.0 | Number of robots |
| ~namespace | string | Namespaces | |
| ~namespace_init_count | float | 1.0 | Starting indexing of bot names. |
| ~delay_after_assignement | float | 0.5 | The unit is seconds. It defines the amount of delay after each robot assignment. |
| ~global_frame | string | "/map" | for the global coordinate system. In a single bot, it is the same as the "map_topic" parameter. In the case of multiple robots, the coordinate system name corresponds to the global coordinate system name. |