# ROS setup
This file explains how to install and set up ROS  (Robot Operating System) such that it can be used to detect the position of the robot via camera.

## Installing Software

This installation process has been tested with Ubuntu 16.04 LTS.

### ROS installation
The first step is to install ROS on your computer. I used the version ROS Kinetic, but other versions of ROS should also work, as long as the packages we use are available or you compile them yourself.

Follow the instructions [here](http://wiki.ros.org/kinetic/Installation) and choose the version `ros-kinetic-desktop-full`.

### Installing required ROS packages
We use the following packages for image detection:

- ros-kinetic-aruco-detect
- ros-kinetic-fiducial-slam
- ros-kinetic-cv-camera

Install each of them using 
```
$ sudo apt-get install <package-name>
```

## Setup

For control experiments we need to know the position of the robots on the ground and its orientation. To achieve this we place a camera on the ceiling and use image detection relying on ArUco markers.

### Camera setup
For the image detection to work correctly we need to calibrate the camera first. 

#### Testing the camera
First, plug in your camera and test if it works with the `cv_camera` package:

- Start ROS:
```
$ roscore
```
- Set the device id for your camera. Typically this will be `0` or `1`
```
$ rosparam set cv_camera/device_id 0
```
- Run the camera node
```
$ rosrun cv_camera cv_camera_node
```
- Test if you can see the live image from the camera using the command
```
$ rosrun image_view image_view image:=/cv_camera/image_raw
```
#### Calibrating the camera
Next, in order to calibrate the camera follow the instructions at [http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration](http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration). 

These instructions mainly consist of the following steps:

- Print a [checkerboard image](https://imaginaerraum.de/git/Telos4/RoboRally/src/branch/master/docs/images/check-108.pdf) on a sheet of paper and measure the size of the squares with a ruler.
- Run the command
```
$ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.108 image:=/cv_camera/image_raw camera:=/cv_camera
```
*Note: Make sure to replace the dimensions of the checkerboard squares with the value you measured in the previous step (in meters)!*

- Move the checkerboard in front of the camera until you have gathered sufficient data and the click on calibrate
- After a successful calibration click *'SAVE'* to store the camera calibration file which ends in `.yaml`
- The calibration data gets written to `/tmp/calibrationdata.tar.gz`. Copy it from there to `~/.ros/camera_info` (create the folder if it does not exists). Then extract it and move and rename the file `ost.yaml` located in `calibrationdata/` to  `~/.ros/camera_info/camera.yaml`. Afterwards, you can delete the `calibrationdata` folder and the file `calibrationdata.tar.gz`.

Now, when you restart the `cv_camera` node it also publishes the intrinsic camera parameters as the topic `/cv_camera/camera_info` which can be used to rectify the image.

*Remark: When you restart the camera node you may receive a warning that the camera name does not match the name in the calibration file. This can be safely ignored, but if you want to get rid of the warning just edit the file `camera.yaml` and replace `narrow_stereo` with `camera`.*


### Preparing robots for image detection
In the next step, we place markers on the robots such that the can be detected by the camera. For this we use the [fiducials](http://wiki.ros.org/fiducials) package:

- Generate some ArUco markers for your robots using this [ArUco marker generator](http://chev.me/arucogen/). Choose Original ArUco as the dictionary. You may have to experiment with the marker size a bit depending on the resolution of your camera and the distance to the robots. I used markers of size `100 mm`.
- Print ArUco markers on paper. Measure the marker size and make sure that it fits the size you specified.

![Printed marker](https://imaginaerraum.de/git/Telos4/RoboRally/raw/branch/master/docs/images/markers_1.JPG)
- Cut the markers and glue them on cardboard. Make sure to leave a white border around the marker. It's also a good idea to label the marker with the id.

![Marker on cardboard](https://imaginaerraum.de/git/Telos4/RoboRally/raw/branch/master/docs/images/markers_2.JPG)
- Fix the markers on the robot. First, add a new layer to the robot using screws and then fix the marker to the layer using e.g. double sided tape or velcro tape.

![Marker on robot bottom view](https://imaginaerraum.de/git/Telos4/RoboRally/raw/branch/master/docs/images/markers_3.JPG)
![Marker on robot top view](https://imaginaerraum.de/git/Telos4/RoboRally/raw/branch/master/docs/images/markers_4.JPG)

### Install position detection node
The image detection computes the position and orientation of the marker that was placed on the robot in relation to the camera position.
In order to get from this the position of the robot in the 2D plane (the floor) we need to transform it first. 
In this step we will build and install a ROS node that will do exactly this for us.
The node projects the position of each detected marker on the 2D plane and publishes its ID, position and angle of rotation in the plane as a ROS messages. 

To install the node do the following:

- Change in the directory `fiducial_transform/`in the root folder of the repository:
```
$ cd fiducial_transform
```
- Build the package:
```
$ catkin_make
```
- Source that package so you can use it:
```
$ source devel/setup.bash
```
*Note: In order to avoid doing this every time you want to start the marker detection you can add the following line to your `~/.bashrc` file:*
```
$ source $ROBO_RALLY_GIT/fiducial_transform/devel/setup.bash
```
*where `$ROBO_RALLY_GIT` is the location of this repository.*

## Testing
Now it's time to test if the position detection is working correctly.

### Running the position detection
Generally, the following steps are necessary to run the position detection for the robots:

- Start roscore:
```
$ roscore
```

- Plug in the camera and start the camera node:
```
$ rosparam set cv_camera/device_id 0 # make sure to set the correct device id for your camera!
$ rosrun cv_camera cv_camera_node
```

- Optional: Check if the camera works correctly;
```
$ rosrun image_view image_view image:=/cv_camera/image_raw
```
You should see a window displaying a live view of the camera image. 

![image_view output](https://imaginaerraum.de/git/Telos4/RoboRally/raw/branch/master/docs/images/image_view_2.jpg)

- Start ArUco marker detection:
```
$ roslaunch aruco_detect aruco_detect.launch camera:=cv_camera image:=image_raw dictionary:=16 transport:= fiducial_len:=0.1
```
*Note: Make sure you use the correct camera, image, dictionary and marker length in this step!*

- Optional: Check if marker detection works correctly:
```
$ rosrun image_view image_view image:=/fiducial_images
```
Place a marker in the field of view of the camera and check if it gets correctly detected by the image detection, i.e. there is a border around the square of the marker and arrows indicating its orientation. 

![fiducial_images output](https://imaginaerraum.de/git/Telos4/RoboRally/raw/branch/master/docs/images/image_view_3.jpg)
![fiducial_images output](https://imaginaerraum.de/git/Telos4/RoboRally/raw/branch/master/docs/images/image_view_4.jpg)
If the marker is not detected correctly, please check that you are using the correct dictionary, that you camera was calibrated and that you are launching the `aruco_detect` node with the correct parameters

- Run marker transformation node
```
$ rosrun fiducial_transform fiducial_transform_script
```

Now when you run
```
$ rostopic echo /marker_id_pos_angle
```
and you place a marker in the field of view of the camera you should see the markers id, position and orientation as output on the console.

![fiducial_transform node output](https://imaginaerraum.de/git/Telos4/RoboRally/raw/branch/master/docs/images/image_view_5.jpg)
You can also access this data using a ROS listener.


## Wrapping up
Now you're all set up and ready to start your own control experiments with the robots. 
You could for example implement a simple PID controller to let the robot perform turns or drive from one position to another.
For some inspiration take a look at the demo programs `MPC_position_controller.py` or `racing_game.py`.