This repository contains the SwarmieOS code based on College’s 2018 & 2019 submissions to the NASA Swarmathon, a national swarm robotics competition.
The base code provided to participating schools can be found here SwarmBaseCode-ROS.
This repository contains:
- Source code for ROS packages that control different aspects of the Swarmie robot, including robot behaviours such as search strategies and obstacle avoidance, diagnostics, and user interface elements.
- 3D .STL models for the physical Swarmie build
- Bash shell scripts and launch files for initializing simulated Swarmies in the Gazebo simulator, as well as physical Swarmies
- Overview
- Table of Contents
- Quick Start Installation Guide
- Software Documentation
- Robot Behaviors
- Using the code
SwarmBaseCode-ROS is designed and tested exclusively on the 64 bit version of Ubuntu 16.04 LTS (Xenial Xerus) and ROS Kinetic Kame. Other systems are untested and are therefore not supported at this time.
Be sure you are using the latest drivers for your video card using the "additional drivers tool" in Ubuntu. Gazebo client often does not do well with the open source drivers.
- Install ROS Kinetic. Instructions below in the detailed guide.
- Install dependencies, clone and build.
sudo apt install python-rosinstall python-catkin-tools ros-kinetic-robot-localization ros-kinetic-hector-gazebo-plugins ros-kinetic-joystick-drivers ros-kinetic-grid-map ros-kinetic-rosserial-python ros-kinetic-rosserial-arduino ros-kinetic-usb-cam ros-kinetic-multimaster-fkie libusb-dev ipython git clone --recursive [email protected]:BCLab-UNM/SwarmieOS.git cd SwarmieOS catkin build source ./devel/setup.bash
Detailed instructions for installing ROS Kinetic Kame under Ubuntu 16.04 here or follow the summarized instructions below:
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
sudo apt update
sudo apt install ros-kinetic-desktop-full
sudo rosdep init
rosdep update # Note this is not run with sudo
Note: if you accidentally ran sudo rosdep update you can repair the permissions sudo rosdep fix-permissions.
From ROS Wiki: It’s convenient if the ROS environment variables are automatically added to your bash session every time a new shell is launched:
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
We use catkin_tools to build:
sudo apt install python-rosinstall python-catkin-tools
Most systems will already have usb development support installed, but just in case:
sudo apt install libusb-dev
We use robot_localization for state estimation and sensor fusion:
sudo apt install ros-kinetic-robot-localization
In simulation, we use hector_gazebo_plugins to provide sonar and IMU sensors:
sudo apt install ros-kinetic-hector-gazebo-plugins
The Swarmies can receive commands from the thumb sticks on a Microsoft Xbox 360 controller. The ROS joystick_drivers package, which contains a generic Linux joystick driver compatible with this controller:
sudo apt install ros-kinetic-joystick-drivers
Joystick commands can also be simulated using the direction keys (Up=I, Down=K, Left=J, Right=L) on the keyboard. The Rover GUI window must have focus for keyboard control to work.
Install the usb camera driver, which runs on the physical rover:
sudo apt install ros-kinetic-usb-cam
Install ROS Serial, which is used to run the IMU calibration code:
sudo apt install ros-kinetic-rosserial-python ros-kinetic-rosserial-arduino
Install ROS Multimaster, which is used to sync the ROS graphs of each physical rover:
sudo apt install ros-kinetic-multimaster-fkie
Install ROS Grid Map, which is used to build maps containing sonar-marked obstacles and locations of AprilTag detections:
sudo apt install ros-kinetic-grid-map
Install IPython, which we use to help develop robot behavior code.
sudo apt install ipython
-
Install git, if necessary.
sudo apt install git -
Clone the repository, initialize submodules, and build:
git clone --recursive [email protected]:BCLab-UNM/SwarmieOS.git cd SwarmieOS catkin build
- Start the GUI
./run.sh
The GUI will now launch.
This is the first screen of the GUI:
Click the "Simulation Control" tab:
There are several settings you can change:
-
Target Distribution: Choose between randomly generating a uniform, clustered, or power law distribution. You can also select a custom Gazebo world file; there are several pre-made world files included in the base code!
-
Number of Cubes: When you select a uniform or clustered target distribution, you can select the total number of apriltag cubes placed. This is not available for the power law distribution or custom world files at this time. The point of this option is to speed up the process of placing cubes in the simulation using these distribution types.
-
Ground Texture: Choose the kind of ground texture that will appear in the Gazebo simulation.
-
Round Type: Preliminary round creates a 15 meter square arena and generates a default of three rovers. Final round creates a 23.1 meter square arena and generates a default of six rovers. Unbounded round does not place any barriers around the edge of the arena, it generates a default of three rovers, and you can set the size of the arena in the dropbox below the unbounded radio button. Cubes placed using the uniform, clustered, or powerlaw target distributions will use this arena size to determine random placements.
-
Set Number of Rovers: Optional. If you select this checkbox, you can customize the exact number of rovers that will be created regardless of the round type. You can create from 0 to 8 rovers.
-
Simulation Length: Optional. By default there is no timer set. If you select a timer from the drop down list, when you click the "All Autonomous" button the Gazebo simulation will run for the specified time before stopping all of the rovers (the equivalent of pressing the "Stop All Rovers" button). Please note that a twenty minute timer will run for twenty minutes of simulated time. That is, the amount of time that the simulation needs to simulate twenty real world minutes of time. You can look at the real time factor in Gazebo to get a feel for this, but it is generally slower than real time.
tldr: A simulation timer for twenty minutes might take anywhere from thirty minutes to sixty minutes to actually complete.
-
Create Savable Gazebo World: Selecting this option will create a Gazebo simulation that does not include rovers, a collection zone, or wall barriers. The point of this is to create a uniform, clustered, or power law distribution (which takes a long time to do) and then save that Gazebo world so that you can load it MUCH more quickly for testing your simulated rovers later on.
Click the "Build Simulation" button when ready.
The Gazebo physics simulator will open.
Click back to the Swarmathon GUI and select the "Sensor Display" tab.
Any active rovers, simulated and/or real, will be displayed in the rover list on the left side. To the right of the rover list is their corresponding connection status. Please note that the number here reflects different things for simulated vs. physical rovers:
- physical rovers: This number represents the wireless link quality and the bitrate of their connection.
- simulated rovers: This number represents their simulation rate as a percentage of real time. For example, 1.0 = moving at 100% real time factor, 0.5 = moving at 50% real time factor, etc.
- all rovers: Green = good connection. Yellow = meh connection. Red = bad connection and/or disconnected.
To the right of the connection status is a checkbox for the map frame. If you select a checkbox for a given rover, it’s map information will appear in the map to the right. You can arbitrarily select any number of rovers to be displayed in the map frame.
Select a rover to view its sensor outputs.
There are four sensor display frames and one settings frame:
The camera output. This is a rover eye’s view of the world.
The ultrasound output is shown as three white rays, one for each ultrasound. The length of the rays indicates the distance to any objects in front of the ultrasound. The distance in meters is displayed in text below these rays. The maximum distance reported by the ultrasounds is 3 meters.
The IMU sensor display consists of a cube where the red face is the bottom of the rover, the blue face is the top of the rover, and the red and blue bars are the front and back of the rover. The cube is viewed from the top down. The cube is positioned according to the IMU orientation data. For example, if the rover flips over, the red side will be closest to the observer. Accelerometer data is shown as a 3D vector projected into 2D space pointing towards the sum of the accelerations in 3D space.
The map view shows the path taken by the currently selected rover. Green is the encoder position data. In simulation, the encoder position data comes from the odometry topic being published by Gazebo’s skid steer controller plugin. In the real robots, it is the encoder output. GPS points are shown as red dots. The EKF is the output of an extended Kalman filter which fuses data from the IMU, GPS, and encoder sensors.
The map settings frame contains several settings that affect the map view:
- Frame Views: You can check whether the map displays data from EKF (MAP), odometry (ODOM), and/or GPS (NAVSAT). Additionally, the Global Frame checkbox moves rovers from all starting at (0,0) to their actual start points (this is for simulated rovers only at this time).
- Panning: You can select whether the map automatically pans to fit all of the map data into the map frame or you can choose to manually pan the map yourself using the mouse to click-and-drag and also zooming with the scroll wheel.
- Unique Rover Colors: Select this option to change the colors in the map frame to display unique rover colors. This will help people to tell the rover paths apart in the map frame after long simulation runs.
- Popout: Click on this box to generate a popout map window that you can maximize or resize however you want. The popout map retains all of the settings you had selected in the original map frame. Changing settings in the RQT GUI will also change things in your popout map.
Look on the left hand side of the RQT Rover GUI.
This section displays important status information:
- (all rovers) The currently selected rover in the RQT GUI.
- (physical rovers) The number of GPS satellites detected.
- (all rovers) The number of obstacle avoidance calls.
- (simulated rovers) The number of targets collected.
- (simulated rovers) The current status of the simulation timer after selecting a timer length and clicking the "All Autonomous" button.
To close the simulation and the GUI, click the red exit button in the top left-hand corner.
Source code can be found in the repository /src directory. This directory contains several subdirectories, each of which contain a single ROS package. Here we present a high-level description of each package.
abridge: A serial interface between SwarmBaseCode-ROS and the A-Star 32U4 microcontroller onboard the physical robot. In the simulation,abridgefunctionality is supplanted bysbridge, which interfaces with gazebo_ros_skid_steer_drive (motor and encoders) and hector_gazebo_plugins (sonar and IMU; see step 3 of the Quick Start guide).rqt_rover_gui: A Qt-based graphical interface for the physical and simulated robots. See How to use Qt Creator for details on this package.mobility: The Swarmie control API, behaviors, helper functions, debug terminal, teleop and various other nodes.mapping: The mapping node uses the Grid Map library to mark sonar obstacles and AprilTag resource locations on a map in the odometry coordinate frame. The node also provides a path-planning navigation service to the Python control code, using A* to find a path to a goal location, returning the path as a list of waypoints avoiding marked sonar obstacles.diagnostics: A node monitoring sensor data, logging messages to therqt_rover_guiso the user knows if any hardware is malfunctioning. This node can also stop the rover from moving if an unrecoverable hardware malfunction prevents the rover from receiving important sensor data.apriltags2_ros: An image processor that detects AprilTag fiducial markers in the onboard camera’s video stream. This package receives images from theusb_camnode (for physical robots) or gazebo_ros_camera (for simulated robots).ublox: An unused dependency ofrqt_rover_gui.
Behavior code is launched by the task manager. The task manager is a state machine that implements the phases of the robot operation during the competition. The task states are shown below:
The task manager operates as a single ROS node, starting each behavior by importing it as a Python module and calling its main() function. Separating tasks into individual modules eliminates interaction between behaviors, reducing programmer error due to unforeseen consequences. Further, each behavior can be tested in isolation. For example, you can place the rover in front of some blocks then manually launch the pick-up node. This experiment can be repeated quickly and enables programmers to write more thoroughly tested code.
initInitializes the rover for a round, saving the collection area location in the odometry coordinate frame.searchImplements a correlated random walk as a basic search method. The behavior also saves resource locations as they are discovered, so the rover can drive directly back to them.pickupPicks up a cube with the rover’s claw.gohomeDrives the rover back to the collection area, planning a path around any obstacles currently on the map.dropoffDrops a cube off inside the collection area.
# deploy code to the rover with hostname lovelace
./misc/deploy_host.sh lovelace
deploy_host.sh builds the code in the current working tree on the local host machine, copies binaries to the rover, and brings up the ROS graph on the rover via SSH.
We use multimaster_fkie to run separate, synchronized ROS graphs on each host machine when running code on multiple physical rovers.
This helps reduce wireless network bandwidth usage by allowing us to sync specific topics across machines. Also, since each rover has its own local ROS graph, with a local ROS Master, it should be more resilient against temporary wireless connectivity dropouts.
mutimaster_fkie requires a master_discovery node running on each machine, and a master_sync node running on at least one machine.
- The
master_discoverynode publishes information about the local ROS Master on the network, making it automatically discoverable. - The
master_syncnode connects to any discoveredmaster_discoverynodes and synchronizes the remote ROS graph with the local one.
To create a mesh of synchronized ROS graphs, we run the master_discovery and master_sync nodes on the laptop and all rovers, so each local ROS Master is connected to every other remote ROS Master. The master_discovery and master_sync nodes are launched automatically alongside the GUI on the laptop and with the rover nodes on each rover.
In the Multimaster system, you can have GUIs open on multiple laptops if you want. Also, closing and re-opening the GUI has no effect on the physical rovers and their local ROS graphs, other than a causing a short burst of network activity serving the sync/unsync procedure amongst the master_discovery/master_sync nodes.
- Each host (laptop and all rovers) must be able to resolve the hostname of all other hosts. This can be done by hard coding names in
/etc/hostsor using a DNS server on the local network. - You cannot rely on mDNS and Avahi’s
.localdomain for name resolution. Neither ROS normaster_discovery/master_synccurrently support this.
As long as names resolve on each host, the master_sync nodes should discover sync with every master_discovery node on the network within 10-15 seconds. On rare occasions, this doesn’t work properly, and not all of the rovers will show up in the GUI display. If this happens, you can:
- Re-launch the GUI and accompanying
master_discovery/master_syncnodes, without re-deploying to any of the rovers. - If the above doesn’t work, re-deploy code to the missing/un-synced rover.
run.sh is a wrapper for a roslaunch command to start the GUI. The launch file has a number of useful arguments you can specify from the command line. Any arguments you give to run.sh will be added as arguments to the roslaunch command it runs.
Example: Start the simulation automatically, with 2 rovers, final round arena dimensions using a specific world file:
./run.sh startsim:=true numrovers:=2 round:=final world:=$(pwd)/simulation/worlds/others/8by8_128_powerlaw_with_obstacles.world
You can view available arguments and their default values using the --ros-args flag. Apologies for the undocumented argument descriptions:
$ ./run.sh --ros-args
Optional Arguments:
gazebo (default "false"): undocumented
multimaster (default "true"): undocumented
numrovers (default "1"): undocumented
round (default "prelim"): undocumented
startsim (default "false"): undocumented
world (default "/home/darren/rover_workspace/simulation/worlds/2018_competition_world_files/prelim2.world"): undocumented
You can control the rover manually two ways:
- Click on a rover’s name in the upper left of the GUI display.
- Use I, J, K, L as arrow keys to move the rover.
Unlike the GUI-enabled control, teleop_keyboard provides keyboard-based claw controls in addition to motion controls.
Start the teleop_keyboard node:
If you’re connected to only one rover:
./dev.sh teleop_keyboard.py
Otherwise, if you’re connected to multiple rovers:
# connect to the rover named achilles
source ./devel/setup.bash
ROS_NAMESPACE=achilles rosrun mobility teleop_keyboard.py
teleop_keyboard has two modes. It defaults to a fully manual mode and you can optionally use it as a wrapper for some of the Swarmie control commands.
$ ROS_NAMESPACE=achilles rosrun mobility teleop_keyboard.py -h
usage: teleop_keyboard.py [-h] [-s]
optional arguments:
-h, --help show this help message and exit
-s, --swarmie use Swarmie API instead to issue driving commands (default:
False)
-
Fully manual mode:
$ ROS_NAMESPACE=achilles rosrun mobility teleop_keyboard.py Welcome achilles to the world of the future. Reading from the keyboard and Publishing to driver! ---------------------------------------------------------- Moving around: Fingers (use the shift key): u i o U O j k l (close) (open) m , . Wrist: Driving Parameters: ----------------- ------------------------------------- t : up 1/2 : -/+ drive speed by 10% g : middle 5/6 : -/+ turn speed by 10% b : down anything else : stop CTRL-C to quit ---------------------------------------------------------- Currently: drive(1/2): 0.20 (m/s) | turn(5/6): 0.90 (rad/s) -
Swarmiecontrol mode:$ ROS_NAMESPACE=achilles rosrun mobility teleop_keyboard.py --swarmie Welcome achilles to the world of the future. Reading from the keyboard and driving using Swarmie API! -------------------------------------------------------- CTRL-C to quit ----------------- ------------------- Moving around: Fingers (use the shift key): i U O j k l (close) (open) , Wrist: Driving Parameters: ----------------- ----------------------------------- t : up 1/2 : -/+ drive speed by 10% g : middle 3/4 : -/+ reverse speed by 10% b : down 5/6 : -/+ turn speed by 10% I/< : -/+ drive distance by 10% J/L : -/+ turn angle by 10% anything else : stop (not implemented) -------------------------------------------------------- Toggle Obstacles to ignore (* = currently ignored): ----------------- (!) PATH_IS_CLEAR = 0* (a) SONAR_LEFT = 1 (s) SONAR_CENTER = 4 (d) SONAR_RIGHT = 2 (f) SONAR_BLOCK = 8 (T) TAG_TARGET = 256 (h) TAG_HOME = 512 (n) INSIDE_HOME = 1024 (C) HOME_CORNER = 2048 (S) IS_SONAR = 15 (V) IS_VISION = 3840 (v) VISION_SAFE = 2816 (H) VISION_HOME = 2560 Currently: drive speed (1/2) : 0.20 (m/s) reverse speed (3/4) : 0.20 (m/s) turn speed (5/6) : 0.90 (rad/s) drive dist (I/<) : 0.50 (m) turn theta (J/L) : 1.57 (rad)
rdb is a ROS node that initializes the Python rover control code and drops into an interactive IPython interpreter. From the shell you can control the rover using any method in the Swarmie control API, and also run all the rover behaviors.
In [1]: swarmie.drive(1)
In [2]: swarmie.turn(math.pi/2)
In [3]: behavior.init.main()
In [4]: behavior.search.main()
In [5]: behavior.pickup.main()
In [6]: behavior.gohome.main(has_block=True)
In [7]: behavior.dropoff.main()
If you’re connected to only one rover:
./rdb.sh
Otherwise, if you’re connected to multiple rovers:
# connect to the rover named achilles
source ./devel/setup.bash
ROS_NAMESPACE=achilles rosrun mobility rdb.py
Each robot behavior is contained in its own Python script. You can run a behavior in two ways.
If you’re connected to only one rover:
./dev.sh {init.py|search.py|pickup.py|gohome.py|dropoff.py}
Otherwise, if you’re connected to multiple rovers:
# connect to the rover named achilles
source ./devel/setup.bash
ROS_NAMESPACE=achilles rosrun mobility {init.py|search.py|pickup.py|gohome.py|dropoff.py}
Start rdb, as shown above in the section describing rdb.
Call a behavior’s main() function:
In [1]: behavior.init.main()
In [2]: behavior.search.main()
In [3]: behavior.pickup.main()
In [4]: behavior.gohome.main(has_block=True)
In [5]: behavior.dropoff.main()
The rover maps can be viewed using RViz and tuned at runtime using rqt_reconfigure. You can open both programs with a launch file:
# connect to the rover named achilles
source ./devel/setup.bach
roslaunch swarmie grid_map_visualization.launch name:=achilles
By default, RViz is configured to display:
- The search frontier.
- This visualizes the cells A*’s frontier expanded through during the most recent path search.
- In Rviz, this layer appears as a translucent purple overlay.
- Note: You have to set
visualize_frontierto"true"by checking the box in therqt_reconfigurewindow in order for themappingnode to publish frontier data.
- Sonar obstacles.
- These are marked and cleared from the map using each sonar range measurement as its received.
- Clear areas of this layer appear as a light grey; marked areas range from blue to purple.
- Targets (AprilTag cubes, ID 0)
- Target tag detections are marked and cleared from the map using the rover’s approximate visual field of view.
- Clear areas of this layer appear as a dark grey; marked areas appear as a translucent purple.
- Home (AprilTag home tags, ID 256)
- Home tag detections are marked and cleared from the map using the rover’s approximate visual field of view.
- Clear areas of this layer appear as a dark grey; marked areas appear as a translucent purple.
These paths are lists of waypoints, displayed as a bright green line in RViz.
Screenshot of RViz occupancy grid display. The visualization shows the search frontier, sonar, target tag, and home tag map layers. The most recent planned path from the rover’s current location (red arrow) to home (multicolor axis) is displayed as a bright green line.
Screenshot of rqt_reconfigure client configured to adjust mapping parameters
For debugging, the mapping node subscribes to a goal topic. When it receives a PoseStamped on this topic it will search for a path from the rover’s current location to the goal location. It will publish the path, but the rover won’t move.
You can publish on the goal topic to trigger a path search by clicking on the "2D Nav Goal" button in the RViz menu bar and then clicking a point in the map.
The Python code that interfaces with the mapping/get_plan service is in planner.py. When a rover is driving autonomously, the Planner module is imported and called from inside the behavior code. However, you can also run planner.py as a standalone node for testing.
If you’re connected to only one rover:
./dev.sh planner.py
Otherwise, if you’re connected to multiple rovers:
# connect to the rover named achilles
source ./devel/setup.bash
ROS_NAMESPACE=achilles rosrun mobility planner.py
ROS_NAMESPACE=achilles rosrun mobility planner.py -h
usage: planner.py [-h] [--avoid-home | --avoid-targets]
optional arguments:
-h, --help show this help message and exit
--avoid-home whether the rover should avoid home during this run
(default: False)
--avoid-targets whether the rover should avoid targets during this run
(default: False)
When the planner node starts, it subscribes to the goal topic. When it receives a PoseStamped on this topic it will call the mapping/get_plan service to get a list of waypoints and then attempt to drive the rover to the goal location, avoiding sonar obstacles and optionally home tags or target tags.
If the planner node is running, when you use RViz’s "2D Nav Goal" button to click a point in the map, the rover will attempt to drive to that point.