search

BCLab-UNM / SwarmBaseCode-ROS

The base code provided to teams participating in NASA Swarmathon IV.
http://nasaswarmathon.com
MIT License
46 stars 46 forks source link

readme

SwarmBaseCode-ROS

This repository is a ROS (Robot Operating System) controller framework for the Swarmie robots used in the NASA Swarmathon, a national swarm robotics competition.

This repository contains:

  1. Source code for ROS libraries (i.e. packages) that control different aspects of the Swarmie robot, including robot behaviours such as search strategies and obstacle avoidance, diagnostics, and user interface elements.
  2. 3D .STL models for the physical Swarmie build
  3. Bash shell scripts for initializing simulated Swarmies in the Gazebo simulator, as well as physical Swarmies

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.

Alt text

Quick Start Installation Guide

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.

1. Install ROS Kinetic Kame

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.

You may request the installation of addition packages on the competition rovers. To do so create a pull request modifying the preinstalled packages list document.

Most systems will already have usb development support installed, but just in case:

 sudo apt install libusb-dev
2. Install additional ROS packages

We use the catkin_tools package to build the SwarmBaseCode-ROS code base:

sudo apt install python-rosinstall python-catkin-tools

Our simulated and physical Swarmies use existing ROS plugins, external to this repo, to facilitate non-linear state estimation through sensor fusion and frame transforms. These plugins are contained in the robot_localization package, which should be installed using the apt-get package management tool:

sudo apt install ros-kinetic-robot-localization

The ROS USB camera package:

sudo apt install ros-kinetic-usb-cam

If you intend to use the rosbridge tools to run robots independently of the laptop you will need to install rosbridge:

sudo apt install ros-kinetic-rosbridge-server
3. Install additional Gazebo plugins

Our simulated Swarmies use existing Gazebo plugins, external to this repo, to replicate sonar, IMU, and GPS sensors. These plugins are contained in the hector_gazebo_plugins package, which should be installed using the apt-get package management tool:

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, should also be installed using the apt tool:

 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.

4. Install git (if git is already installed, skip to step 5):
sudo apt install git
5. Install SwarmBaseCode-ROS

1a. To update your existing repository using the base code (your competition repository name will have been provided to you):

cd YourRepositoryName
git remote add SwarmBaseCode https://github.com/BCLab-UNM/SwarmBaseCode-ROS
git pull SwarmBaseCode

OR

If you just want a clean copy of the base code then:

1b. Clone this GitHub repository to your home directory (~), renaming the repo so ROS and catkin can properly identify it (you can name the target directory whatever you like):

  cd ~
  git clone https://github.com/BCLab-UNM/SwarmBaseCode-ROS.git SwarmBaseCode-ROS
  cd SwarmBaseCode-ROS

  1. Set up ublox GPS submodule and April Tag library:

    git submodule init
git submodule update

  2. Compile SwarmBaseCode-ROS as a ROS catkin workspace:

    Make sure bash is aware of the location of the ROS environment:

    if ! grep -q "source /opt/ros/kinetic/setup.bash" ~/.bashrc
then 
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
fi
source ~/.bashrc

    Tell catkin to build the Swarmathon code:

    catkin build
6. Run the SwarmBaseCode-ROS simulation:

  1. Change the permissions on the simulation run script to make it exectuatable (assuming you use the target directory name SwarmBaseCode-ROS):

    cd ~/SwarmBaseCode-ROS
chmod +x ./run.sh

  2. Start the simulation

    ./run.sh

The GUI will now launch. The run script kills a number of gazebo and ROS processes. Killing these processes is suggested by gazebosim.com as the best way to clean up the gazebo environment at the moment.

This is the first screen of the GUI:

Alt text

Click the "Simulation Control" tab:

Alt text

There are several settings you can change:

Click the "Build Simulation" button when ready.

The Gazebo physics simulator will open.

Alt text

Click back to the Swarmathon GUI and select the "Sensor Display" tab.

Alt text

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:

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.

Alt text

Select a rover to view its sensor outputs.

Alt text

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:

Look on the left hand side of the RQT Rover GUI.

Alt text

This section displays important status information:

  1. (all rovers) The currently selected rover in the RQT GUI.
  2. (physical rovers) The number of GPS satellites detected.
  3. (all rovers) The number of obstacle avoidance calls.
  4. (simulated rovers) The number of targets collected.
  5. (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.

Software Documentation

Source code for SwarmBaseCode-ROS can be found in the repository /src directory. This diretory contains severals subdirectories, each of which contain a single ROS package. Here we present a high-level description of each package.

How to use Qt Creator to edit the GUI

Steps that are crossed out below show how the procedure has changed for QT Creator 3.5.1 which is installed with QT 5.

  1. Install Qt Creator:

    sudo apt-get install qtcreator

  2. Build the workspace:

    catkin build

  3. Run Qt Creator:

    qtcreator &

  4. Choose "Open File or Project" from the File menu

  5. Navigate to ~/SwarmBaseCode-ROS/src/rqt_rover_gui/

Replace ~/SwarmBaseCode-ROS with the path to your git repository.

  1. Select CMakeLists.txt.

  2. Click "Open" to continue.

  3. Enter path to your home directory/SwarmBaseCode-ROS/build in the text box, this is the default build path. You cannot use the ~ as a shorthand to your home directory here.

9. Click Configure Project.

10. Click on the Projects icon on the left toolbar.

  1. QTCreator will prompt you to enter cmake arguments.

  2. Enter -DCMAKE_INSTALL_PREFIX=../install -DCATKIN_DEVEL_PREFIX=../devel in the CMake arguments text box.

  3. Press the run cmake button.

  4. Press the "Finish" button.

11. Click the "Edit" toolbox icon on the left.

12. Double-click CMakeLists.txt.

  1. Click the "Build Now" button to build the project.

Qt Creator can now be used to build your git repository.

Note: start qtcreator in your terminal with your git repository as the current directory. Source the ~/.bashrc if the catkin environment variables are not set so that QT Creator can properly build the project.

Debugging with GDB and Qt Creator

Debuggers are particularly useful for tracking down segfaults and for tracing through the logic of your programs. In order to use the GNU debugger (GDB) with the swarmathon competition add the following line to the CMakelists.txt file for the project you want to debug.

set(CMAKE_BUILD_TYPE Debug)

This will compile your code with debugging symbols enabled.

Since ROS is multithreaded you may need to attach the debugger to threads that have been spawned by your program. To enable this enter the following in a terminal:

sudo apt-get install libcap2-bin

sudo setcap cap_sys_ptrace=eip /usr/bin/gdb

To use QT Creator to debug your already running program click the "Debug" menu. Choose "Start Debugging" and then "Attach to Running Application...". You will be able to use the graphical interface to GDB from here.

Using the deploy.sh script

The "deploy.sh" script has been added to allow for rapid and easy development for physical teams and judges alike! This script is found in the SwarmBaseCode-ROS/misc folder and automates several key tasks for the user! Now connecting to swarmies takes seconds.

The script is run from your workstation and not the swarmies themselves. Keeping work on the workstation has many benefits with one being a fast and reliable way to transfer and run code. You will need to ensure the GUI is running before running this script!

Before running the code, navigate to the misc folder:

cd ~/SwarmBaseCode-ROS/misc

Give permission to the script to be executable:

chmod +x deploy.sh

You are now set for rapid deployment and development!

"deploy.sh" has 3 built-in options to be used:

./deploy.sh -R

./deploy.sh -L

deploy.sh -G {branch} (where branch is the desired branch you wish to pull)

If you want to use the script, and don't want to use the interface, you are able to do so. Running any option (-R, -L, -G) followed by a -S will trigger a silent mode for the command, running your commands and returning to the terminal that the script was called from immediately after all operations are performed.

EX: ./deploy.sh -R -S robot1 will attempt to connect and run robot1 and then return to the current terminal without opening the interface.

Feature:

Typing "REBOOT {hostname}" in any option will allow you to reboot the selected rover.

Running multiple commands at once is allowed. So typing in a line such as "rover1 rover2 REBOOT rover3" will work.

NOTES FOR DEPLOY.SH SCRIPT: Applying Keys

This script runs better when using ssh-keys. Keys allow you to SSH without requiring the user to type in a password every time.

If unfamiliar or have not setup an SSH-Key on a current machine, users can type: ssh-keygen and follow the prompt

Once the key has been setup, copy the key from the users machine to each rover you wish to add it to with ssh-copy-id swarmie@{hostname} where hostname is the rover's hostname

That's it! You should now have a seamless way to SSH without having to type in passwords each time!

Notes for using rosbridge

The misc/start_robots.sh script now accepts a --bridged option that will run each robot independently and connect them to the GUI using rosbridge. For example:

$ ./start_robots.sh --bridged --prefix path/to/directory/containing/code/on/robot <calibration file> Swarmathon-UNM <list robot names>

You can also start a single robot manually using misc/rover_launch_local.sh <calibration file> script. Once the robot is running and the GUI is running use misc/start_bridge.sh <robot-name> to connect the robot to the GUI via rosbridge.

A firefox tab will open for each robot (if there is an error connecting to the robot it will appear in the browser console). If the robot does not appear in the GUI try reloading the robot interface page in the browser.

Behaviours

This section provides an overview of the behaviours package. We describe the overall architecture of the package and a few pitfalls you may encounter when writing your own behaviours. The behaviours package is designed to separate the logic of individual behaviours from the details of implementing them on ROS (ie. the messages that are actually published). This allows for the possibility of easily porting the behavior to a different system without significantly rewriting the code. The interface between the abstract behaviours and ROS is handled by ROSAdapter which is responsible for sending and receiving all ROS messages.

The architecture of the behaviours package is hierarchical, with the ROSAdapter at the top of the hierarchy. The ROSAdapter interacts with the LogicController which in turn manages all the individual controllers that implement specific behaviours.

Controller

An abstract class that all controllers implement. The swarmies are structured as a collection of low level controllers that each perform a simple task such as deciding where to go next when searching (SearchController), or determining whether to turn left or right to avoid an obstacle (ObstacleController). These low-level behaviours, implemented as simple controllers are combined by the logic controller into the high-level foraging behavior of the swarmies. To add a new behavior to the swarmies you would write a new controller and integrate it in to the logic controller through the priority system described below.

Controller API

ROSAdapter

The main role of ROSAdapter is to manage sensor input, passing relevant data coming from ROS messages to the logic controller (which in turn may pass that data on to individual controllers). ROSAdapter calls LogicController::DoWork() ten times per second and takes action based on the Result returned by that call. Taking action typically means sending a message that will cause the wheels or gripper to move. The easiest way to trigger some other action is to have the ROSAdapter poll the logic controller at regular intervals to check whether some event has occurred (a good example of this is checking whether a manual waypoint has been reached).

Logic Controller

The logic controller manages all the other controllers in the behaviours package. There are two finite state machines at its core, one for the logic state and one for the process state

The logic state state-machine has three states

The interrupt state is entered whenever one of the controllers has signaled that it has work to do. In the interrupt state the logic controller polls all controllers to determine which ones have work and calls DoWork() on the controller with highest priority (priority is determined by the current process state of the logic controller, discussed below). The next logic state is determined by the result returned by that controller. The other two states are relatively simple. The waiting is used when waiting for the drive controller to reach its last waypoint (note that the drive controller is not part of the priority system described above, rather it is called directly whenever the logic state is waiting). The precision state is used when a controller wants to take direct control of the robot's actuators. In this state the result from the highest priority controller is passed directly to the drive controller do be acted on. This allows for very high precision driving to perform tasks such as aligning with a cube when picking it up.

The process state state-machine determines the priorities of the controllers. There are four states:

The states searching, target_pickup, and drop_off are entered when searching for cubes, picking up cubes, and dropping off cubes respectively. Under each state the priority of the controllers changes. Check LogicController::ProcessData() to see the priorities (higher is higher priority, -1 is disabled). The manual state is a special state that is unreachable while the robot is in autonomous mode. The only active controller in the manual state is the manual waypoint controller.

One more important function in the logic controller is LogicController::controllerInterconnect() which can be used to share data between controllers.

The remaining functions on LogicController are use to pass data from the ROSAdapter to the relevant controllers.

List of Controllers

PID

The PID class implements a generic proportional-integral-derivative controller that is configured using the PIDConfig struct. This struct is used to set the gains and the anti-windup parameters of the PID. The PID parameters can be tuned by modifying the config structs in the drive controller.