/**

\page tutorial-mbot-vs Tutorial: Visual-servoing with mBot Ranger educational robot kit
\tableofcontents

\section mbot_intro Introduction

This tutorial explains how to do a position-based visual-servoing with the mBot Ranger educational robot kit equipped with a Raspberry Pi 3 board connected to a camera.

\image html img-mbot-robot.jpg

The following material is requested:
- mBot Ranger educational Kit and especially the pre-set <a href="http://www.makeblock.com/steam-kits/mbot-ranger/">Dashing Raptor</a> form that has a [Me Auriga](http://store.makeblock.com/maker-kits-STEM/me-auriga) board
- Makeblock <a href="https://makeblockshop.eu/products/me-shield-for-raspberry-pi">Me Shield</a> for Raspberry Pi
- 1 RJ25 cable between the Me Shield and the Me Auriga port 5
- we recommend the usage of <a href="https://www.raspberrypi.org/products/raspberry-pi-3-model-b/">Raspebby Pi 3</a> Model B or Model B+ even if the tutorial is also compatible with a Raspberry Pi 2.
- Raspberry Pi <a href="https://www.raspberrypi.org/products/camera-module-v2/">camera module V2</a>
- an AprilTag that will serve as target for the visual servoing. \ref franka_prereq_target.

The communication between Raspberry Pi and mBot Auriga board is achieved via a [serial link](http://learn.makeblock.com/me-shield-for-raspberry-pi/). Images acquired on the Raspberry Pi are processed in order to detect the AprilTag pose. From this pose a visual-servoing computes the velocities that have to be send to the Me Auriga. On the Me Auriga there is an infinite loop that is waiting for velocities that have to be applied to the Dashing Raptor wheels. A watch dog ensure the mobile robot stops when nothing is sent throw the serial link after 1 second.

The following video shows the result of this tutorial; the mBot Ranger follows the AprilTag target in order to maintain a given distance between the tag and the camera.
\htmlonly
<p align="center"><iframe width="560" height="315" src="https://www.youtube.com/embed/n50CFukmEYs" frameborder="0" allow="autoplay; encrypted-media" allowfullscreen></iframe></p>
\endhtmlonly

\note Before continuing we recommend that you succeed to follow \ref tutorial-install-raspberry and \ref tutorial-detection-apriltag.

\section mbot_mounting Build the robot

The following image shows the material that we use to build the robot:
\image html img-mbot-material.jpg

First you need to follow these [instructions](http://download.makeblock.com/ranger/resource/mBot-Ranger_Blue_STD_EN_D2.1.3_7.40.3703_Edit-1.pdf) to mount the Dashing Raptor.

<b>Mount the camera on the plexiglass support</b>

As shown in the next image, we build a plexiglass support with external dimensions 26 mm by 85 mm
\image html img-mbot-material-camera.jpg
Fix the camera on its support
\image html img-mbot-camera-support.jpg

<b>Mount the Raspberry Pi</b>
We use a plexiglass support with dimensions 60 mm by 90 mm to fix the Raspberry Pi on the Dashing Raptor.

Mount the plexiglass support
\image html img-mbot-rpi-support.jpg
Mount the Raspberry Pi
\image html img-mbot-rpi-mounted.jpg
Plug the Me Shield
\image html img-mbot-rpi-shield-mounted.jpg
Plug the camera on the Raspberry Pi and screw the camera support on the robot
\image html img-mbot-camera-mounted.jpg
Mount the Me Auriga over the Raspberry Pi
\image html img-mbot-auriga-mounted.jpg
Plug the cable between Me Shield and Me Auriga port 5
\image html img-mbot-cable-mounted.jpg

\section mbot_ranger mBot Ranger setup

\subsection mbot_ranger_mblock_install Install mBlock

mBlock is a graphical programming software which is designed based on Scratch and compatible with Arduino boards such
as Makeblock board like the Me Auriga. Visit the following URL for more details:
http://learn.makeblock.com/getting-started-programming-with-mblock-2/. We suggest also to follow the getting started
courses http://learn.makeblock.com/ranger-online-course/ if you are not familiar with mBot and mBlock.

These are the steps to work on Windows, with mBlock:

- Install mblock 3: (latest 3.4.11) http://www.mblock.cc/software/mblock/
- Launch mBlock from the Windows `Start menu > mBlock > mBlock`
- Check which is the serial port that is already visible (in our case Menu `Connect > Serial Port` shows COM3)
- Plug the USB cable between the computer and the mBot Ranger Auriga board. You should hear a beep when connexion is
  established
- After few seconds, a new serial port should appear in `Connect > Serial Port` menu (in our case COM4)
- Connect to Auriga: `Connect > Serial Port > COM4`. You should hear a new beep

\subsection mbot_ranger_arduino Install Arduino library

Install Arduino Library for Makeblock Electronic Modules as described in
https://github.com/Makeblock-official/Makeblock-Libraries.

To resume:

    C:> cd C:\temp
    C:> git clone https://github.com/Makeblock-official/Makeblock-Libraries

Then copy `C:\temp\Makeblock-Libraries\makeblock` into `C:\Program Files (x86)\mBlock\Arduino\libraries`. This
copy requires administration rights.

\subsection mbot_ranger_mblock_test Test mBlock

In ViSP we provide in `tutorial/robot/mbot/mblock/test-ring-led.sb2` an mBlock test that is be useful to check if the
ME Auriga board is working as expected. This file can also be downloaded from
https://github.com/lagadic/visp/tree/master/tutorial/robot/mbot/mblock.

To achieve the test:

- In mBlock, enter menu `File > Load Project` to load `test-ring-led.sb2`.
\image html img-mbot-mblock-led.png
- Upload this test to the Me Auriga board entering menu `Connect > Upgrade Firmware`. When the upload is finished you
should hear a beep
- In mBlock click on the green flag.
- You should see the Me Auriga ring led successively red, then green, then blue for 1 sec.

\subsection mbot_ranger_arduino_controller Upload the controller

We provide `mbot-serial-controller.ino` file that contains our controller written in Arduino. It has to be compiled
and uploaded on Me Auriga board. This file is part of ViSP and located in
`tutorial/robot/mbot/arduino/mbot-serial-controller`.

Get the source code in `C:\visp-mblock` using:

    C:/> mkdir C:\visp-mblock
    C:/> cd C:\visp-mblock

and donwload the source code from https://github.com/lagadic/visp/tree/master/tutorial/robot/mbot/arduino.

The controller implemented in `mbot-serial-controller.ino` enables the serial link at 115200 baud and also the motors.
In the `loop()` when a serial data is available with the `MOTOR_RPM` keyword it applies the corresponding motor
velocity to the left and right wheels, and with the `LED_RING` keyword it turn the ring led on with the corresponding
color or off.

To compile and upload this controller to the Me Auriga board:

- Enter menu `Edit > Arduino mode` to open the Arduino panel
\image html img-mbot-mblock-arduino-panel.png
- In this panel, click on `Edit with Arduino IDE`. It will open the Arduino IDE.
- In this IDE, enter menu `File > Open` to open `mbot-serial-controller.ino`
\image html img-mbot-arduino-controller.png
- Before uploading you need to select the correct board from the `Tool > Board` menu that should be set to
`Arduino/Genuino Mega or Mega 2560`, and also the correct port from the `Tool > Port` menu; in our case `COM4`.
- In Arduino IDE, upload the Arduino code to the Me Auriga board entering menu `Sketch > Upload`.
- Unplug the USB connexion between the laptop and the Me Auriga

Since in this tutorial we want to control the motors, you need to connect an external power supply using for example
six 1.5 Volt batteries.

- Plug the external power supply to the Me Auriga board, and press the button located on the led ring. This action
allows to turn Me Auriga on.

\section mbot_rpi_install Raspberry Pi setup

In this section we will see how to configure the serial port on Raspberry Pi in order to use the serial port
`/dev/ttyAMA0` to communicate. Depending on your Raspebrry Pi version this is slightly different.

\subsection mbot_rpi_install_serial_pi3 Configure RPi 3 serial port

This section is inspired from this
[tutorial](https://spellfoundry.com/2016/05/29/configuring-gpio-serial-port-raspbian-jessie-including-pi-3/).
The following instructions resume what has to be done.

The GPIO serial port is disabled by default. In order to enable it, edit `/boot/config.txt` and add the line
`enable_uart=1` at the bottom:

    $ sudo nano /boot/config.txt
    enable_uart=1

Disabling the Console:

    $ sudo systemctl stop serial-getty@ttyS0.service
    $ sudo systemctl disable serial-getty@ttyS0.service

You also need to remove the console from the `/boot/cmdline.txt`. If you edit this with:

    $ sudo nano /boot/cmdline.txt

You will see something like:

    dwc_otg.lpm_enable=0 console=serial0,115200 console=tty1 root=/dev/mmcblk0p7 rootfstype=ext4 elevator=deadline fsck.repair=yes rootwait

Remove `console=serial0,115200`, save and reboot for changes to take effect.

Swapping the serial ports on Raspberry Pi 3

    $ ll /dev/serial*
    lrwxrwxrwx 1 root root 5 Mar 10 13:30 /dev/serial0 -> ttyS0
    lrwxrwxrwx 1 root root 7 Mar 10 13:30 /dev/serial1 -> ttyAMA0

To swap the serial ports `ttyS0` and `ttyAMA0` add the following line to the /boot/config.txt

    $ sudo nano /boot/config.txt

and add:

    dtoverlay=pi3-miniuart-bt

Save and reboot for changes to take effect.

You can check that it has worked by:

    $ ls -l /dev/serial*

and you’ll see something like this:

    lrwxrwxrwx 1 root root 7 Mar 10 22:00 /dev/serial0 -> ttyAMA0
    lrwxrwxrwx 1 root root 5 Mar 10 22:00 /dev/serial1 -> ttyS0

\subsection mbot_rpi_install_serial_pi2 Configure RPi 1 or 2 serial port

We provide hereafter the instructions to configure the serial port on Raspberry Pi 1 or 2 if you don't have a Pi 3
under the hand.

To be able to use the serial port to connect and talk to other devices (e.g. in our case the
[Me Auriga](https://multimedia.bbycastatic.ca/multimedia/products/500x500/123/12309/12309022.jpg)), the serial
port console login needs to be disabled.

If this is the case, you should see something similar to:

    $ ssh -Y pi@raspberry
    $ dmesg | grep ttyAMA0
    [    0.509769] console [ttyAMA0] enabled

As described in this [tutorial](http://www.instructables.com/id/Read-and-write-from-serial-port-with-Raspberry-Pi/)
to disable the console on `ttyAMA0` do the following:

    $ sudo raspi-config
    7 Advanced Options > A8 Serial > Would you like a login shell to be accessible over serial > No
    Serial is now disable > Ok

Output is now as expected:

    $ dmesg | grep ttyAMA0
    [    0.135617] 20201000.uart: ttyAMA0 at MMIO 0x20201000 (irq = 83, base_baud = 0) is a PL011 rev2


\subsection mbot_rpi_config Configure RPi camera

First of all, with the Pi switched off, you’ll need to connect the Camera Module to the Raspberry Pi’s camera port,
then start up the Pi and ensure the software is enabled.

This could be achieved following this
[tutorial](https://projects.raspberrypi.org/en/projects/getting-started-with-picamera/4) or this
[tutorial](http://visp-doc.inria.fr/doxygen/visp-daily/tutorial-install-raspberry.html#install_raspberry_camera).

\section mbot_test Testing the setup

\subsection mbot_rpi_test_serial Test serial port on RPi

Here we use our Raspberry Pi 3 connected to an external power supply, a RS232/TTL 3-5,5V adapter and a home made
adapter that is used to connect the RS232/TTL 3-5,5V adapter to the Raspberry Pi
[GPIO connector](http://pi4j.com/images/j8header-3b.png). The home made adapter connects Raspberry PI GPIO pins 6 (GND),
8 (TxD UART), 10 (RxD UART) to a RS232 DB9 female connector.

\warning Don't forget here to unplug the RJ25 cable between the Me Shield and the Me Auriga board.

The following image shows the Me Shield pinout
\image html img-mbot-shield-wiring.jpg
while the next image shows the usage:
\image html img-mbot-serial-adapter.jpg

The following table shows the wiring pinout:

~~~
RPi GPIO                  | Me Shield | DB9 female  | Color
------------------------------------------------------------
pin  6 GND                | pin 2     | pin 5       | black
pin  8 GPIO 15 (TxD UART) | pin 8     | pin 2 (RxD) | yellow
pin 10 GPIO 16 (RxD UART) | pin 10    | pin 3 (TxD) | orange
~~~

Now we can use two python files, `test-serial-write.py` which writes something on the `/dev/ttyAMA0` port and the
`test-serial-read.py` that reads on a laptop running Ubuntu 16.04 on `/dev/ttyUSB0` port.

These files can be found in https://github.com/lagadic/visp/tree/master/tutorial/robot/mbot/raspberry/python.

\note If you build ViSP from source as explained in \ref tutorial-install-raspberry the corresponding python files are
available in ViSP source code in `/home/pi/visp-ws/visp/robot/mbot/raspberry/python`.

After plugging the ME Shield to the Raspberry Pi and connecting the home made adapter to yo your laptop, you can test
if you succeed to configure Raspberry Pi serial port, running on your laptop:

    $ sudo chmod a+rw /dev/ttyUSB0
    $ python test-serial-read.py
    not blocked
    not blocked
    not blocked
    Got 5: data: <non alpha characters>
    not blocked

and on the Raspberry Pi

    $ python test-serial-write.py

After running `test-serial-write.py` you should see lines like `Got 5: data: <non alpha characters>` on the laptop side.

\subsection mbot_rpi_test_tag_detection Testing Apriltag detection

To test if you are able to acquire images from your RPi camera and detect your Apriltag, you may run
tutorial-apriltag-detector-live.cpp. If you build ViSP from source as explained in \ref tutorial-install-raspberry the
corresponding binary is available in `/home/pi/visp-ws/visp-build/tutorial/detection/tag`.

To run this test on the Raspberry Pi:

    $ cd /home/pi/visp-ws/visp-build/tutorial/detection/tag
    $ ./tutorial-apriltag-detector-live


\subsection mbot_test_motor Testing mBot Ranger control

We suppose here that you have \ref mbot_ranger_arduino_controller. The communication between the uploaded mBot
controller and Raspberry Pi is achieved by a serial link using RPi `/dev/ttyAMA0` port configured with the
following parameters: 115200 baud, 8 bits, parity none, 1 stop bit and no control flow.

We implement a basic communication protocol with the following keywords `MOTOR_RPM` and `LED_RING`. Each keyword
has a set of parameters with comma as separator. The R,G,B color level is in range 0-255.
~~~
Keyword   | Parameters                                    | Example
------------------------------------------------------------------------------
MOTOR_RPM | left wheel RPM, right wheel RPM               | MOTOR_RPM=-100,100
LED_RING  | led index, red level, green level, blue Level | LED_RING=0,0,10,0
~~~

The following sample code shows how to send instructions from Raspberry Pi to mBot controller to turn left wheel
at -100 round per minute (RPM), right wheel at 100 RPM, and turn all the ring led on with green intensity set to 10.
After 5 seconds, the ring led is turned off. Since in the mBot controller we implement a watch dog, the motors will
be stop automatically by the watchdog.
\code
#include <visp3/core/vpSerial.h>
#include <visp3/core/vpTime.h>

int main()
{
  vpSerial serial("/dev/ttyAMA0", 115200);
  serial.write("MOTOR_RPM=-100,100\n");
  serial.write("LED_RING=0,0,10,0\n");
  vpTime::sleep(5000);
  serial.write("LED_RING=0,0,0,0\n");
}
\endcode

We provide a test in test-serial-mbot.cpp that allows to check if instructions send from the Raspberry Pi to the mBot
Ranger by serial link is working. If you build ViSP from source as explained in \ref tutorial-install-raspberry the
corresponding binary is available in `/home/pi/visp-ws/visp/robot/mbot/raspberry/visp`.

Run first this test on the Raspberry Pi with `--help` option:

    $ cd /home/pi/visp-ws/visp/robot/mbot/raspberry/visp
    $ ./test-serial-mbot --help
    Usage:
    ./test-serial-mbot --vx <linear velocity in m/s> --wz <rotational velocity in deg/s> --rpm-l <motor left RPM> --rpm-r <motor right RPM> --t <duration of the command in second> --help

    Example:
    ./test-serial-mbot --vx 0.05 --wz 0 --t 4

You can now try to send a linear velocity of 5 cm/s during 4 sec to the robot using:

    $ ./test-serial-mbot --vx 0.05 --wz 0 --t 4
    Apply v_x=0.05 m/s  w_z=0 deg/s during 4 seconds
    Motor left vel: -1.53846 motor right vel: 1.53846 (rad/s)
    Send: MOTOR_RPM=-14,14

\section mbot_pi_vs Visual-servoing
\subsection mbot_pi_ibvs Image-based visual-servoing

The next step is now to run the image-based visual servoing example implemented in mbot-apriltag-ibvs.cpp. In this
example we use two image moments as visual features and especially
the normalized gravity center along \f$ x \f$ axis implemented in vpFeatureMomentGravityCenterNormalized and the
normalized area implemented in vpFeatureMomentAreaNormalized. The first one allows to control the orientation of the
robot to maintain the target on a vertical line in the middle of the image, while the second feature allows to
regulate the distance wrt the target. These features are interesting here since they don't require any 3D information
to compute the error vector or the interaction matrix.

If you build ViSP from source as explained in \ref tutorial-install-raspberry the corresponding binary is available in
`/home/pi/visp-ws/visp-build/tutorial/robot/mbot/raspberry/visp/`.

    $ cd /home/pi/visp-ws/visp-build/tutorial/robot/mbot/raspberry/visp
    $ ./mbot-apriltag-ibvs

Run `./mbot-apriltag-ibvs --help` to see which are the command line options available.
Adding option `--display-on` allows to display images like the following that shows the result of the image processing
and features that are used:
\image html img-mbot-ibvs-features.png
Since using this option could introduce a lag due to the network connection, we recommend to use it only for curiosity.

\subsection mbot_pi_2Dhalf_vs 2D-half visual-servoing

We provide also a 2D half visual servoing example implemented in mbot-apriltag-2D-half-vs.cpp. This example uses a mix
between 2D and 3D visual features and especially the 2D \f$ x \f$ coordinate of the target cog implemented in
vpFeaturePoint and the depth feature implemented in vpFeatureDepth. The first one allows to control the orientation
of the robot to maintain the target on a vertical line in the middle of the image, while the second feature allows
to regulate the distance Z wrt the target.

The corresponding binary is available in `/home/pi/visp-ws/visp-build/tutorial/robot/mbot/raspberry/visp/`.

    $ cd /home/pi/visp-ws/visp-build/tutorial/robot/mbot/raspberry/visp
    $ ./mbot-apriltag-2D-half-vs

Run `./mbot-apriltag-2D-half-vs --help` to see which are the command line options available.
Adding option `--display-on` allows to display images like the following that shows the result of the image processing
and features that are used:
\image html img-mbot-2D-half-vs-features.png
Since using this option could introduce a lag due to the network connection, we recommend to use it only for curiosity.

\subsection mbot_pi_pbvs Position-based visual-servoing

We provide also a position-based visual servoing example implemented in mbot-apriltag-pbvs.cpp. Here we use pure 3D
visual features implemented in vpFeaturePoint3D and especially X and Z values obtained from the pose of the tag.

The corresponding binary is available in `/home/pi/visp-ws/visp-build/tutorial/robot/mbot/raspberry/visp/`.

    $ cd /home/pi/visp-ws/visp-build/tutorial/robot/mbot/raspberry/visp
    $ ./mbot-apriltag-pbvs

Run `./mbot-apriltag-pbvs --help` to see which are the command line options available. Adding option `--display-on`
allows to display images like the following that shows the result of the image processing and features that are used:
\image html img-mbot-pbvs-features.png
Since using this option could introduce a lag due to the network connection, we recommend to use it only for curiosity.

\section mbot_pi_service Howto launch the demo once mBot powered on

It is possible to transform the `mbot-apriltag-2D-half-vs` demo into a service on the Raspberry Pi in order to start
the binary when raspberry has booted. To this end, we need to install a daemon. This example uses a 3D visual features
and especially the \f$ X \f$ 3D coordinate of the target 3D frame implemented in vpFeaturePoint3D and the depth feature
implemented in vpFeatureDepth. The first one allows to control the orientation of the robot to maintain the target
origin frame on a vertical line in the middle of the image, while the second feature allows to regulate the distance
Z wrt the target.

If you follow \ref tutorial-install-raspberry, there is already a daemon in
`/home/pi/visp-ws/visp/tutorial/robot/mbot/raspberry/daemon`. You need to edit `visual-servo` file and adapt the
location of the visual-servo binary location.

    DAEMON="/home/pi/visp-ws/visp-build/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs"

Install the daemon

    $ sudo cp /home/pi/visp-ws/visp/tutorial/robot/mbot/raspberry/daemon/visual-servo /etc/init.d
    $ sudo chmod 0755 /etc/init.d/visual-servo

Reboot or reload the daemon

    $ sudo systemctl daemon-reload

Testing the daemon

    $ sudo /etc/init.d/visual-servo start
    $ sudo /etc/init.d/visual-servo stop

Now we can add the service at startup

    $ sudo update-rc.d visual-servo defaults

To remove the service

    $ sudo update-rc.d visual-servo remove

*/