├── LICENSE
├── MORSE
├── README.md
├── mapping.launch
├── mapping.rviz
├── mapping.sh
├── multirobot.py
├── pr2.urdf
├── scenarios.png
├── scenarios
│ ├── cross.blend
│ ├── cross.pgm
│ ├── cross.yaml
│ ├── loop.blend
│ ├── loop.pgm
│ ├── loop.yaml
│ ├── maze.blend
│ ├── maze.pgm
│ ├── maze.yaml
│ ├── zigzag.blend
│ ├── zigzag.pgm
│ └── zigzag.yaml
├── singlerobot.py
└── test.sh
├── README.md
├── ROS
└── README.md
├── architecture.png
├── exploration
├── README.md
├── frame.png
├── maze_1robots.py
├── maze_7robots.py
├── multi_robot.launch
├── pioneer3dx.launch
└── yaml
│ ├── costmap_common_p3dx.yaml
│ ├── explore_costmap.yaml
│ ├── global_costmap.yaml
│ ├── local_costmap.yaml
│ ├── navfn.yaml
│ └── trajectory_planner.yaml
├── scripts
├── README.md
├── clean
│ └── remove_all_in_scratch.sh
├── log_robot_performance.sh
├── start_simulation.sh
├── submit_job.sh
└── vm_setup_main.sh
└── tutorial
├── demo_28112013.mp4
└── tutorial.pdf
/LICENSE:
--------------------------------------------------------------------------------
1 | BSD 3-Clause License
2 |
3 | Copyright (c) 2020, Zhi Yan
4 | All rights reserved.
5 |
6 | Redistribution and use in source and binary forms, with or without
7 | modification, are permitted provided that the following conditions are met:
8 |
9 | 1. Redistributions of source code must retain the above copyright notice, this
10 | list of conditions and the following disclaimer.
11 |
12 | 2. Redistributions in binary form must reproduce the above copyright notice,
13 | this list of conditions and the following disclaimer in the documentation
14 | and/or other materials provided with the distribution.
15 |
16 | 3. Neither the name of the copyright holder nor the names of its
17 | contributors may be used to endorse or promote products derived from
18 | this software without specific prior written permission.
19 |
20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 |
--------------------------------------------------------------------------------
/MORSE/README.md:
--------------------------------------------------------------------------------
1 | Inspired by the RoboCup Rescue competition, We provide here four experimental terrains created with the same size but different topological structures:
2 |
3 | 
4 |
5 | * The `loop` terrain has a low obstacle density and a simple obstacle shape, in which there is no road fork (similar to beltway).
6 | * The `cross` terrain contains five road forks but the obstacle density is still low (similar to crossroad).
7 | * The `zigzag` terrain has no road fork but more obstacles, and it has a long solution path for the robot (similar to square-grid street).
8 | * The `maze` terrain is the most complex which contains many obstacles and dead ends (similar to whole city).
9 |
10 | ## How to play? ##
11 |
12 | * Two simulated Pioneer P3-DX robots within the `maze` environment: `./test.sh scenarios/maze.blend`
13 |
14 | ## How to build a map? ##
15 |
16 | * A simulated PR2 robots within the `zigzag` environment: `./mapping.sh scenarios/zigzag.blend`
17 | * Set Blender to active window, use the arrow keys on the keyboard to control the robot.
18 | * Inspect RVIZ window, save the map when you satisfy: `$ rosrun map_server map_saver -f map`
19 |
--------------------------------------------------------------------------------
/MORSE/mapping.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
--------------------------------------------------------------------------------
/MORSE/mapping.rviz:
--------------------------------------------------------------------------------
1 | Panels:
2 | - Class: rviz/Displays
3 | Help Height: 78
4 | Name: Displays
5 | Property Tree Widget:
6 | Expanded:
7 | - /Global Options1
8 | - /Status1
9 | - /TF1/Status1
10 | Splitter Ratio: 0.608696
11 | Tree Height: 495
12 | - Class: rviz/Selection
13 | Name: Selection
14 | - Class: rviz/Tool Properties
15 | Expanded:
16 | - /2D Pose Estimate1
17 | - /2D Nav Goal1
18 | - /Publish Point1
19 | Name: Tool Properties
20 | Splitter Ratio: 0.588679
21 | - Class: rviz/Views
22 | Expanded:
23 | - /Current View1
24 | Name: Views
25 | Splitter Ratio: 0.5
26 | - Class: rviz/Time
27 | Experimental: false
28 | Name: Time
29 | SyncMode: 0
30 | SyncSource: Scan
31 | Visualization Manager:
32 | Class: ""
33 | Displays:
34 | - Alpha: 0.5
35 | Cell Size: 1
36 | Class: rviz/Grid
37 | Color: 160; 160; 164
38 | Enabled: true
39 | Line Style:
40 | Line Width: 0.03
41 | Value: Lines
42 | Name: Grid
43 | Normal Cell Count: 0
44 | Offset:
45 | X: 0
46 | Y: 0
47 | Z: 0
48 | Plane: XY
49 | Plane Cell Count: 10
50 | Reference Frame:
51 | Value: true
52 | - Class: rviz/TF
53 | Enabled: true
54 | Frame Timeout: 15
55 | Frames:
56 | /base_bellow_link:
57 | Value: true
58 | /base_footprint:
59 | Value: true
60 | /base_laser_link:
61 | Value: true
62 | /base_link:
63 | Value: true
64 | /double_stereo_link:
65 | Value: true
66 | /head_mount_kinect_ir_link:
67 | Value: true
68 | /head_mount_kinect_ir_optical_frame:
69 | Value: true
70 | /head_mount_kinect_rgb_link:
71 | Value: true
72 | /head_mount_kinect_rgb_optical_frame:
73 | Value: true
74 | /head_mount_link:
75 | Value: true
76 | /head_mount_prosilica_link:
77 | Value: true
78 | /head_mount_prosilica_optical_frame:
79 | Value: true
80 | /head_plate_frame:
81 | Value: true
82 | /head_tilt_link:
83 | Value: true
84 | /high_def_frame:
85 | Value: true
86 | /high_def_optical_frame:
87 | Value: true
88 | /imu_link:
89 | Value: true
90 | /l_forearm_cam_frame:
91 | Value: true
92 | /l_forearm_cam_optical_frame:
93 | Value: true
94 | /l_forearm_link:
95 | Value: true
96 | /l_forearm_roll_link:
97 | Value: true
98 | /l_gripper_led_frame:
99 | Value: true
100 | /l_gripper_motor_accelerometer_link:
101 | Value: true
102 | /l_gripper_palm_link:
103 | Value: true
104 | /l_gripper_tool_frame:
105 | Value: true
106 | /l_torso_lift_side_plate_link:
107 | Value: true
108 | /l_upper_arm_link:
109 | Value: true
110 | /l_upper_arm_roll_link:
111 | Value: true
112 | /l_wrist_roll_link:
113 | Value: true
114 | /laser_tilt_link:
115 | Value: true
116 | /laser_tilt_mount_link:
117 | Value: true
118 | /map:
119 | Value: true
120 | /narrow_stereo_l_stereo_camera_frame:
121 | Value: true
122 | /narrow_stereo_l_stereo_camera_optical_frame:
123 | Value: true
124 | /narrow_stereo_link:
125 | Value: true
126 | /narrow_stereo_optical_frame:
127 | Value: true
128 | /narrow_stereo_r_stereo_camera_frame:
129 | Value: true
130 | /narrow_stereo_r_stereo_camera_optical_frame:
131 | Value: true
132 | /odom:
133 | Value: true
134 | /projector_wg6802418_child_frame:
135 | Value: true
136 | /projector_wg6802418_frame:
137 | Value: true
138 | /r_forearm_cam_frame:
139 | Value: true
140 | /r_forearm_cam_optical_frame:
141 | Value: true
142 | /r_forearm_link:
143 | Value: true
144 | /r_forearm_roll_link:
145 | Value: true
146 | /r_gripper_led_frame:
147 | Value: true
148 | /r_gripper_motor_accelerometer_link:
149 | Value: true
150 | /r_gripper_palm_link:
151 | Value: true
152 | /r_gripper_tool_frame:
153 | Value: true
154 | /r_torso_lift_side_plate_link:
155 | Value: true
156 | /r_upper_arm_link:
157 | Value: true
158 | /r_upper_arm_roll_link:
159 | Value: true
160 | /r_wrist_roll_link:
161 | Value: true
162 | /sensor_mount_link:
163 | Value: true
164 | /torso_lift_link:
165 | Value: true
166 | /wide_stereo_l_stereo_camera_frame:
167 | Value: true
168 | /wide_stereo_l_stereo_camera_optical_frame:
169 | Value: true
170 | /wide_stereo_link:
171 | Value: true
172 | /wide_stereo_optical_frame:
173 | Value: true
174 | /wide_stereo_r_stereo_camera_frame:
175 | Value: true
176 | /wide_stereo_r_stereo_camera_optical_frame:
177 | Value: true
178 | All Enabled: true
179 | Marker Scale: 1
180 | Name: TF
181 | Show Arrows: true
182 | Show Axes: true
183 | Show Names: true
184 | Tree:
185 | /map:
186 | /odom:
187 | /base_footprint:
188 | /base_link:
189 | /base_bellow_link:
190 | {}
191 | /base_laser_link:
192 | {}
193 | Update Interval: 0
194 | Value: true
195 | - Alpha: 1
196 | Class: rviz/RobotModel
197 | Collision Enabled: false
198 | Enabled: true
199 | Links:
200 | All Links Enabled: true
201 | Expand Joint Details: false
202 | Expand Link Details: false
203 | Expand Tree: false
204 | Link Tree Style: Links in Alphabetic Order
205 | base_bellow_link:
206 | Alpha: 1
207 | Show Axes: false
208 | Show Trail: false
209 | Value: true
210 | base_footprint:
211 | Alpha: 1
212 | Show Axes: false
213 | Show Trail: false
214 | Value: true
215 | base_laser_link:
216 | Alpha: 1
217 | Show Axes: false
218 | Show Trail: false
219 | base_link:
220 | Alpha: 1
221 | Show Axes: false
222 | Show Trail: false
223 | Value: true
224 | bl_caster_l_wheel_link:
225 | Alpha: 1
226 | Show Axes: false
227 | Show Trail: false
228 | Value: true
229 | bl_caster_r_wheel_link:
230 | Alpha: 1
231 | Show Axes: false
232 | Show Trail: false
233 | Value: true
234 | bl_caster_rotation_link:
235 | Alpha: 1
236 | Show Axes: false
237 | Show Trail: false
238 | Value: true
239 | br_caster_l_wheel_link:
240 | Alpha: 1
241 | Show Axes: false
242 | Show Trail: false
243 | Value: true
244 | br_caster_r_wheel_link:
245 | Alpha: 1
246 | Show Axes: false
247 | Show Trail: false
248 | Value: true
249 | br_caster_rotation_link:
250 | Alpha: 1
251 | Show Axes: false
252 | Show Trail: false
253 | Value: true
254 | double_stereo_link:
255 | Alpha: 1
256 | Show Axes: false
257 | Show Trail: false
258 | Value: true
259 | fl_caster_l_wheel_link:
260 | Alpha: 1
261 | Show Axes: false
262 | Show Trail: false
263 | Value: true
264 | fl_caster_r_wheel_link:
265 | Alpha: 1
266 | Show Axes: false
267 | Show Trail: false
268 | Value: true
269 | fl_caster_rotation_link:
270 | Alpha: 1
271 | Show Axes: false
272 | Show Trail: false
273 | Value: true
274 | fr_caster_l_wheel_link:
275 | Alpha: 1
276 | Show Axes: false
277 | Show Trail: false
278 | Value: true
279 | fr_caster_r_wheel_link:
280 | Alpha: 1
281 | Show Axes: false
282 | Show Trail: false
283 | Value: true
284 | fr_caster_rotation_link:
285 | Alpha: 1
286 | Show Axes: false
287 | Show Trail: false
288 | Value: true
289 | head_mount_kinect_ir_link:
290 | Alpha: 1
291 | Show Axes: false
292 | Show Trail: false
293 | Value: true
294 | head_mount_kinect_ir_optical_frame:
295 | Alpha: 1
296 | Show Axes: false
297 | Show Trail: false
298 | head_mount_kinect_rgb_link:
299 | Alpha: 1
300 | Show Axes: false
301 | Show Trail: false
302 | Value: true
303 | head_mount_kinect_rgb_optical_frame:
304 | Alpha: 1
305 | Show Axes: false
306 | Show Trail: false
307 | head_mount_link:
308 | Alpha: 1
309 | Show Axes: false
310 | Show Trail: false
311 | Value: true
312 | head_mount_prosilica_link:
313 | Alpha: 1
314 | Show Axes: false
315 | Show Trail: false
316 | Value: true
317 | head_mount_prosilica_optical_frame:
318 | Alpha: 1
319 | Show Axes: false
320 | Show Trail: false
321 | head_pan_link:
322 | Alpha: 1
323 | Show Axes: false
324 | Show Trail: false
325 | Value: true
326 | head_plate_frame:
327 | Alpha: 1
328 | Show Axes: false
329 | Show Trail: false
330 | Value: true
331 | head_tilt_link:
332 | Alpha: 1
333 | Show Axes: false
334 | Show Trail: false
335 | Value: true
336 | high_def_frame:
337 | Alpha: 1
338 | Show Axes: false
339 | Show Trail: false
340 | high_def_optical_frame:
341 | Alpha: 1
342 | Show Axes: false
343 | Show Trail: false
344 | imu_link:
345 | Alpha: 1
346 | Show Axes: false
347 | Show Trail: false
348 | l_elbow_flex_link:
349 | Alpha: 1
350 | Show Axes: false
351 | Show Trail: false
352 | Value: true
353 | l_forearm_cam_frame:
354 | Alpha: 1
355 | Show Axes: false
356 | Show Trail: false
357 | l_forearm_cam_optical_frame:
358 | Alpha: 1
359 | Show Axes: false
360 | Show Trail: false
361 | l_forearm_link:
362 | Alpha: 1
363 | Show Axes: false
364 | Show Trail: false
365 | Value: true
366 | l_forearm_roll_link:
367 | Alpha: 1
368 | Show Axes: false
369 | Show Trail: false
370 | Value: true
371 | l_gripper_l_finger_link:
372 | Alpha: 1
373 | Show Axes: false
374 | Show Trail: false
375 | Value: true
376 | l_gripper_l_finger_tip_frame:
377 | Alpha: 1
378 | Show Axes: false
379 | Show Trail: false
380 | l_gripper_l_finger_tip_link:
381 | Alpha: 1
382 | Show Axes: false
383 | Show Trail: false
384 | Value: true
385 | l_gripper_led_frame:
386 | Alpha: 1
387 | Show Axes: false
388 | Show Trail: false
389 | l_gripper_motor_accelerometer_link:
390 | Alpha: 1
391 | Show Axes: false
392 | Show Trail: false
393 | Value: true
394 | l_gripper_motor_screw_link:
395 | Alpha: 1
396 | Show Axes: false
397 | Show Trail: false
398 | l_gripper_motor_slider_link:
399 | Alpha: 1
400 | Show Axes: false
401 | Show Trail: false
402 | l_gripper_palm_link:
403 | Alpha: 1
404 | Show Axes: false
405 | Show Trail: false
406 | Value: true
407 | l_gripper_r_finger_link:
408 | Alpha: 1
409 | Show Axes: false
410 | Show Trail: false
411 | Value: true
412 | l_gripper_r_finger_tip_link:
413 | Alpha: 1
414 | Show Axes: false
415 | Show Trail: false
416 | Value: true
417 | l_gripper_tool_frame:
418 | Alpha: 1
419 | Show Axes: false
420 | Show Trail: false
421 | l_shoulder_lift_link:
422 | Alpha: 1
423 | Show Axes: false
424 | Show Trail: false
425 | Value: true
426 | l_shoulder_pan_link:
427 | Alpha: 1
428 | Show Axes: false
429 | Show Trail: false
430 | Value: true
431 | l_torso_lift_side_plate_link:
432 | Alpha: 1
433 | Show Axes: false
434 | Show Trail: false
435 | l_upper_arm_link:
436 | Alpha: 1
437 | Show Axes: false
438 | Show Trail: false
439 | Value: true
440 | l_upper_arm_roll_link:
441 | Alpha: 1
442 | Show Axes: false
443 | Show Trail: false
444 | Value: true
445 | l_wrist_flex_link:
446 | Alpha: 1
447 | Show Axes: false
448 | Show Trail: false
449 | Value: true
450 | l_wrist_roll_link:
451 | Alpha: 1
452 | Show Axes: false
453 | Show Trail: false
454 | Value: true
455 | laser_tilt_link:
456 | Alpha: 1
457 | Show Axes: false
458 | Show Trail: false
459 | laser_tilt_mount_link:
460 | Alpha: 1
461 | Show Axes: false
462 | Show Trail: false
463 | Value: true
464 | narrow_stereo_l_stereo_camera_frame:
465 | Alpha: 1
466 | Show Axes: false
467 | Show Trail: false
468 | narrow_stereo_l_stereo_camera_optical_frame:
469 | Alpha: 1
470 | Show Axes: false
471 | Show Trail: false
472 | narrow_stereo_link:
473 | Alpha: 1
474 | Show Axes: false
475 | Show Trail: false
476 | narrow_stereo_optical_frame:
477 | Alpha: 1
478 | Show Axes: false
479 | Show Trail: false
480 | narrow_stereo_r_stereo_camera_frame:
481 | Alpha: 1
482 | Show Axes: false
483 | Show Trail: false
484 | narrow_stereo_r_stereo_camera_optical_frame:
485 | Alpha: 1
486 | Show Axes: false
487 | Show Trail: false
488 | projector_wg6802418_child_frame:
489 | Alpha: 1
490 | Show Axes: false
491 | Show Trail: false
492 | projector_wg6802418_frame:
493 | Alpha: 1
494 | Show Axes: false
495 | Show Trail: false
496 | r_elbow_flex_link:
497 | Alpha: 1
498 | Show Axes: false
499 | Show Trail: false
500 | Value: true
501 | r_forearm_cam_frame:
502 | Alpha: 1
503 | Show Axes: false
504 | Show Trail: false
505 | r_forearm_cam_optical_frame:
506 | Alpha: 1
507 | Show Axes: false
508 | Show Trail: false
509 | r_forearm_link:
510 | Alpha: 1
511 | Show Axes: false
512 | Show Trail: false
513 | Value: true
514 | r_forearm_roll_link:
515 | Alpha: 1
516 | Show Axes: false
517 | Show Trail: false
518 | Value: true
519 | r_gripper_l_finger_link:
520 | Alpha: 1
521 | Show Axes: false
522 | Show Trail: false
523 | Value: true
524 | r_gripper_l_finger_tip_frame:
525 | Alpha: 1
526 | Show Axes: false
527 | Show Trail: false
528 | r_gripper_l_finger_tip_link:
529 | Alpha: 1
530 | Show Axes: false
531 | Show Trail: false
532 | Value: true
533 | r_gripper_led_frame:
534 | Alpha: 1
535 | Show Axes: false
536 | Show Trail: false
537 | r_gripper_motor_accelerometer_link:
538 | Alpha: 1
539 | Show Axes: false
540 | Show Trail: false
541 | Value: true
542 | r_gripper_motor_screw_link:
543 | Alpha: 1
544 | Show Axes: false
545 | Show Trail: false
546 | r_gripper_motor_slider_link:
547 | Alpha: 1
548 | Show Axes: false
549 | Show Trail: false
550 | r_gripper_palm_link:
551 | Alpha: 1
552 | Show Axes: false
553 | Show Trail: false
554 | Value: true
555 | r_gripper_r_finger_link:
556 | Alpha: 1
557 | Show Axes: false
558 | Show Trail: false
559 | Value: true
560 | r_gripper_r_finger_tip_link:
561 | Alpha: 1
562 | Show Axes: false
563 | Show Trail: false
564 | Value: true
565 | r_gripper_tool_frame:
566 | Alpha: 1
567 | Show Axes: false
568 | Show Trail: false
569 | r_shoulder_lift_link:
570 | Alpha: 1
571 | Show Axes: false
572 | Show Trail: false
573 | Value: true
574 | r_shoulder_pan_link:
575 | Alpha: 1
576 | Show Axes: false
577 | Show Trail: false
578 | Value: true
579 | r_torso_lift_side_plate_link:
580 | Alpha: 1
581 | Show Axes: false
582 | Show Trail: false
583 | r_upper_arm_link:
584 | Alpha: 1
585 | Show Axes: false
586 | Show Trail: false
587 | Value: true
588 | r_upper_arm_roll_link:
589 | Alpha: 1
590 | Show Axes: false
591 | Show Trail: false
592 | Value: true
593 | r_wrist_flex_link:
594 | Alpha: 1
595 | Show Axes: false
596 | Show Trail: false
597 | Value: true
598 | r_wrist_roll_link:
599 | Alpha: 1
600 | Show Axes: false
601 | Show Trail: false
602 | Value: true
603 | sensor_mount_link:
604 | Alpha: 1
605 | Show Axes: false
606 | Show Trail: false
607 | Value: true
608 | torso_lift_link:
609 | Alpha: 1
610 | Show Axes: false
611 | Show Trail: false
612 | Value: true
613 | torso_lift_motor_screw_link:
614 | Alpha: 1
615 | Show Axes: false
616 | Show Trail: false
617 | wide_stereo_l_stereo_camera_frame:
618 | Alpha: 1
619 | Show Axes: false
620 | Show Trail: false
621 | wide_stereo_l_stereo_camera_optical_frame:
622 | Alpha: 1
623 | Show Axes: false
624 | Show Trail: false
625 | wide_stereo_link:
626 | Alpha: 1
627 | Show Axes: false
628 | Show Trail: false
629 | wide_stereo_optical_frame:
630 | Alpha: 1
631 | Show Axes: false
632 | Show Trail: false
633 | wide_stereo_r_stereo_camera_frame:
634 | Alpha: 1
635 | Show Axes: false
636 | Show Trail: false
637 | wide_stereo_r_stereo_camera_optical_frame:
638 | Alpha: 1
639 | Show Axes: false
640 | Show Trail: false
641 | Name: PR2
642 | Robot Description: robot_description
643 | TF Prefix: ""
644 | Update Interval: 0
645 | Value: true
646 | Visual Enabled: true
647 | - Alpha: 1
648 | Autocompute Intensity Bounds: true
649 | Autocompute Value Bounds:
650 | Max Value: 10
651 | Min Value: -10
652 | Value: true
653 | Axis: Z
654 | Channel Name: intensity
655 | Class: rviz/LaserScan
656 | Color: 255; 255; 255
657 | Color Transformer: Intensity
658 | Decay Time: 0
659 | Enabled: true
660 | Max Color: 255; 255; 255
661 | Max Intensity: 4096
662 | Min Color: 0; 0; 0
663 | Min Intensity: 0
664 | Name: Scan
665 | Position Transformer: XYZ
666 | Queue Size: 10
667 | Selectable: true
668 | Size (Pixels): 3
669 | Size (m): 0.01
670 | Style: Flat Squares
671 | Topic: /pr2/scan
672 | Use Fixed Frame: true
673 | Use rainbow: true
674 | Value: true
675 | - Angle Tolerance: 0.1
676 | Class: rviz/Odometry
677 | Color: 255; 85; 0
678 | Enabled: true
679 | Keep: 100
680 | Length: 1
681 | Name: Odometry
682 | Position Tolerance: 0.1
683 | Topic: /pr2/odometry
684 | Value: true
685 | - Alpha: 0.7
686 | Class: rviz/Map
687 | Draw Behind: false
688 | Enabled: true
689 | Name: Map
690 | Topic: /map
691 | Value: true
692 | Enabled: true
693 | Global Options:
694 | Background Color: 48; 48; 48
695 | Fixed Frame: /map
696 | Name: root
697 | Tools:
698 | - Class: rviz/Interact
699 | Hide Inactive Objects: true
700 | - Class: rviz/MoveCamera
701 | - Class: rviz/Select
702 | - Class: rviz/FocusCamera
703 | - Class: rviz/Measure
704 | - Class: rviz/SetInitialPose
705 | Topic: /initialpose
706 | - Class: rviz/SetGoal
707 | Topic: /move_base_simple/goal
708 | - Class: rviz/PublishPoint
709 | Single click: true
710 | Topic: /clicked_point
711 | Value: true
712 | Views:
713 | Current:
714 | Class: rviz/Orbit
715 | Distance: 8.62849
716 | Focal Point:
717 | X: -0.654321
718 | Y: 0.525014
719 | Z: 0.665176
720 | Name: Current View
721 | Near Clip Distance: 0.01
722 | Pitch: 0.615398
723 | Target Frame:
724 | Value: Orbit (rviz)
725 | Yaw: 5.45539
726 | Saved: ~
727 | Window Geometry:
728 | Displays:
729 | collapsed: false
730 | Height: 776
731 | Hide Left Dock: false
732 | Hide Right Dock: false
733 | QMainWindow State: 000000ff00000000fd00000004000000000000010f0000027efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000027e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000027efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000280000027e000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000047a0000003efc0100000002fb0000000800540069006d006501000000000000047a000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002500000027e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
734 | Selection:
735 | collapsed: false
736 | Time:
737 | collapsed: false
738 | Tool Properties:
739 | collapsed: false
740 | Views:
741 | collapsed: false
742 | Width: 1146
743 | X: 58
744 | Y: 45
745 |
--------------------------------------------------------------------------------
/MORSE/mapping.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | # Copyright (C) 2014 Zhi Yan
4 |
5 | if [ "$#" -ne 1 ]
6 | then
7 | echo "Usage : $0 "
8 | exit 1
9 | fi
10 |
11 | scenario=$(pwd)/$(echo $1 | cut -f 1 -d '.')
12 | sed -i 's?blender_file?'$(echo $scenario)'?' singlerobot.py
13 |
14 | PARAMS=()
15 | PARAMS+=( --tab -e "bash -c 'roscore;exec bash'" )
16 | PARAMS+=( --tab -e "bash -c 'morse run -g 768x768 $(pwd)/singlerobot.py;exec bash'" )
17 | PARAMS+=( --tab -e "bash -c 'sleep 3;roslaunch mapping.launch;exec bash'" )
18 | gnome-terminal --working-directory=$(pwd) "${PARAMS[@]}"
19 |
20 | sed -i 's?'$(echo $scenario)'?blender_file?' singlerobot.py
--------------------------------------------------------------------------------
/MORSE/multirobot.py:
--------------------------------------------------------------------------------
1 | # Two simulated Pioneer-3DX robots
2 |
3 | # Copyright (C) 2014 Zhi Yan
4 |
5 | from morse.builder import *
6 |
7 | ##################### ROBOT 0 ####################
8 |
9 | p3dx0 = Pioneer3DX()
10 | p3dx0.add_default_interface('ros')
11 | p3dx0.translate(x=-36.0, y=36.0, z=0.0)
12 |
13 | odom = Odometry()
14 | odom.add_interface('ros', frame_id='/p3dx0_tf/odom', child_frame_id='/p3dx0_tf/base_footprint')
15 | p3dx0.append(odom)
16 |
17 | sick = Sick()
18 | sick.translate(z=0.252)
19 | sick.properties(Visible_arc=True)
20 | sick.add_interface('ros', frame_id='/p3dx0_tf/base_laser_link')
21 | p3dx0.append(sick)
22 |
23 | motion = MotionVWDiff()
24 | motion.add_interface('ros')
25 | p3dx0.append(motion)
26 |
27 | ##################### ROBOT 1 ####################
28 |
29 | p3dx1 = Pioneer3DX()
30 | p3dx1.add_default_interface('ros')
31 | p3dx1.translate(x=-36.0, y=32.0, z=0.0)
32 |
33 | odom = Odometry()
34 | odom.add_interface('ros', frame_id='/p3dx1_tf/odom', child_frame_id='/p3dx1_tf/base_footprint')
35 | p3dx1.append(odom)
36 |
37 | sick = Sick()
38 | sick.translate(z=0.252)
39 | sick.properties(Visible_arc=True)
40 | sick.add_interface('ros', frame_id='/p3dx1_tf/base_laser_link')
41 | p3dx1.append(sick)
42 |
43 | motion = MotionVWDiff()
44 | motion.add_interface('ros')
45 | p3dx1.append(motion)
46 |
47 | ##################### ENVIRONMENT ####################
48 |
49 | env = Environment('blender_file', fastmode=False)
50 | env.place_camera([0, 0, 60])
51 | env.aim_camera([0, 0, 0])
52 |
--------------------------------------------------------------------------------
/MORSE/scenarios.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/MORSE/scenarios.png
--------------------------------------------------------------------------------
/MORSE/scenarios/cross.blend:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/MORSE/scenarios/cross.blend
--------------------------------------------------------------------------------
/MORSE/scenarios/cross.pgm:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/MORSE/scenarios/cross.pgm
--------------------------------------------------------------------------------
/MORSE/scenarios/cross.yaml:
--------------------------------------------------------------------------------
1 | image: cross.pgm
2 | resolution: 0.200000
3 | origin: [-30.000000, -87.600000, 0.000000]
4 | negate: 0
5 | occupied_thresh: 0.65
6 | free_thresh: 0.196
7 |
8 |
--------------------------------------------------------------------------------
/MORSE/scenarios/loop.blend:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/MORSE/scenarios/loop.blend
--------------------------------------------------------------------------------
/MORSE/scenarios/loop.pgm:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/MORSE/scenarios/loop.pgm
--------------------------------------------------------------------------------
/MORSE/scenarios/loop.yaml:
--------------------------------------------------------------------------------
1 | image: loop.pgm
2 | resolution: 0.200000
3 | origin: [-30.000000, -81.200000, 0.000000]
4 | negate: 0
5 | occupied_thresh: 0.65
6 | free_thresh: 0.196
7 |
8 |
--------------------------------------------------------------------------------
/MORSE/scenarios/maze.blend:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/MORSE/scenarios/maze.blend
--------------------------------------------------------------------------------
/MORSE/scenarios/maze.pgm:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/MORSE/scenarios/maze.pgm
--------------------------------------------------------------------------------
/MORSE/scenarios/maze.yaml:
--------------------------------------------------------------------------------
1 | image: maze.pgm
2 | resolution: 0.200000
3 | origin: [-30.000000, -81.200000, 0.000000]
4 | negate: 0
5 | occupied_thresh: 0.65
6 | free_thresh: 0.196
7 |
8 |
--------------------------------------------------------------------------------
/MORSE/scenarios/zigzag.blend:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/MORSE/scenarios/zigzag.blend
--------------------------------------------------------------------------------
/MORSE/scenarios/zigzag.pgm:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/MORSE/scenarios/zigzag.pgm
--------------------------------------------------------------------------------
/MORSE/scenarios/zigzag.yaml:
--------------------------------------------------------------------------------
1 | image: map.pgm
2 | resolution: 0.200000
3 | origin: [-30.000000, -87.600000, 0.000000]
4 | negate: 0
5 | occupied_thresh: 0.65
6 | free_thresh: 0.196
7 |
8 |
--------------------------------------------------------------------------------
/MORSE/singlerobot.py:
--------------------------------------------------------------------------------
1 | # One simulated PR2 robots
2 |
3 | # Copyright (C) 2014 Zhi Yan
4 |
5 | from morse.builder import *
6 |
7 | ##################### MAPPING ROBOT ####################
8 | pr2 = BasePR2()
9 | pr2.add_default_interface('ros')
10 | pr2.translate(x=-36.0, y=36.0, z=0.0)
11 |
12 | odometry = Odometry()
13 | odometry.add_interface('ros')
14 | pr2.append(odometry)
15 |
16 | scan = Sick()
17 | scan.translate(x=0.275, z=0.252)
18 | scan.properties(Visible_arc=True)
19 | scan.properties(laser_range=30.0)
20 | scan.properties(resolution=1.0)
21 | scan.properties(scan_window=180.0)
22 | scan.add_interface('ros')
23 | pr2.append(scan)
24 |
25 | keyboard = Keyboard()
26 | keyboard.properties(Speed=1.0)
27 | pr2.append(keyboard)
28 |
29 | ##################### SCENARIO ####################
30 | env = Environment('blender_file', fastmode=False)
31 | env.place_camera([0, 0, 60])
32 | env.aim_camera([0, 0, 0])
33 |
--------------------------------------------------------------------------------
/MORSE/test.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | # Copyright (C) 2014 Zhi Yan
4 |
5 | if [ "$#" -ne 1 ]
6 | then
7 | echo "Usage : $0 "
8 | exit 1
9 | fi
10 |
11 | scenario=$(pwd)/$(echo $1 | cut -f 1 -d '.')
12 | sed -i 's?blender_file?'$(echo $scenario)'?' multirobot.py
13 |
14 | PARAMS=()
15 | PARAMS+=( --tab -e "bash -c 'roscore;exec bash'" )
16 | PARAMS+=( --tab -e "bash -c 'morse run -g 640x640 $(pwd)/multirobot.py;exec bash'" )
17 | gnome-terminal --working-directory=$(pwd) "${PARAMS[@]}"
18 |
19 | sed -i 's?'$(echo $scenario)'?blender_file?' multirobot.py
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Multi-robot Exploration Testbed #
2 |
3 | [](https://www.codacy.com/app/yzrobot/mrs_testbed?utm_source=github.com&utm_medium=referral&utm_content=yzrobot/mrs_testbed&utm_campaign=Badge_Grade)
4 | [](https://opensource.org/licenses/BSD-3-Clause)
5 |
6 | This testbed includes:
7 | - [ROS](http://wiki.ros.org/) packages for multi-robot exploration
8 | - [MORSE](https://www.openrobots.org/wiki/morse) simulation scenarios
9 | - Scripts for autonomous deployment of the infrastructure and experiments (computer cluster required)
10 |
11 | To have a general idea, please refer to the videos below, respectively showing simulated and real robot exploration:
12 |
13 | [](https://www.youtube.com/watch?v=SrA_1ITJo7A)
14 | [](https://www.youtube.com/watch?v=xCW0WT_G5OA)
15 |
16 | ## Citation ##
17 |
18 | If you are considering using these resources, please reference the following:
19 | ```
20 | @article{yz17robotics,
21 | author = {Zhi Yan and Luc Fabresse and Jannik Laval and Noury Bouraqadi},
22 | title = {Building a ROS-based Testbed for Realistic Multi-robot Simulation: Taking the Exploration as an Example},
23 | year = {2017},
24 | journal = {Robotics},
25 | volume = {6},
26 | number = {3},
27 | pages = {1--21}
28 | }
29 | ```
30 |
31 | ## Overview ##
32 |
33 | 
34 |
35 | The testbed is composed of four parts: a simulator, a monitor, a set of robot controllers, and the ROS middleware used to connect all of them. In particular, we use the MORSE 3D realistic simulator and wrap it up into a ROS node. The monitor is also performed as a ROS node, which allows us to supervise the experimental processes. Specifically, it can stop the experiment when the stop condition is triggered, collect measurement data and compute the metrics afterwards.
36 |
37 | ## Prerequisites ##
38 |
39 | * ROS Groovy or +
40 | * MORSE 1.2 or +
41 |
--------------------------------------------------------------------------------
/ROS/README.md:
--------------------------------------------------------------------------------
1 | The relevant ROS packages are available here:
2 |
3 | * `explore`: Willow Garage's explore package for ROS Indigo. [ [code](https://github.com/yzrobot/explore) | [wiki](http://wiki.ros.org/explore) ]
4 | * `explore_multirobot`: A multi-robot version of the ROS explore package. [ [code](https://github.com/yzrobot/explore_multirobot) | [wiki](http://wiki.ros.org/explore_multirobot) ]
5 | * `map_merging`: Merging multiple maps with knowledge of the initial relative positions of robots. [ [code](https://github.com/yzrobot/map_merging) | [wiki](http://wiki.ros.org/map_merging) ]
6 | * `tf_splitter`: Decomposing the /tf topic into multiple ones. [ [code](https://github.com/yzrobot/tf_splitter) | [wiki](http://wiki.ros.org/tf_splitter) ]
7 | * `pose_publisher`: providing current position and orientation of a robot in a map. [ [code](https://github.com/yzrobot/pose_publisher) | [wiki](http://wiki.ros.org/pose_publisher) ]
8 |
--------------------------------------------------------------------------------
/architecture.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/architecture.png
--------------------------------------------------------------------------------
/exploration/README.md:
--------------------------------------------------------------------------------
1 | ## The full coordinate transform tree ##
2 |
3 | 
4 |
5 | ## How to play? ##
6 |
7 | `roslaunch multi_robot.launch`
8 |
9 |
--------------------------------------------------------------------------------
/exploration/frame.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/exploration/frame.png
--------------------------------------------------------------------------------
/exploration/maze_1robots.py:
--------------------------------------------------------------------------------
1 | from morse.builder import *
2 |
3 | # http://www.openrobots.org/morse/doc/1.2/dev/time_event.html
4 | # Set the default value of the logic tic rate to 60 Hz
5 | #bge.logic.setLogicTicRate(60.0)
6 | #bge.logic.setPhysicsTicRate(60.0)
7 |
8 | #bpymorse.set_speed(fps=10)
9 |
10 | ##################### ROBOT 0 ####################
11 | p3dx0 = Pioneer3DX()
12 | p3dx0.add_default_interface('ros')
13 | p3dx0.translate(x=-38.0, y=38.0, z=0.0)
14 |
15 | # http://wiki.ros.org/ROSARIA
16 | odom0 = Odometry()
17 | odom0.add_interface('ros', frame_id='/p3dx0_tf/odom', child_frame_id='/p3dx0_tf/base_footprint')
18 | odom0.alter('Noise', pos_std=0.022, rot_std=0.02)
19 | odom0.frequency(10) # 10Hz is the default rate for Pioneer 3-DX
20 | p3dx0.append(odom0)
21 |
22 | sick0 = Sick()
23 | sick0.translate(z=0.252)
24 | sick0.properties(Visible_arc = True)
25 | sick0.properties(scan_window = 190)
26 | sick0.properties(laser_range = 26.0)
27 | sick0.add_interface('ros', frame_id='/p3dx0_tf/base_laser_link')
28 | sick0.frequency(25) # problem of setting 25Hz, automatically change 30Hz by MORSE
29 | p3dx0.append(sick0)
30 |
31 | motion0 = MotionVWDiff()
32 | motion0.add_interface('ros')
33 | p3dx0.append(motion0)
34 |
35 | #clock = Clock()
36 | #clock.add_interface('ros', topic='/clock')
37 | #p3dx0.append(clock)
38 |
39 | ################ ENVIRONMENT ###############
40 | env = Environment('../MORSE/scenarios/maze', fastmode=False)
41 | env.place_camera([0, 0, 60])
42 | env.aim_camera([0, 0, 0])
43 | #env.set_time_strategy(TimeStrategies.FixedSimulationStep)
44 | env.show_debug_properties()
45 | env.show_framerate()
46 | env.show_physics()
47 |
48 |
--------------------------------------------------------------------------------
/exploration/maze_7robots.py:
--------------------------------------------------------------------------------
1 | from morse.builder import *
2 |
3 | ##################### ROBOT 0 ####################
4 | p3dx0 = Pioneer3DX()
5 | p3dx0.add_default_interface('ros')
6 | p3dx0.translate(x=-38.0, y=38.0, z=0.0)
7 |
8 | odom0 = Odometry()
9 | odom0.add_interface('ros', frame_id='/p3dx0_tf/odom', child_frame_id='/p3dx0_tf/base_footprint')
10 | odom0.alter('Noise', pos_std=0.022, rot_std=0.02)
11 | odom0.frequency(10)
12 | p3dx0.append(odom0)
13 |
14 | sick0 = Sick()
15 | sick0.translate(z=0.252)
16 | sick0.properties(Visible_arc=True)
17 | sick0.properties(scan_window = 190)
18 | sick0.properties(laser_range = 26.0)
19 | sick0.add_interface('ros', frame_id='/p3dx0_tf/base_laser_link')
20 | sick0.frequency(25)
21 | p3dx0.append(sick0)
22 |
23 | motion0 = MotionVWDiff()
24 | motion0.add_interface('ros')
25 | p3dx0.append(motion0)
26 |
27 | ##################### ROBOT 1 ####################
28 | p3dx1 = Pioneer3DX()
29 | p3dx1.add_default_interface('ros')
30 | p3dx1.translate(x=-38.0, y=36.0, z=0.0)
31 |
32 | odom1 = Odometry()
33 | odom1.add_interface('ros', frame_id='/p3dx1_tf/odom', child_frame_id='/p3dx1_tf/base_footprint')
34 | odom1.alter('Noise', pos_std=0.022, rot_std=0.02)
35 | odom1.frequency(10)
36 | p3dx1.append(odom1)
37 |
38 | sick1 = Sick()
39 | sick1.translate(z=0.252)
40 | sick1.properties(Visible_arc=True)
41 | sick1.properties(scan_window = 190)
42 | sick1.properties(laser_range = 26.0)
43 | sick1.add_interface('ros', frame_id='/p3dx1_tf/base_laser_link')
44 | sick1.frequency(25)
45 | p3dx1.append(sick1)
46 |
47 | motion1 = MotionVWDiff()
48 | motion1.add_interface('ros')
49 | p3dx1.append(motion1)
50 |
51 | ##################### ROBOT 2 ####################
52 | p3dx2 = Pioneer3DX()
53 | p3dx2.add_default_interface('ros')
54 | p3dx2.translate(x=-38.0, y=34.0, z=0.0)
55 |
56 | odom2 = Odometry()
57 | odom2.add_interface('ros', frame_id='/p3dx2_tf/odom', child_frame_id='/p3dx2_tf/base_footprint')
58 | odom2.alter('Noise', pos_std=0.022, rot_std=0.02)
59 | odom2.frequency(10)
60 | p3dx2.append(odom2)
61 |
62 | sick2 = Sick()
63 | sick2.translate(z=0.252)
64 | sick2.properties(Visible_arc=True)
65 | sick2.properties(scan_window = 190)
66 | sick2.properties(laser_range = 26.0)
67 | sick2.add_interface('ros', frame_id='/p3dx2_tf/base_laser_link')
68 | sick2.frequency(25)
69 | p3dx2.append(sick2)
70 |
71 | motion2 = MotionVWDiff()
72 | motion2.add_interface('ros')
73 | p3dx2.append(motion2)
74 |
75 | ##################### ROBOT 3 ####################
76 | p3dx3 = Pioneer3DX()
77 | p3dx3.add_default_interface('ros')
78 | p3dx3.translate(x=-38.0, y=32.0, z=0.0)
79 |
80 | odom3 = Odometry()
81 | odom3.add_interface('ros', frame_id='/p3dx3_tf/odom', child_frame_id='/p3dx3_tf/base_footprint')
82 | odom3.alter('Noise', pos_std=0.022, rot_std=0.02)
83 | odom3.frequency(10)
84 | p3dx3.append(odom3)
85 |
86 | sick3 = Sick()
87 | sick3.translate(z=0.252)
88 | sick3.properties(Visible_arc=True)
89 | sick3.properties(scan_window = 190)
90 | sick3.properties(laser_range = 26.0)
91 | sick3.add_interface('ros', frame_id='/p3dx3_tf/base_laser_link')
92 | sick3.frequency(25)
93 | p3dx3.append(sick3)
94 |
95 | motion3 = MotionVWDiff()
96 | motion3.add_interface('ros')
97 | p3dx3.append(motion3)
98 |
99 | ##################### ROBOT 4 ####################
100 | p3dx4 = Pioneer3DX()
101 | p3dx4.add_default_interface('ros')
102 | p3dx4.translate(x=-38.0, y=30.0, z=0.0)
103 |
104 | odom4 = Odometry()
105 | odom4.add_interface('ros', frame_id='/p3dx4_tf/odom', child_frame_id='/p3dx4_tf/base_footprint')
106 | odom4.alter('Noise', pos_std=0.022, rot_std=0.02)
107 | odom4.frequency(10)
108 | p3dx4.append(odom4)
109 |
110 | sick4 = Sick()
111 | sick4.translate(z=0.252)
112 | sick4.properties(Visible_arc=True)
113 | sick4.properties(scan_window = 190)
114 | sick4.properties(laser_range = 26.0)
115 | sick4.add_interface('ros', frame_id='/p3dx4_tf/base_laser_link')
116 | sick4.frequency(25)
117 | p3dx4.append(sick4)
118 |
119 | motion4 = MotionVWDiff()
120 | motion4.add_interface('ros')
121 | p3dx4.append(motion4)
122 |
123 | ##################### ROBOT 5 ####################
124 | p3dx5 = Pioneer3DX()
125 | p3dx5.add_default_interface('ros')
126 | p3dx5.translate(x=-38.0, y=28.0, z=0.0)
127 |
128 | odom5 = Odometry()
129 | odom5.add_interface('ros', frame_id='/p3dx5_tf/odom', child_frame_id='/p3dx5_tf/base_footprint')
130 | odom5.alter('Noise', pos_std=0.022, rot_std=0.02)
131 | odom5.frequency(10)
132 | p3dx5.append(odom5)
133 |
134 | sick5 = Sick()
135 | sick5.translate(z=0.252)
136 | sick5.properties(Visible_arc=True)
137 | sick5.properties(scan_window = 190)
138 | sick5.properties(laser_range = 26.0)
139 | sick5.add_interface('ros', frame_id='/p3dx5_tf/base_laser_link')
140 | sick5.frequency(25)
141 | p3dx5.append(sick5)
142 |
143 | motion5 = MotionVWDiff()
144 | motion5.add_interface('ros')
145 | p3dx5.append(motion5)
146 |
147 | ##################### ROBOT 6 ####################
148 | p3dx6 = Pioneer3DX()
149 | p3dx6.add_default_interface('ros')
150 | p3dx6.translate(x=-38.0, y=26.0, z=0.0)
151 |
152 | odom6 = Odometry()
153 | odom6.add_interface('ros', frame_id='/p3dx6_tf/odom', child_frame_id='/p3dx6_tf/base_footprint')
154 | odom6.alter('Noise', pos_std=0.022, rot_std=0.02)
155 | odom6.frequency(10)
156 | p3dx6.append(odom6)
157 |
158 | sick6 = Sick()
159 | sick6.translate(z=0.252)
160 | sick6.properties(Visible_arc=True)
161 | sick6.properties(scan_window = 190)
162 | sick6.properties(laser_range = 26.0)
163 | sick6.add_interface('ros', frame_id='/p3dx6_tf/base_laser_link')
164 | sick6.frequency(25)
165 | p3dx6.append(sick6)
166 |
167 | motion6 = MotionVWDiff()
168 | motion6.add_interface('ros')
169 | p3dx6.append(motion6)
170 |
171 | ################ ENVIRONMENT ###############
172 | env = Environment('../MORSE/scenarios/maze', fastmode=False)
173 | env.place_camera([0, 0, 60])
174 | env.aim_camera([0, 0, 0])
175 |
--------------------------------------------------------------------------------
/exploration/multi_robot.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
43 |
44 |
45 |
46 |
47 |
48 |
49 |
50 |
51 |
52 |
53 |
54 |
55 |
56 |
57 |
58 |
59 |
60 |
61 |
62 |
63 |
64 |
65 |
66 |
67 |
68 |
69 |
70 |
71 |
72 |
73 |
74 |
75 |
76 |
77 |
78 |
79 |
80 |
81 |
82 |
83 |
84 |
85 |
86 |
87 |
88 |
89 |
90 |
91 |
92 |
93 |
94 |
95 |
96 |
97 |
98 |
99 |
100 |
101 |
102 |
103 |
104 |
105 |
106 |
107 |
108 |
109 |
110 |
111 |
112 |
113 |
114 |
115 |
116 |
117 |
118 |
119 |
120 |
121 |
122 |
123 |
124 |
125 |
126 |
127 |
128 |
129 |
130 |
131 |
132 |
133 |
134 |
135 |
136 |
137 |
138 |
139 |
140 |
141 |
142 |
143 |
144 |
145 |
146 |
147 |
148 |
149 |
150 |
151 |
152 |
153 |
154 |
155 |
156 |
157 |
158 |
159 |
160 |
161 |
162 |
163 |
164 |
165 |
166 |
167 |
168 |
169 |
170 |
171 |
172 |
173 |
174 |
--------------------------------------------------------------------------------
/exploration/pioneer3dx.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
14 |
15 |
16 |
17 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
31 |
44 |
45 |
46 |
47 |
48 |
49 |
50 |
51 |
52 |
53 |
54 |
55 |
56 |
57 |
58 |
59 |
60 |
61 |
62 |
63 |
64 |
65 |
66 |
67 |
68 |
69 |
70 |
71 |
72 |
73 |
74 |
75 |
76 |
77 |
78 |
79 |
80 |
81 |
82 |
83 |
84 |
85 |
86 |
87 |
88 |
89 |
90 |
91 |
92 |
93 |
94 |
95 |
96 |
97 |
98 |
99 |
100 |
101 |
102 |
103 |
104 |
105 |
106 |
107 |
108 |
--------------------------------------------------------------------------------
/exploration/yaml/costmap_common_p3dx.yaml:
--------------------------------------------------------------------------------
1 | map_type: costmap
2 | transform_tolerance: 0.2
3 |
4 | # Global costmap parameters
5 | max_obstacle_height: 2.0
6 | obstacle_range: 2.5
7 | raytrace_range: 3.0
8 | cost_scaling_factor: 10.0
9 |
10 | # Robot description parameters
11 | inflation_radius: 0.55
12 | footprint: [[0.254, -0.0508], [0.1778, -0.0508], [0.1778, -0.1778], [-0.1905, -0.1778], [-0.254, 0], [-0.1905, 0.1778], [0.1778, 0.1778], [0.1778, 0.0508], [0.254, 0.0508]]
13 | #inscribed_radius: 0.25
14 |
15 | # Map management parameters
16 | unknown_cost_value: 255
17 | lethal_cost_threshold: 100
18 |
19 | # Sensor management parameters
20 | observation_sources: base_scan
21 | base_scan: {sensor_frame: base_laser_link, data_type: LaserScan, expected_update_rate: 0.2, observation_persistance: 0.0, marking: true, clearing: true}
22 |
--------------------------------------------------------------------------------
/exploration/yaml/explore_costmap.yaml:
--------------------------------------------------------------------------------
1 | explore_costmap:
2 | global_frame: map
3 | robot_base_frame: base_link
4 | update_frequency: 5.0
5 | publish_frequency: 0.0
6 | raytrace_range: 3.0
7 | obstacle_range: 2.5
8 | static_map: false
9 | rolling_window: false
10 | width: 160.0
11 | height: 160.0
12 | resolution: 0.2
13 | origin_x: -80.0
14 | origin_y: -80.0
15 | track_unknown_space: true
16 | unknown_cost_value: 255
17 | lethal_cost_threshold: 100
18 | # inflation_radius: 1.0
--------------------------------------------------------------------------------
/exploration/yaml/global_costmap.yaml:
--------------------------------------------------------------------------------
1 | global_costmap:
2 | global_frame: map
3 | robot_base_frame: base_link
4 | update_frequency: 5.0
5 | publish_frequency: 0.0
6 | raytrace_range: 3.0
7 | obstacle_range: 2.5
8 | static_map: false
9 | rolling_window: false
10 | width: 160.0
11 | height: 160.0
12 | resolution: 0.2
13 | origin_x: -80.0
14 | origin_y: -80.0
15 | # inflation_radius: 1.0
--------------------------------------------------------------------------------
/exploration/yaml/local_costmap.yaml:
--------------------------------------------------------------------------------
1 | local_costmap:
2 | global_frame: odom
3 | robot_base_frame: base_link
4 | update_frequency: 5.0
5 | publish_frequency: 0.0
6 | static_map: false
7 | rolling_window: true
8 | width: 6.0
9 | height: 6.0
10 | resolution: 0.1
11 | origin_x: 0.0
12 | origin_y: 0.0
13 | #inflation_radius: 0.3 # set a smaller value for narrow pass
--------------------------------------------------------------------------------
/exploration/yaml/navfn.yaml:
--------------------------------------------------------------------------------
1 | NavfnROS:
2 | transform_tolerance: 0.3
3 |
--------------------------------------------------------------------------------
/exploration/yaml/trajectory_planner.yaml:
--------------------------------------------------------------------------------
1 | TrajectoryPlannerROS:
2 | #Robot Configuration Parameters
3 | acc_lim_x: 2.5
4 | acc_lim_y: 2.5
5 | acc_lim_theta: 3.2
6 | max_vel_x: 1.2
7 | min_vel_x: 0.1
8 | max_vel_theta: 5.24
9 | min_vel_theta: -5.24
10 | min_in_place_vel_theta: 0.4
11 | escape_vel: -1.2
12 | holonomic_robot: false
13 | #Goal Tolerance Parameters
14 | yaw_goal_tolerance: 0.05
15 | xy_goal_tolerance: 0.1
16 | latch_xy_goal_tolerance: false
17 | #Forward Simulation Parameters
18 | sim_time: 1.0
19 | sim_granularity: 0.025
20 | vx_samples: 3
21 | vtheta_samples: 20
22 | #Trajectory Scoring Parameters
23 | pdist_scale: 0.6
24 | gdist_scale: 0.8
25 | occdist_scale: 0.5
26 | heading_lookahead: 0.325
27 | heading_scoring: false
28 | heading_scoring_timestep: 0.8
29 | dwa: true
30 |
--------------------------------------------------------------------------------
/scripts/README.md:
--------------------------------------------------------------------------------
1 | *** Version 1.0 (uploaded 25/07/2014) ***
2 | 1. Scripts for multi-robot exploration simulation:
3 | - submit_job.sh
4 | - start_simulation.sh
5 | - vm_setup_main.sh
6 |
7 | *** Version 1.1 (uploaded 28/08/2014) ***
8 | 1. System cleanup tool:
9 | - clean/remove_all_in_scratch.sh
10 | 2. Robot performance logger (cpu, ram, network I/O):
11 | - log_robot_performance.sh
12 |
13 | *** Version 1.1.1 (uploaded 04/09/2014) ***
14 | 1. Updated robot prototype checking:
15 | - cp -rf /lustre/zhi.yan/robot_prototype/ $ROBOT_PROTOTYPE_PATH
16 | 2. Solved robot deployment permission problems:
17 | - ROBOT_DEPLOYMENT_PATH="/scratch/${HOME##*/}"
18 |
19 | *** Version 1.1.2 (uploaded 28/09/2014) ***
20 | 1. Solved robot SSH authentication problems:
21 | - "auto_ssh" test before "ssh" in vm_setup_main.sh
22 |
23 | *** TODO: resolve "VM can't be shutdown properly" in each run ***
--------------------------------------------------------------------------------
/scripts/clean/remove_all_in_scratch.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | for i in $(seq 0 59)
4 | do
5 | echo -ne "\033[31mclean up chapo$i......\033[0m"
6 | ssh chapo$i "rm -rf /scratch/*"
7 | [ $? -eq 0 ] && echo -e "\033[31mdone!\033[0m"
8 | done
9 |
--------------------------------------------------------------------------------
/scripts/log_robot_performance.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | ### Copyright (C) 2014 Zhi Yan
4 |
5 | LOG_FILE_NAME="team$1_test$2_robot$3.csv"
6 | LOG_FILE_PATH="/media/sf_scratch/$4/log"
7 | LOG_FREQUENCY=5 # record once every LOG_FREQUENCY seconds
8 |
9 | mkdir -p $LOG_FILE_PATH
10 | echo "cpu(used/total),ram(used/total),net_in(Kbytes/s),net_out(Kbytes/s)" > $LOG_FILE_PATH/$LOG_FILE_NAME
11 |
12 | cpu_usage=0
13 | memory_usage=0
14 | net_in=0
15 | net_out=0
16 |
17 | record_loop() {
18 | while [ 1 ]
19 | do
20 | cpu_usage=$((100-$(vmstat | tail -1 | awk '{print $15}')))
21 | memory_usage=$(free -m | grep Mem | awk '{print $3/$2 * 100}')
22 | net_in=$(nicstat | grep eth0 | awk '{print $3}')
23 | net_out=$(nicstat | grep eth0 | awk '{print $4}')
24 | echo "$cpu_usage,$memory_usage,$net_in,$net_out" >> $LOG_FILE_PATH/$LOG_FILE_NAME
25 | sleep $LOG_FREQUENCY
26 | done
27 | }
28 |
29 | record_loop & # we have to put it to background to not block roslaunch
--------------------------------------------------------------------------------
/scripts/start_simulation.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | ############################ JOB SUBMISSION OPTIONS ############################
4 | ############################### !!! WARNING !!! ################################
5 | ############ !!! Do not edit this part to change PBS parameters !!! ############
6 | ##### You should edit the parameters for command "qsub" in "submit_job.sh" #####
7 | # #
8 | # --- specifying a job name --- #
9 | # #PBS -N car_mrs_explore #
10 | # #
11 | # --- working directory --- #
12 | # #PBS -d /home/zhi.yan/Simulation/Scripts #
13 | # #
14 | # --- marking a job as rerunnable or not --- #
15 | # #PBS -r n #
16 | # #
17 | # --- specifying e-mail notification --- #
18 | # --- (a)boreted (b)egins (e)nds --- #
19 | # #PBS -m abe #
20 | # #
21 | # --- setting e-mail recipient list --- #
22 | # #PBS -M zhi.yan@mines-douai.fr #
23 | # #
24 | # --- resource request, can be overridden by command line --- #
25 | # --- 8 (p)rocessors (p)er (n)ode --- #
26 | # #PBS -l nodes=4:ppn=8 #
27 | # --- 24 hours maximum for the total job --- #
28 | # #PBS -l walltime=24:00:00 #
29 | # --- 1gb RAM per processor --- #
30 | # #PBS -l pmem=1gb #
31 | # #
32 | # --- redirecting (o)utput and (e)rror files --- #
33 | # #PBS -o /home/zhi.yan/Simulation/Qsubresults #
34 | # #PBS -e /home/zhi.yan/Simulation/Qsubresults #
35 | ################################################################################
36 |
37 | ################################### DOCUMENTS ##################################
38 | # http://wiki.ibest.uidaho.edu/index.php/Tutorial:_Submitting_a_job_using_qsub #
39 | # https://www.virtualbox.org/manual/ch08.html #
40 | # https://github.com/andrewpile/ssh-copy-id/blob/master/ssh-copy-id #
41 | ################################################################################
42 |
43 | ########################## LAUNCH ME BY COMMAND LINE ###########################
44 | # qsub -l nodes=4:ppn=8:pmem=1gb \ #
45 | # -v N_ROBOTS_MIN=1,N_ROBOTS_MAX=16,N_TRIALS_PTS=5,N_ROBOTS_PER_NODE=4, \ #
46 | # N_VM_MEMS=2048,N_VM_CPUS=2 start_simulation.sh #
47 | ################################################################################
48 |
49 | echo "
50 | ##############################################################
51 | # Script for Multi-robot Exploration Simulation #
52 | # Zhi Yan, Luc Fabresse, Jannik Laval, and Noury Bouraqadi #
53 | # Copyright 2014 #
54 | ##############################################################
55 | "
56 |
57 | ####################### DEBUG SETTINGS #######################
58 | [ -z $DEBUG_MODE ] && DEBUG_MODE="true" # Debug mode
59 | if [ $DEBUG_MODE == "true" ]
60 | then
61 | RESET_VM="false" # Re-copy and re-clone all virtual machines (need more time!)
62 | SIM_ENV_NAME="maze" # morse simulation environment file names (blend, pgm, yaml, sh)
63 | ODOMETRY_NOISE="false" # set odometry noise or perfect in morse description file
64 | N_ROBOTS_MIN=4 # Minimum number of robots
65 | N_ROBOTS_MAX=4 # Maximum number of robots
66 | N_TRIALS_PTS=1 # Number of trials per team size
67 | N_ROBOTS_PER_NODE=2 # Number of robots per CHAPO node
68 | N_VM_MEMS=2048 # Memory size (MB) for 1 virtual machine
69 | N_VM_CPUS=2 # Number of CPUs for 1 virtual machine
70 |
71 | echo "
72 | ##############################################################
73 | Script running in *DEBUG* mode with following parameters:
74 | * RESET_VM = $RESET_VM
75 | * SIM_ENV_NAME = $SIM_ENV_NAME
76 | * ODOMETRY_NOISE = $ODOMETRY_NOISE
77 | * N_ROBOTS_MIN = $N_ROBOTS_MIN
78 | * N_ROBOTS_MAX = $N_ROBOTS_MAX
79 | * N_TRIALS_PTS = $N_TRIALS_PTS
80 | * N_ROBOTS_PER_NODE = $N_ROBOTS_PER_NODE
81 | * N_VM_MEMS = $N_VM_MEMS
82 | * N_VM_CPUS = $N_VM_CPUS
83 | ##############################################################
84 | "
85 | fi
86 | ##############################################################
87 |
88 | ######################### ERROR CODES ########################
89 | ERROR_FILE_NOT_FOUND=1
90 | ERROR_DIRECTORY_NOT_FOUND=2
91 | ERROR_AUTO_SSH_COPY_ID=3
92 | ERROR_AUTO_SSH=4
93 | ERROR_SSH_INTERACTIVE_MODE=5
94 | ERROR_VM_SETUP_FAILS=6
95 | ERROR_RESOURCE_CONFLICT=7
96 | ERROR_MAX_FAILED_ROBOTS=8
97 | ##############################################################
98 |
99 | ###################### SCRIPT VARIABLES ######################
100 | ROBOT_DEPLOYMENT_PATH="/scratch/${HOME##*/}" # where we deploy robot(s)
101 | ROBOT_PROTOTYPE_PATH="/home/${HOME##*/}/lustre/robot_prototype" # robot prototype directory (VM) on CHAPO
102 | ROBOT_PROTOTYPE_IP="10.3.12.99" # IP of robot prototype VM
103 | ROBOT_PROTOTYPE_USER="viki" # default username of robot prototype VM
104 | ROBOT_PROTOTYPE_PASS="viki" # default password of robot prototype VM
105 | ROBOT_INIT_POSE_X=0 # initial posiiton x for robot(s)
106 | ROBOT_INIT_POSE_Y=0 # initial posiiton y for robot(s)
107 | ROBOT_INIT_POSE_Z=0 # initial posiiton z for robot(s)
108 |
109 | MORSE_USER="car" # ssh username for HP-Z420 Workstation
110 | MORSE_HOST="10.1.10.84" # IP of HP-Z420 Workstation
111 | MORSE_PASS="car" # ssh password for HP-Z420 Workstation
112 | MORSE_FILE_PATH="/home/car/test" # where we store files to launch morse (absolute path!)
113 |
114 | MONITOR_PKG_NAME="zsupervision" # monitor name (ROS package)
115 | MONITOR_LOG_PATH="/home/car/test/results" # location for storing experimental results
116 | MONITOR_LISTEN_PORT="1234" # exploration end signal listening port
117 | ##############################################################
118 |
119 | ###################### SHELL FUNCTIONS #######################
120 | auto_ssh() {
121 | expect -c "set timeout -1;
122 | spawn ssh -o StrictHostKeyChecking=no $2 ${@:3};
123 | expect {
124 | *password* {
125 | send -- $1\r;
126 | expect {
127 | *denied* {exit 3;}
128 | eof
129 | }
130 | }
131 | *route* {exit 2;}
132 | eof {exit 1;}
133 | }"
134 | return $?
135 | }
136 |
137 | #auto_scp() {
138 | # expect -c "set timeout -1;
139 | # spawn scp -o StrictHostKeyChecking=no ${@:2};
140 | # expect {
141 | # *password* {
142 | # send -- $1\r;
143 | # expect {
144 | # *denied* {exit 2;}
145 | # eof
146 | # }
147 | # }
148 | # eof {exit 1;}
149 | # }"
150 | # return $?
151 | #}
152 |
153 | auto_ssh_copy_id() {
154 | expect -c "set timeout -1;
155 | spawn ssh-copy-id $2;
156 | expect {
157 | *ERROR* {exit 1;}
158 | *Usage* {exit 1;}
159 | *(yes/no)* {send -- yes\r;exp_continue;}
160 | *password:* {send -- $1\r;exp_continue;}
161 | eof {exit 0;}
162 | }"
163 | }
164 |
165 | set_robot_init_pose() {
166 | ROBOT_INIT_POSE_X=-38.0
167 | ROBOT_INIT_POSE_Y=$(echo "38.0 - 2.0 * $1" | bc)
168 | ROBOT_INIT_POSE_Z=0.0
169 | }
170 | ##############################################################
171 |
172 | ############## EXPERIMENT ENVIRONMENT TESTINGS ###############
173 | ### TEST 0 ###
174 | [ -d morse ] || mkdir -p morse
175 | [ -d robot ] || mkdir -p robot
176 | [ -d log ] || mkdir -p log
177 | ### TEST 1 ###
178 | echo -n "===> Checking file \"$SIM_ENV_NAME.blend\" ...... "
179 | if [ ! -f $PWD/morse/$SIM_ENV_NAME.blend ]
180 | then
181 | echo "****************************************************
182 | ERROR: $SIM_ENV_NAME.blend not found.
183 | SOLUTION: put \"$SIM_ENV_NAME.blend\" into \"$PWD/morse\".
184 | ****************************************************"
185 | exit $ERROR_FILE_NOT_FOUND
186 | fi
187 | echo "found!"
188 | ### TEST 2 ###
189 | echo -n "===> Checking file \"id_rsa.pub\" ...... "
190 | if [ ! -f $HOME/.ssh/id_rsa.pub ]
191 | then
192 | echo "****************************************************
193 | ERROR: $HOME/.ssh/id_rsa.pub not found.
194 | SOLUTION: execute \"ssh-keygen\" command first.
195 | ****************************************************"
196 | exit $ERROR_FILE_NOT_FOUND
197 | fi
198 | echo "found!"
199 | ### TEST 3 ###
200 | auto_ssh $MORSE_PASS $MORSE_USER@$MORSE_HOST :
201 | SSH_ERROR_CODE=$?
202 | if [ $SSH_ERROR_CODE -eq 0 ]
203 | then
204 | auto_ssh_copy_id $MORSE_PASS "-i $HOME/.ssh/id_rsa.pub $MORSE_USER@$MORSE_HOST"
205 | if [ $? -eq 1 ]
206 | then
207 | exit $ERROR_AUTO_SSH_COPY_ID
208 | fi
209 | fi
210 | if [ $SSH_ERROR_CODE -gt 1 ]
211 | then
212 | exit $ERROR_AUTO_SSH
213 | fi
214 | echo "===> SSH authentication is ok!"
215 | ### TEST 4 ###
216 | if [ -z "$(ssh $MORSE_USER@$MORSE_HOST 'echo $ROS_HOSTNAME')" ]
217 | then
218 | echo "****************************************************
219 | ERROR: $0 can not be executed in interactive mode.
220 | SOLUTION: comment out \"[ -z \"\$PS1\" ] && return\" in $MORSE_USER@$MORSE_HOST:~/.bashrc.
221 | ****************************************************"
222 | exit $ERROR_SSH_INTERACTIVE_MODE
223 | fi
224 | echo "===> MORSE host is ready!"
225 | ### TEST 5 ###
226 | echo -n "===> Checking directory \"$ROBOT_PROTOTYPE_PATH\" ...... "
227 | if [ ! -d $ROBOT_PROTOTYPE_PATH ]
228 | then
229 | echo -n "not found. Copy from /lustre/zhi.yan ...... "
230 | cp -rf /lustre/zhi.yan/robot_prototype/ $ROBOT_PROTOTYPE_PATH
231 | if [ $? -ne 0 ]
232 | then
233 | echo "****************************************************
234 | ERROR: can not get the robot prototype.
235 | SOLUTION: please contact Zhi Yan (yz@ai.univ-paris8.fr).
236 | ****************************************************"
237 | exit $ERROR_DIRECTORY_NOT_FOUND
238 | else
239 | echo "done!"
240 | fi
241 | else
242 | echo "found!"
243 | fi
244 | ### TEST 6 ###
245 | [ -z $PBS_JOBNAME ] && PBS_JOBNAME=$0
246 | if [[ $(qstat | grep -c $PBS_JOBNAME) -gt 1 || $(ssh $MORSE_USER@$MORSE_HOST "ps -ef | grep -c ros") -gt 2 || $(ps -ef | grep -c $0) -gt 3 ]]
247 | then
248 | echo "****************************************************
249 | ERROR: another experiment in progress.
250 | SOLUTION: please try again later.
251 | ****************************************************"
252 | exit $ERROR_RESOURCE_CONFLICT
253 | fi
254 | ##############################################################
255 |
256 | #### EXPERIMENT DEPLOYMENT PHASE I: CHAPO NODE ALLOCATION ####
257 | echo "
258 | **** EXPERIMENT DEPLOYMENT PHASE I: CHAPO NODE ALLOCATION ****
259 | "
260 | NODE_ALLOCATION_LIST=()
261 | if [ $DEBUG_MODE == "false" ]
262 | then
263 | NODES=$(sort -u $PBS_NODEFILE)
264 | for node_name in $NODES
265 | do
266 | NODE_ALLOCATION_LIST+=(${node_name})
267 | done
268 | else
269 | NODE_ALLOCATION_LIST+=( "chapo47" "chapo48" )
270 | fi
271 | #echo "NODE_ALLOCATION_LIST = ${NODE_ALLOCATION_LIST[*]}"
272 |
273 | ROBOT_IP_LIST=()
274 | for robot_id in $(seq 0 $[ $N_ROBOTS_MAX - 1 ])
275 | do
276 | ROBOT_IP_LIST+=(${ROBOT_PROTOTYPE_IP/99/$[ 100 + robot_id ]})
277 | echo "robot$robot_id (${ROBOT_IP_LIST[$robot_id]}) will be deployed on ${NODE_ALLOCATION_LIST[$[ $robot_id / $N_ROBOTS_PER_NODE ]]}."
278 | done
279 | #echo "ROBOT_IP_LIST = ${ROBOT_IP_LIST[*]}"
280 | ##############################################################
281 |
282 | #### EXPERIMENT DEPLOYMENT PHASE II: VIRTUALMACHINE SETUP ####
283 | echo "
284 | **** EXPERIMENT DEPLOYMENT PHASE II: VIRTUALMACHINE SETUP ****
285 | "
286 | echo "### THIS FILE IS AUTOMATICALLY GENERATED BY $0 ###
287 | $ROBOT_PROTOTYPE_IP" > $PWD/robot/ipSynchronizationTable
288 |
289 | echo "### THIS FILE IS AUTOMATICALLY GENERATED BY $0 ###
290 | ### YOU CAN RUN IT MANUALLY IF NECESSARY ###
291 |
292 | #!/bin/bash
293 | " > $PWD/clean/kill_all.sh
294 |
295 | NODE_NAME_INDEX=0
296 | for node_name in ${NODE_ALLOCATION_LIST[*]}
297 | do
298 | echo "ssh $node_name \"kill -9 \\\$(ps -ef | grep ${HOME##*/} | grep -v grep | awk '{print \\\$2}')\"
299 | echo -e \"\\033[31m===> All processes have been killed on $node_name.\\033[0m\"" >> $PWD/clean/kill_all.sh
300 |
301 | echo "#!/bin/bash
302 | ### THIS FILE IS AUTOMATICALLY GENERATED BY $0 ###
303 |
304 | RESET_VM=\"$RESET_VM\"
305 |
306 | N_ROBOTS_PER_NODE=$N_ROBOTS_PER_NODE
307 | N_VM_MEMS=$N_VM_MEMS
308 | N_VM_CPUS=$N_VM_CPUS
309 |
310 | ROBOT_PROTOTYPE_PATH=\"$ROBOT_PROTOTYPE_PATH\"
311 | ROBOT_PROTOTYPE_IP=\"$ROBOT_PROTOTYPE_IP\"
312 | ROBOT_PROTOTYPE_USER=\"$ROBOT_PROTOTYPE_USER\"
313 | ROBOT_PROTOTYPE_PASS=\"$ROBOT_PROTOTYPE_PASS\"
314 |
315 | ROBOT_DEPLOYMENT_PATH=\"$ROBOT_DEPLOYMENT_PATH\"
316 | ROBOT_IP_LIST=( ${ROBOT_IP_LIST[@]:$[ $NODE_NAME_INDEX * $N_ROBOTS_PER_NODE ]:$N_ROBOTS_PER_NODE} )
317 |
318 | SCRIPT_HOME=$PWD" > vm_setup_header.sh
319 |
320 | echo -n "===> Generating file \"vm_setup_$node_name.sh\" ...... "
321 | cat vm_setup_header.sh vm_setup_main.sh > $PWD/robot/vm_setup_$node_name.sh
322 | rm vm_setup_header.sh
323 | chmod a+x $PWD/robot/vm_setup_$node_name.sh
324 | echo "done!"
325 |
326 | ssh $node_name '[ -d $ROBOT_DEPLOYMENT_PATH ] || mkdir -p $ROBOT_DEPLOYMENT_PATH'
327 | scp $PWD/log_robot_performance.sh $PWD/robot/vm_setup_$node_name.sh $node_name:$ROBOT_DEPLOYMENT_PATH
328 | [ -f $PWD/log/vm_setup_$node_name.log ] && rm log/vm_setup_$node_name.log # Important! To avoid synchronization error for VM_SETUP_FAILS/VM_SETUP_COMPLETED test.
329 | ssh -f $node_name '$SHELL $ROBOT_DEPLOYMENT_PATH/vm_setup_$node_name.sh > $PWD/log/vm_setup_$node_name.log 2>&1'
330 | echo "===> Deploying robot(s) on $node_name ......"
331 |
332 | NODE_NAME_INDEX=$(( NODE_NAME_INDEX + 1 ))
333 | done
334 | echo -e "\033[34mYou can check the log files in $PWD/log any time to know the deployment process.\033[0m"
335 |
336 | ### This test is not robust, but I currently can not find a better way. ###
337 | for node_name in ${NODE_ALLOCATION_LIST[*]}
338 | do
339 | while [ 1 ]
340 | do
341 | if [ -f $PWD/log/vm_setup_$node_name.log ]
342 | then
343 | case $(tail -1 $PWD/log/vm_setup_$node_name.log) in
344 | VM_SETUP_FAILS)
345 | echo "ERROR: robot(s) deployment is failed on $node_name."
346 | $SHELL $PWD/clean/kill_all.sh
347 | exit $ERROR_VM_SETUP_FAILS
348 | ;;
349 | VM_SETUP_COMPLETED)
350 | echo "===> Robot(s) deployment is completed on $node_name."
351 | break
352 | ;;
353 | esac
354 | fi
355 | sleep 3
356 | done
357 | done
358 | ##############################################################
359 |
360 | ###################### START EXPERIMENT ######################
361 | echo "
362 | ********************** START EXPERIMENT **********************
363 | "
364 | BOOTED_ROBOTS=0
365 | READY_ROBOTS_TABLE=()
366 | PING_TIMER=0
367 | PING_TIMEOUT=60
368 | FAILED_ROBOTS=0
369 | MAX_FAILED_ROBOTS=0
370 |
371 | for n_robots in $(seq $N_ROBOTS_MIN $N_ROBOTS_MAX)
372 | do
373 | for trial_id in $(seq 1 $N_TRIALS_PTS)
374 | do
375 | echo "===> $n_robots robots | test $trial_id <==="
376 | ### The following codes are not indented for ease reading! ###
377 | ############ CREATE A SIMULATION DESCRIPTION FILE ############
378 | echo -n "===> Creating file \"$SIM_ENV_NAME.py\" ...... "
379 | echo "from morse.builder import *
380 | ### THIS FILE IS AUTOMATICALLY GENERATED BY $0 ###" > $PWD/morse/$SIM_ENV_NAME.py
381 | for i in $(seq 0 $[ $n_robots - 1 ])
382 | do
383 | set_robot_init_pose $i
384 | echo "
385 | ################ ROBOT -- $i ###############
386 | p3dx$i = Pioneer3DX()
387 | p3dx$i.add_default_interface('ros')
388 | p3dx$i.translate(x=$ROBOT_INIT_POSE_X, y=$ROBOT_INIT_POSE_Y, z=$ROBOT_INIT_POSE_Z)
389 |
390 | odom$i = Odometry()
391 | odom$i.add_interface('ros', frame_id='/p3dx$i_tf/odom', child_frame_id='/p3dx$i_tf/base_footprint')" >> $PWD/morse/$SIM_ENV_NAME.py
392 | [ $ODOMETRY_NOISE == "true" ] && echo "odom$i.alter('Noise', pos_std=0.022, rot_std=0.02)" >> $PWD/morse/$SIM_ENV_NAME.py
393 | echo "odom$i.frequency(10)
394 | p3dx$i.append(odom$i)
395 |
396 | sick$i = Sick()
397 | sick$i.translate(z=0.252)
398 | sick$i.properties(Visible_arc=True)
399 | sick$i.properties(scan_window=190)
400 | sick$i.properties(laser_range=26.0)
401 | sick$i.add_interface('ros', frame_id='/p3dx$i_tf/base_laser_link')
402 | sick$i.frequency(25)
403 | p3dx$i.append(sick$i)
404 |
405 | motion$i = MotionVWDiff()
406 | motion$i.add_interface('ros')
407 | p3dx$i.append(motion$i)" >> $PWD/morse/$SIM_ENV_NAME.py
408 | done
409 | echo "
410 | ################ ENVIRONMENT ###############
411 | env = Environment('$MORSE_FILE_PATH/$SIM_ENV_NAME', fastmode=False)
412 | env.place_camera([0, 0, 60])
413 | env.aim_camera([0, 0, 0])" >> $PWD/morse/$SIM_ENV_NAME.py
414 | echo "done!"
415 | ##############################################################
416 |
417 | ############## CREATE A SCRIPT TO LAUNCH MORSE ###############
418 | echo -n "===> Creating file \"$SIM_ENV_NAME.sh\" ...... "
419 | echo "#!/bin/bash
420 | ### THIS FILE IS AUTOMATICALLY GENERATED BY $0 ###
421 |
422 | export ROS_HOSTNAME=$MORSE_HOST
423 | export ROS_MASTER_URI=http://$MORSE_HOST:11311
424 |
425 | mkdir -p $MONITOR_LOG_PATH
426 |
427 | rm -rf ~/.ros/log
428 |
429 | PARAMS=()
430 | PARAMS+=( --tab -e \"bash -c 'roscore;exec bash'\" )
431 | PARAMS+=( --tab -e \"bash -c 'morse run -g 768x768 $SIM_ENV_NAME.py;exec bash'\" )
432 | PARAMS+=( --tab -e \"bash -c 'sleep 3;rosrun tf_splitter tf_splitter;exec bash'\" )
433 | PARAMS+=( --tab -e \"bash -c 'sleep 3;rosrun $MONITOR_PKG_NAME $MONITOR_PKG_NAME _number_robots:=$n_robots _static_map_file:=$MORSE_FILE_PATH/$SIM_ENV_NAME.pgm _dynamic_map_node:=zexploration _explore_cost_node:=zexploration _result_file_path:=$MONITOR_LOG_PATH/${n_robots}robots_test${trial_id}.csv _supervision_rate:=5.0 _explored_match_size:=0.99 _running_timeout:=2000;nc -w 0 $(hostname --ip-address) $MONITOR_LISTEN_PORT;exit;exec bash'\" )
434 | gnome-terminal --working-directory=$MORSE_FILE_PATH \"\${PARAMS[@]}\"" > $PWD/morse/$SIM_ENV_NAME.sh
435 | chmod a+x $PWD/morse/$SIM_ENV_NAME.sh
436 | echo "done!"
437 | ##############################################################
438 |
439 | ################### START MORSE SIMULATOR ####################
440 | echo "#!/bin/bash
441 | ### THIS FILE IS AUTOMATICALLY GENERATED BY $0 ###
442 | ### YOU CAN RUN IT MANUALLY IF NECESSARY ###
443 |
444 | ssh $MORSE_USER@$MORSE_HOST \"kill -9 \\\$(ps -ef | grep morse | grep -v gnome-terminal | grep -v grep | awk '{print \\\$2}')\"
445 | echo -e \"\\033[31m===> MORSE has been shutdown on \\\"$MORSE_HOST\\\".\\033[0m\"
446 | ssh $MORSE_USER@$MORSE_HOST \"kill -9 \\\$(ps -ef | grep ros | grep -v gnome-terminal | grep -v grep | awk '{print \\\$2}')\"
447 | echo -e \"\\033[31m===> ROS has been shutdown on \\\"$MORSE_HOST\\\".\\033[0m\"
448 | sleep 3" > $PWD/clean/kill_run.sh
449 |
450 | scp $PWD/morse/$SIM_ENV_NAME.* $MORSE_USER@$MORSE_HOST:$MORSE_FILE_PATH
451 | echo "===> Simulation files have been copied to \"$MORSE_USER@$MORSE_HOST:$MORSE_FILE_PATH\"."
452 | ssh -f $MORSE_USER@$MORSE_HOST 'DISPLAY=:0 $MORSE_FILE_PATH/$SIM_ENV_NAME.sh'
453 | echo "===> MORSE has been launched on \"$MORSE_HOST\"."
454 | echo "===> Monitor has been launched on \"$MORSE_HOST\"."
455 | ##############################################################
456 |
457 | ####################### START ROBOT(S) #######################
458 | BOOTED_ROBOTS=0
459 | for node_name in ${NODE_ALLOCATION_LIST[*]}
460 | do
461 | for i in $(seq 0 $[ $N_ROBOTS_PER_NODE - 1 ])
462 | do
463 | echo "
464 | echo -ne \"\\033[31m===> robot_${node_name}_$i OFF \\033[0m\"
465 | ssh $node_name \"export VBOX_USER_HOME=$ROBOT_DEPLOYMENT_PATH/vm; vboxmanage controlvm robot_${node_name}_$i poweroff\"" >> $PWD/clean/kill_run.sh
466 |
467 | ssh $node_name 'export VBOX_USER_HOME=$ROBOT_DEPLOYMENT_PATH/vm; vboxmanage startvm robot_${node_name}_$i --type headless'
468 | BOOTED_ROBOTS=$(( BOOTED_ROBOTS + 1 ))
469 | [ $BOOTED_ROBOTS -ge $n_robots ] && break
470 | done
471 | [ $BOOTED_ROBOTS -ge $n_robots ] && break
472 | done
473 |
474 | unset READY_ROBOTS_TABLE
475 | for i in $(seq 0 $[ $n_robots - 1 ])
476 | do
477 | PING_TIMER=0
478 | while [ 1 ]
479 | do
480 | if [ $(ping -c 1 -w 1 ${ROBOT_IP_LIST[$i]} &>/dev/null;echo $?) -ne 0 ]
481 | then
482 | sleep 1
483 | PING_TIMER=$(( PING_TIMER + 1 ))
484 | echo "Robot ${ROBOT_IP_LIST[$i]} does not respond for $PING_TIMER seconds."
485 | if [ $PING_TIMER -ge $PING_TIMEOUT ]
486 | then
487 | READY_ROBOTS_TABLE+=( "false" )
488 | echo "ERROR: robot ${ROBOT_IP_LIST[$i]} does not respond within $PING_TIMER seconds."
489 | echo "WARNING: the experiment will start without robot ${ROBOT_IP_LIST[$i]}."
490 | break
491 | fi
492 | else
493 | READY_ROBOTS_TABLE+=( "true" )
494 | echo "===> Robot ${ROBOT_IP_LIST[$i]} is ready."
495 | break
496 | fi
497 | done
498 | done
499 |
500 | FAILED_ROBOTS=0
501 | for i in $(seq 0 $[ $n_robots - 1 ])
502 | do
503 | set_robot_init_pose $i
504 | if [ ${READY_ROBOTS_TABLE[i]} == "true" ]
505 | then
506 | ssh -f $ROBOT_PROTOTYPE_USER@${ROBOT_IP_LIST[$i]} 'rm -rf /home/viki/.ros/log; /media/sf_scratch/${HOME##*/}/log_robot_performance.sh $n_robots $trial_id $i ${HOME##*/}; roslaunch /home/viki/Desktop/car_mrs_explore/launch/pioneer3dx.launch i:=$i x:=$ROBOT_INIT_POSE_X y:=$ROBOT_INIT_POSE_Y z:=$ROBOT_INIT_POSE_Z' > /dev/null 2>&1
507 | echo "===> Robot ${ROBOT_IP_LIST[$i]} has been launched at ($ROBOT_INIT_POSE_X, $ROBOT_INIT_POSE_Y, $ROBOT_INIT_POSE_Z)."
508 | else
509 | echo "===> Robot ${ROBOT_IP_LIST[$i]} can not be launched."
510 | FAILED_ROBOTS=$(( FAILED_ROBOTS + 1 ))
511 | if [ $FAILED_ROBOTS -gt $MAX_FAILED_ROBOTS ]
512 | then
513 | echo -e "\033[31mERROR: failed robots exceeds the maximum number $MAX_FAILED_ROBOTS.\033[0m"
514 | echo -e "\032[31mSOLUTION: please check the virtual machine is set up correctly.\033[0m"
515 | #exit $ERROR_MAX_FAILED_ROBOTS # patch20140916
516 | ERROR_MAX_FAILED_ROBOTS=0 # patch20140916
517 |
518 | fi
519 | fi
520 | done
521 | ##############################################################
522 |
523 | ################### END RUN & CLEANING UP ####################
524 | if [ $ERROR_MAX_FAILED_ROBOTS -ne 0 ] # patch20140916
525 | then # patch20140916
526 | echo -n "===> Waiting for END RUN signal from the monitor $MORSE_HOST ...... "
527 | nc -d -l $MONITOR_LISTEN_PORT
528 | echo "roger!"
529 | fi # patch20140916
530 | $SHELL $PWD/clean/kill_run.sh
531 | ###### The following lines handle the fucking bug "ssh -f" (not been solved) ######
532 | #for node_name in ${NODE_ALLOCATION_LIST[*]}
533 | #do
534 | # ssh $node_name "kill -9 \$(ps -ef | grep \"ssh -f\" | awk '{print \$2}')"
535 | #done
536 | ###### The following lines handle the fucking bug "VM shutdown blocked" (not been solved) ######
537 | #for node_name in ${NODE_ALLOCATION_LIST[*]}
538 | #do
539 | # ssh $node_name "kill -9 \$(ps -ef | grep \"VBoxXPCOMIPCD\" | awk '{print \$2}')"
540 | # ssh $node_name "kill -9 \$(ps -ef | grep \"VBoxSVC --auto-shutdown\" | awk '{print \$2}')"
541 | #done
542 | ##############################################################
543 | #################### end for not indented ####################
544 | done
545 | [ $ERROR_MAX_FAILED_ROBOTS -eq 0 ] && break # patch20140916
546 | done
547 | ##############################################################
548 |
549 | ################## GET EXPERIMENTAL RESULTS ##################
550 | EXPERIMENTAL_RESULTS_PATH=$(dirname $(pwd))/${HOME##*/}_$(date '+%Y%m%d_%H%M')
551 | mkdir -p $EXPERIMENTAL_RESULTS_PATH/system_metric
552 | mkdir -p $EXPERIMENTAL_RESULTS_PATH/robot_metric
553 |
554 | scp $MORSE_USER@$MORSE_HOST:$MONITOR_LOG_PATH/* $EXPERIMENTAL_RESULTS_PATH/system_metric
555 | ssh $MORSE_USER@$MORSE_HOST 'rm -rf $MONITOR_LOG_PATH'
556 |
557 | for node_name in ${NODE_ALLOCATION_LIST[*]}
558 | do
559 | scp $node_name:$ROBOT_DEPLOYMENT_PATH/log/* $EXPERIMENTAL_RESULTS_PATH/robot_metric
560 | ssh $node_name 'rm -rf $ROBOT_DEPLOYMENT_PATH/log'
561 | done
562 |
563 | echo "===> Experimental results are stored in $EXPERIMENTAL_RESULTS_PATH."
564 | ##############################################################
565 |
566 | echo "
567 | ##############################################################
568 | # Script has been successfully executed! #
569 | # Zhi Yan, Luc Fabresse, Jannik Laval, and Noury Bouraqadi #
570 | # Copyright 2014 #
571 | ##############################################################
572 | "
573 |
--------------------------------------------------------------------------------
/scripts/submit_job.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | ##############################################################################
4 | # Script for multi-robot exploration experiments on CHAPO of Ecole des Mines #
5 | # Zhi Yan, Luc Fabresse, Jannik Laval, and Noury Bouraqadi #
6 | # Copyright 2014 yz@ai.univ-paris8.fr #
7 | ##############################################################################
8 |
9 | ###### Set your name to run start_simulation.sh correctly. ######
10 | # sed -i 's?zhi.yan?'$(echo ${HOME##*/})'?' start_simulation.sh #
11 | ###### Create a directory to store job submission results. ######
12 | [ -d ../Qsubresults ] || mkdir -p ../Qsubresults
13 | #################################################################
14 |
15 | ##################### ADVISE FOR FIRST RUN #####################
16 | if [ $# -eq 0 ]
17 | then
18 | echo -e "\033[34mWelcome to use our script for multi-robot exploration experiments!\033[0m"
19 | echo -e "\033[34mIf this is your first time, I suggest you to run once in debug mode, rather than submit the job to CHAPO immediately.\033[0m"
20 | echo -ne "\033[33mYou want to run it in debug mode? [y/n]: \033[0m"
21 | read ANSWER
22 | case $ANSWER in
23 | y|Y|yes|YES)
24 | $SHELL start_simulation.sh
25 | exit 0
26 | ;;
27 | esac
28 | fi
29 | #################################################################
30 |
31 | ###### EXPERIMENT PARAMETERS PART 1: NEED TO BE SPECIFIED ######
32 | if [[ $# -ne 3 ]]
33 | then
34 | echo "Usage:" $0 "[MIN NUMBER OF ROBOTS] [MAX NUMBER OF ROBOTS] [NUMBER OF TRIALS PER TEAM SIZE]"
35 | exit 0
36 | fi
37 | N_ROBOTS_MIN=$1
38 | N_ROBOTS_MAX=$2
39 | N_TRIALS_PTS=$3
40 | #################################################################
41 |
42 | ############## EXPERIMENT PARAMETERS PART 2: PRESET #############
43 | N_ROBOTS_PER_NODE=4
44 | N_VM_MEMS=2048
45 | N_VM_CPUS=2
46 | #################################################################
47 |
48 | ############# EXPERIMENT PARAMETERS PART 2: ADAPTED #############
49 | N_NODES=$[ $N_ROBOTS_MAX / $N_ROBOTS_PER_NODE ]
50 | if [ $[ $N_ROBOTS_MAX % $N_ROBOTS_PER_NODE ] -ne 0 ]
51 | then
52 | N_NODES=$[ $N_NODES + 1 ]
53 | fi
54 | #################################################################
55 |
56 | ######################## SUBMIT THE JOB #########################
57 | qsub \
58 | -N car_mrs_explore \
59 | -d $PWD \
60 | -r n \
61 | -m abe \
62 | -M ${HOME##*/}@mines-douai.fr \
63 | -l nodes=$N_NODES:ppn=8 \
64 | -l walltime=500:00:00 \
65 | -l pmem=1gb \
66 | -o $(dirname $(pwd))/Qsubresults \
67 | -e $(dirname $(pwd))/Qsubresults \
68 | -j oe \
69 | -v DEBUG_MODE=false,RESET_VM=true,SIM_ENV_NAME=maze,ODOMETRY_NOISE=true,N_ROBOTS_MIN=$N_ROBOTS_MIN,N_ROBOTS_MAX=$N_ROBOTS_MAX,N_TRIALS_PTS=$N_TRIALS_PTS,N_ROBOTS_PER_NODE=$N_ROBOTS_PER_NODE,N_VM_MEMS=$N_VM_MEMS,N_VM_CPUS=$N_VM_CPUS \
70 | start_simulation.sh
71 | #################################################################
72 |
73 | echo "
74 | ##############################################################
75 | ## Your job has been submitted with following parameters: ##
76 | ## ##
77 | * N_ROBOTS_MIN = $N_ROBOTS_MIN
78 | * N_ROBOTS_MAX = $N_ROBOTS_MAX
79 | * N_TRIALS_PTS = $N_TRIALS_PTS
80 | * N_ROBOTS_PER_NODE = $N_ROBOTS_PER_NODE
81 | * N_VM_MEMS = $N_VM_MEMS
82 | * N_VM_CPUS = $N_VM_CPUS
83 | * N_NODES = $N_NODES
84 | ## ##
85 | ## Zhi Yan, Luc Fabresse, Jannik Laval, and Noury Bouraqadi ##
86 | ## Copyright 2014 ##
87 | ##############################################################
88 | "
89 |
--------------------------------------------------------------------------------
/scripts/vm_setup_main.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | ### Copyright (C) 2014 Zhi Yan
4 |
5 | NFS_BUG_TIMER=0
6 | NFS_BUG_TIMEOUT=30
7 |
8 | PING_TIMER=0
9 | PING_TIMEOUT=60
10 |
11 | VM_SHUTDOWN_TIMER=0
12 | VM_SHUTDOWN_TIMEOUT=30
13 |
14 | auto_ssh() {
15 | expect -c "set timeout -1;
16 | spawn ssh -o StrictHostKeyChecking=no $2 ${@:3};
17 | expect {
18 | *password* {
19 | send -- $1\r;
20 | expect {
21 | *denied* {exit 3;}
22 | eof
23 | }
24 | }
25 | *route* {exit 2;}
26 | eof {exit 1;}
27 | }"
28 | return $?
29 | }
30 |
31 | auto_ssh_copy_id() {
32 | expect -c "set timeout -1;
33 | spawn ssh-copy-id $2;
34 | expect {
35 | *ERROR* {exit 1;}
36 | *Usage* {exit 1;}
37 | *(yes/no)* {send -- yes\r;exp_continue;}
38 | *password:* {send -- $1\r;exp_continue;}
39 | eof {exit 0;}
40 | }"
41 | }
42 |
43 | ### Reset VM configuration file (VirtualBox.xml) and log files ###
44 | # [ -d vm ] && rm -rf vm
45 | # mkdir vm
46 | ### The above 2 lines is the cleanest solution, ###
47 | ### but it raises a fucking synchronization bug: ###
48 | ### rm: cannot remove vm/.nfsXXX: Device or resource busy! ###
49 | ### So we have to do is: ###
50 | if [ -d $ROBOT_DEPLOYMENT_PATH/vm ]
51 | then
52 | while [ $(rm -rf $ROBOT_DEPLOYMENT_PATH/vm &>/dev/null;echo $?) -ne 0 ]
53 | do
54 | sleep 1
55 | NFS_BUG_TIMER=$(( NFS_BUG_TIMER + 1 ))
56 | echo ".nfs file(s) can not be removed for $NFS_BUG_TIMER seconds."
57 | if [ $NFS_BUG_TIMER -ge $NFS_BUG_TIMEOUT ]
58 | then
59 | echo "ERROR: .nfs file(s) can not be removed within $NFS_BUG_TIMER seconds!"
60 | { echo "VM_SETUP_FAILS";exit 1; }
61 | fi
62 | done
63 | fi
64 | mkdir -p $ROBOT_DEPLOYMENT_PATH/vm
65 | echo "Folder \"$ROBOT_DEPLOYMENT_PATH/vm\" has been cleared."
66 |
67 | ### Set VBOX environment variables for depositing VirtualBox.xml ###
68 | export VBOX_USER_HOME=$ROBOT_DEPLOYMENT_PATH/vm
69 | echo VBOX_USER_HOME=$VBOX_USER_HOME
70 |
71 | ### Copy and register robot prototype ###
72 | if [[ $RESET_VM == "true" || ! -d $ROBOT_DEPLOYMENT_PATH/robot_prototype ]]
73 | then
74 | cp -r $ROBOT_PROTOTYPE_PATH $ROBOT_DEPLOYMENT_PATH
75 | echo "robot_prototype has been copied from $ROBOT_PROTOTYPE_PATH to $ROBOT_DEPLOYMENT_PATH."
76 | fi
77 | vboxmanage registervm $ROBOT_DEPLOYMENT_PATH/robot_prototype/robot_prototype.vbox || { echo "VM_SETUP_FAILS";exit 1; }
78 |
79 | ### Clone and register robots (VMs) ###
80 | for i in $(seq 0 $[ ${#ROBOT_IP_LIST[@]} - 1 ])
81 | do
82 | ### For safety and also avoiding bugs, we always clone VMs. It does not take long time. ###
83 | #if [[ $RESET_VM == "true" || ! -d $ROBOT_DEPLOYMENT_PATH/robot_${HOSTNAME%%.*}_$i ]]
84 | #then
85 | #vboxmanage clonevm robot_prototype --name robot_${HOSTNAME%%.*}_$i --basefolder $ROBOT_DEPLOYMENT_PATH
86 | #fi
87 | #vboxmanage registervm $ROBOT_DEPLOYMENT_PATH/robot_${HOSTNAME%%.*}_$i/robot_${HOSTNAME%%.*}_$i.vbox
88 | [ -d $ROBOT_DEPLOYMENT_PATH/robot_${HOSTNAME%%.*}_$i ] && rm -rf $ROBOT_DEPLOYMENT_PATH/robot_${HOSTNAME%%.*}_$i
89 | vboxmanage clonevm robot_prototype --name robot_${HOSTNAME%%.*}_$i --basefolder $ROBOT_DEPLOYMENT_PATH --register || { echo "VM_SETUP_FAILS";exit 1; }
90 | done
91 |
92 | ### Modify robots' IP address ###
93 | LAST_ASSIGNED_IP_ADDRESS=$(tail -1 $SCRIPT_HOME/robot/ipSynchronizationTable)
94 | for i in $(seq 0 $[ ${#ROBOT_IP_LIST[@]} - 1 ])
95 | do
96 | echo -e "\n**************** robot_${HOSTNAME%%.*}_$i ****************\n"
97 |
98 | ### Avoid to simultaneously modify multiple ROBOT_PROTOTYPE_IPs causing conflict ###
99 | echo -n "Waiting for IP address assignment ${ROBOT_IP_LIST[$i]} ...... "
100 | while [ $[ ${LAST_ASSIGNED_IP_ADDRESS##*.} + 1 ] -ne ${ROBOT_IP_LIST[$i]##*.} ]
101 | do
102 | sleep 1
103 | LAST_ASSIGNED_IP_ADDRESS=$(tail -1 $SCRIPT_HOME/robot/ipSynchronizationTable)
104 | done
105 | echo "ok!"
106 |
107 | ### Start the virtual machine (robot) ###
108 | vboxmanage startvm robot_${HOSTNAME%%.*}_$i --type headless || { echo "VM_SETUP_FAILS";exit 1; }
109 |
110 | ### Make sure the VM has been started within the prescribed time ###
111 | PING_TIMER=0
112 | while [ $(ping -c 1 -w 1 $ROBOT_PROTOTYPE_IP &>/dev/null;echo $?) -ne 0 ]
113 | do
114 | sleep 1
115 | PING_TIMER=$(( PING_TIMER + 1 ))
116 | echo "robot_${HOSTNAME%%.*}_$i does not respond for $PING_TIMER seconds."
117 | if [ $PING_TIMER -ge $PING_TIMEOUT ]
118 | then
119 | echo "ERROR: robot_${HOSTNAME%%.*}_$i does not respond within $PING_TIMER seconds, force shutdown:"
120 | vboxmanage controlvm robot_${HOSTNAME%%.*}_$i poweroff
121 | { echo "VM_SETUP_FAILS";exit 1; }
122 | fi
123 | done
124 |
125 | ### (28/09/2014) This part should be tested and validated by Luc ###
126 | auto_ssh $ROBOT_PROTOTYPE_PASS $ROBOT_PROTOTYPE_USER@$ROBOT_PROTOTYPE_IP :
127 | SSH_ERROR_CODE=$?
128 | if [ $SSH_ERROR_CODE -eq 0 ]
129 | then
130 | auto_ssh_copy_id $ROBOT_PROTOTYPE_PASS "-i $HOME/.ssh/id_rsa.pub $ROBOT_PROTOTYPE_USER@$ROBOT_PROTOTYPE_IP"
131 | [ $? -eq 1 ] && { echo "VM_SETUP_FAILS";exit 1; }
132 | fi
133 | [ $SSH_ERROR_CODE -gt 1 ] && { echo "VM_SETUP_FAILS";exit 1; }
134 | echo "===> SSH authentication is ok!"
135 | ### (28/09/2014) end of part ###
136 |
137 | ### Modify the virtual machine IP address ###
138 | echo "robot_${HOSTNAME%%.*}_$i is ready."
139 | ### IF the route for CHAPO is changed, DO ###
140 | # ssh $ROBOT_PROTOTYPE_USER@$ROBOT_PROTOTYPE_IP "echo -e \"\nauto eth0\niface eth0 inet static\naddress ${ROBOT_IP_LIST[$i]}\nnetmask 255.255.0.0\ngateway 10.3.0.1\ndns-nameservers 10.3.11.1\npost-up route add -net 10.1.0.0 netmask 255.255.0.0 gw 10.3.11.1\n\" | sudo tee -a /etc/network/interfaces; sudo shutdown -h now"
141 | ### IF NOT ###
142 | ssh $ROBOT_PROTOTYPE_USER@$ROBOT_PROTOTYPE_IP 'echo -e \"\nauto eth0\niface eth0 inet static\naddress ${ROBOT_IP_LIST[$i]}\nnetmask 255.255.0.0\ngateway 10.3.0.1\ndns-nameservers 10.3.11.1\n\" | sudo tee -a /etc/network/interfaces; sudo shutdown -h now'
143 | ### END IF ###
144 | echo "robot_${HOSTNAME%%.*}_$i was set a new IP address: ${ROBOT_IP_LIST[$i]}."
145 |
146 | ### The following lines handle the fucking bug "VM can not be shutdown properly" ###
147 | VM_SHUTDOWN_TIMER=0
148 | while [[ $(vboxmanage showvminfo robot_${HOSTNAME%%.*}_$i | grep "State") == *running* ]]
149 | do
150 | sleep 1
151 | VM_SHUTDOWN_TIMER=$(( VM_SHUTDOWN_TIMER + 1 ))
152 | echo "robot_${HOSTNAME%%.*}_$i does not shutdown for $VM_SHUTDOWN_TIMER seconds."
153 | if [ $VM_SHUTDOWN_TIMER -ge $VM_SHUTDOWN_TIMEOUT ]
154 | then
155 | echo "robot_${HOSTNAME%%.*}_$i does not shutdown within $VM_SHUTDOWN_TIMER seconds, force shutdown:"
156 | vboxmanage controlvm robot_${HOSTNAME%%.*}_$i poweroff || { echo "VM_SETUP_FAILS";exit 1; }
157 | fi
158 | done
159 | echo "robot_${HOSTNAME%%.*}_$i has been shutdown."
160 | echo "${ROBOT_IP_LIST[$i]}" >> $SCRIPT_HOME/robot/ipSynchronizationTable
161 |
162 | ### Setup the memory size (MB) and the number of CPUs for VM ###
163 | sleep 3 # sleep for avoiding the fucking VBoxManage error: The machine is already locked for a session (or being unlocked)
164 | vboxmanage modifyvm robot_${HOSTNAME%%.*}_$i --memory $N_VM_MEMS --cpus $N_VM_CPUS || { echo "VM_SETUP_FAILS";exit 1; }
165 | echo "robot_${HOSTNAME%%.*}_$i memory = $N_VM_MEMS, cpus = $N_VM_CPUS."
166 | done
167 |
168 | echo "VM_SETUP_COMPLETED"
169 |
--------------------------------------------------------------------------------
/tutorial/demo_28112013.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/tutorial/demo_28112013.mp4
--------------------------------------------------------------------------------
/tutorial/tutorial.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yzrobot/mrs_testbed/bb48b2f9afed0951517ba626452aec4ec7a99805/tutorial/tutorial.pdf
--------------------------------------------------------------------------------