Skip to content

Commit df65e1f

Browse files
OphienOphien
authored
After doing some experimentations with ROS2 with nav2 and SLAM-Toolbox, I'm uploading some findings into this ROS1 sandbox version I use as a 'sandbox' or testbed for multi-robot stuff (#7)
- Increased number of via poses feed into the controller for faster robots - Added an exclusive cspace for path planning to prevent robots being stuck - Fix issues with the path planning, where only 6 neighbours were being visited - Fix issues with the nearest goalpoint path finding algorithm to prevent robots being stuck - Added a more structured sandbox map - Updated readme sandbox development notice
1 parent f1d2951 commit df65e1f

File tree

8 files changed

+5101
-107
lines changed

8 files changed

+5101
-107
lines changed

‎README.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616

1717
## [Development Notice](#development-notice)
1818

19-
Further development is being done on the ROS 2 version at: [https://github.com/multirobotplayground/Jazzy-Multi-Robot-Sandbox](https://github.com/multirobotplayground/Jazzy-Multi-Robot-Sandbox).
19+
I'm going to continue using this repository as a testbed, or the name say 'Sandbox', to validade some ideas for multi-robot coordination and control. However, I'm also working in a ROS 2 version integrated with SLAM-ToolBox and Nav2 at: [https://github.com/multirobotplayground/Jazzy-Multi-Robot-Sandbox](https://github.com/multirobotplayground/Jazzy-Multi-Robot-Sandbox).
2020

2121
## [ROS-Noetic-Multi-robot-Sandbox](#ros-noetic-multi-robot-sandbox)
2222

‎gazebo_resources/worlds/sandbox.world‎

Lines changed: 4861 additions & 0 deletions
Large diffs are not rendered by default.

‎src/multirobotexploration/launch/exploration_stack_bringup.launch‎

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -176,6 +176,7 @@
176176
<param name="rate" type="double" value="1"/>
177177
<param name="max_lidar_range" type="double" value="10.0"/>
178178
<param name="queue_size" type="int" value="2" />
179+
<remap from="/robot_$(arg robot_id)/c_space" to="/robot_$(arg robot_id)/c_space_path_plan"/>
179180
</node>
180181

181182
<node name="node_averagevelocityestimator" pkg="multirobotexploration" type="averagevelocityestimatornode" output="screen">
@@ -208,6 +209,18 @@
208209
<remap from="/robot_$(arg robot_id)/map" to="/robot_$(arg robot_id)/fusion"/>
209210
</node>
210211

212+
<node name="node_cspace_path_plan" pkg="multirobotexploration" type="cspacenode" output="screen">
213+
<param name="id" type="int" value="$(arg robot_id)"/>
214+
<param name="queue_size" type="int" value="2"/>
215+
<param name="rate" type="double" value="1"/>
216+
<param name="max_lidar_range" type="double" value="10.0"/>
217+
<param name="free_inflation_radius" type="double" value="0.5"/>
218+
<param name="ocu_inflation_radius" type="double" value="1.0"/>
219+
<param name="lidar_sources" type="int" value="4"/>
220+
<remap from="/robot_$(arg robot_id)/map" to="/robot_$(arg robot_id)/fusion"/>
221+
<remap from="/robot_$(arg robot_id)/c_space" to="/robot_$(arg robot_id)/c_space_path_plan"/>
222+
</node>
223+
211224
<node name="node_localcspace" pkg="multirobotexploration" type="localcspacenode" output="screen">
212225
<param name="id" type="int" value="$(arg robot_id)"/>
213226
<param name="rate" type="double" value="15"/>
@@ -239,7 +252,7 @@
239252
<param name="via_points_increment" type="int" value="3"/>
240253
<param name="rate" type="double" value="15.0"/>
241254
<param name="controls_to_share" type="int" value="10"/>
242-
<param name="use_priority_stop_behavior" value="False"/>
255+
<param name="use_priority_stop_behavior" value="True"/>
243256
<param name="odom_topic" value="/robot_$(arg robot_id)/odom"/>
244257
<param name="map_frame" value="robot_$(arg robot_id)/map"/>
245258
<param name="max_vel_x" value="0.5"/>

‎src/multirobotexploration/rviz/robot_0_full.rviz‎

Lines changed: 15 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -3,9 +3,10 @@ Panels:
33
Help Height: 0
44
Name: Displays
55
Property Tree Widget:
6-
Expanded: ~
6+
Expanded:
7+
- /Global Options1
78
Splitter Ratio: 0.5
8-
Tree Height: 898
9+
Tree Height: 441
910
- Class: rviz/Selection
1011
Name: Selection
1112
- Class: rviz/Tool Properties
@@ -101,7 +102,7 @@ Visualization Manager:
101102
Marker Topic: /robot_0/relative_pose_estimator/pose_far_markers
102103
Name: far_robots
103104
Namespaces:
104-
{}
105+
/robot_0: true
105106
Queue Size: 100
106107
Value: true
107108
- Class: rviz/Marker
@@ -610,23 +611,25 @@ Visualization Manager:
610611
Marker Topic: /robot_0/local_planner/global_via_points
611612
Name: Marker
612613
Namespaces:
613-
{}
614+
/robot_0: true
614615
Queue Size: 100
615616
Value: true
616617
- Class: rviz/Marker
617618
Enabled: true
618619
Marker Topic: /robot_0/local_planner/teb_markers
619620
Name: Marker
620621
Namespaces:
621-
{}
622+
PolyObstacles: true
623+
TebContainer: true
624+
ViaPoints: true
622625
Queue Size: 100
623626
Value: true
624627
Enabled: true
625628
Global Options:
626629
Background Color: 48; 48; 48
627630
Default Light: true
628631
Fixed Frame: robot_0/map
629-
Frame Rate: 30
632+
Frame Rate: 244
630633
Name: root
631634
Tools:
632635
- Class: rviz/Interact
@@ -647,25 +650,25 @@ Visualization Manager:
647650
Views:
648651
Current:
649652
Class: rviz/Orbit
650-
Distance: 16.228466033935547
653+
Distance: 12.91609001159668
651654
Enable Stereo Rendering:
652655
Stereo Eye Separation: 0.05999999865889549
653656
Stereo Focal Distance: 1
654657
Swap Stereo Eyes: false
655658
Value: false
656659
Field of View: 0.7853981852531433
657660
Focal Point:
658-
X: 2.9160966873168945
659-
Y: 2.728337287902832
660-
Z: -0.4948226809501648
661+
X: 2.9622278213500977
662+
Y: 3.851478099822998
663+
Z: -0.30416473746299744
661664
Focal Shape Fixed Size: false
662665
Focal Shape Size: 0.05000000074505806
663666
Invert Z Axis: false
664667
Name: Current View
665668
Near Clip Distance: 0.009999999776482582
666-
Pitch: 0.5697975754737854
669+
Pitch: 1.4097964763641357
667670
Target Frame: <Fixed Frame>
668-
Yaw: 1.0299559831619263
671+
Yaw: 0.019956249743700027
669672
Saved: ~
670673
Window Geometry:
671674
Displays:

‎src/multirobotexploration/source/common/SearchAlgorithms.cpp‎

Lines changed: 25 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include "ros/ros.h"
2121
#include <queue>
2222
#include <set>
23+
#include <cmath>
2324

2425
std::mt19937* randomglobal() {
2526
static std::random_device rd;
@@ -138,7 +139,7 @@ namespace sa {
138139
// iterate over children
139140
for(int x = 0; x < 3; ++x) {
140141
for(int y = 0; y < 3; ++y) {
141-
if(x == y) continue;
142+
if(x == 1 && y == 1) continue;
142143
children = Vec2i::Create(current.x - 1 + x, current.y - 1 + y);
143144
// only add children that were not visited
144145
if(IsInBounds(rInput, children) && control[children.y][children.x].visi == false) {
@@ -177,6 +178,18 @@ namespace sa {
177178
// in the frontier discovery
178179
rOutPath.clear();
179180

181+
// validate input bounds
182+
if(!IsInBounds(rInput, const_cast<Vec2i&>(rStart)) ||
183+
!IsInBounds(rInput, const_cast<Vec2i&>(rEnd))) {
184+
return;
185+
}
186+
187+
// check if start and end are passable
188+
if(rInput.data[rStart.y*rInput.info.width+rStart.x] > 50 ||
189+
rInput.data[rEnd.y*rInput.info.width+rEnd.x] > 50) {
190+
return;
191+
}
192+
180193
// initialize distances and predecessors
181194
// using struct with all elements to optimize
182195
// cache hits
@@ -198,8 +211,6 @@ namespace sa {
198211

199212
// do search
200213
while(q.size() > 0) {
201-
// TODO:swap the extract min function to
202-
// be more optimized
203214
current = q.top().pos;
204215
q.pop();
205216

@@ -220,23 +231,23 @@ namespace sa {
220231
children = Vec2i::Create(current.x - 1 + x, current.y - 1 + y);
221232
if(IsInBounds(rInput, children)
222233
&& control[children.y][children.x].visi == false
223-
&& rInput.data[children.y*rInput.info.width+children.x] >= 0
224-
&& rInput.data[children.y*rInput.info.width+children.x] < 50) {
225-
// compute distance and heuristic to parent
226-
// and end points
227-
dist = control[current.y][current.x].dist + Distance(current, children);
228-
heuristic = Distance(rEnd, children);
234+
&& rInput.data[children.y*rInput.info.width+children.x] <= 90) {
235+
236+
// compute actual distance (sqrt of squared distance) for correct A*
237+
double edge_cost = sqrt(Distance(current, children));
238+
dist = control[current.y][current.x].dist + edge_cost;
239+
heuristic = sqrt(Distance(rEnd, children));
229240
distance_metric = dist + heuristic;
230241

231-
// relax edge
242+
// relax edge - only add to queue if we found a better path
232243
if(control[children.y][children.x].dist > distance_metric) {
233244
control[children.y][children.x].dist = distance_metric;
234245
control[children.y][children.x].pred = Vec2i::Create(current.x, current.y);
246+
247+
// only add to queue when we update the distance
248+
el element(children, distance_metric);
249+
q.push(element);
235250
}
236-
237-
// if not on the heap, add to the heap
238-
el element(children, control[children.y][children.x].dist);
239-
q.push(element);
240251
}
241252
}
242253
}

0 commit comments

Comments
 (0)