Perception-Driver
=================

Unitree quadrupeds `Aliengo <https://quadruped.de/aliengo/>`__ and
`A1 <https://quadruped.de/unitree-a1-en/>`__ are equipped with depth
cameras. Here for the sake this documentation Unitree A1 is tested which
has `Intel Realsense
D435i <https://www.intelrealsense.com/depth-camera-d435i/>`__. Moreover
for this specific model we have `Rasberry Pi
4b <https://www.raspberrypi.org/products/raspberry-pi-4-model-b/>`__\ (Later
will be referred to RPI), so all the installation instructions are
specific to this specific model.

Driver Installation
-------------------

Intel RealSense has its `ROS
driver <https://github.com/IntelRealSense/realsense-ros>`__ available.
This ROS driver is dependent on ``librealsense`` `RealSense
SDK <https://www.intelrealsense.com/sdk-2/>`__ which also has a few
dependencies. So here we'll start by first installing dependencies for
RealSense SDK.

-  Let's start by updating, upgrading the system and installing
   dependencies and some useful tools.

.. code:: bash

    sudo apt-get update && sudo apt-get dist-upgrade
    sudo apt-get install automake libtool vim cmake libusb-1.0-0-dev libx11-dev xorg-dev libglu1-mesa-dev

-  Next task is to expand the filesystem in RPI by selecting the menu
   entry ``Advanced Options`` and reboot.

.. code:: bash

    sudo raspi-config

-  After successful reboot now increase SWAP size in
   ``/etc/dphys-swapfile`` by adding ``CONF_SWAPSIZE=2048`` and
   commenting out the deafult one(usually its ``CONF_SWAPSIZE=100``)

.. code:: bash

    sudo vim /etc/dphys-swapfile

-  Apply the above mentioned SWAP changes by:

.. code:: bash

    sudo /etc/init.d/dphys-swapfile restart swapon -s

-  Create a new udev rule for the installation by running following
   commands:

.. code:: bash

    cd ~
    git clone https://github.com/IntelRealSense/librealsense.git
    cd librealsense
    sudo cp config/99-realsense-libusb.rules /etc/udev/rules.d/

-  Apply udev changes within root:

.. code:: bash

    sudo su
    udevadm control --reload-rules && udevadm trigger
    exit

-  Add the following line to your ``.bashrc`` file to modify the path:

.. code:: bash

    export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH

-  Apply the changes made so far by:

.. code:: bash

    source ~/.bashrc

**Dependencies**

1. Install ``protobuf`` — Google's platform-neutral, language-neutral,
   extensible mechanism for serializing structured data by(in case of
   errors in this installatio go to next step 1.1. for binary
   installations):

.. code:: bash

    cd ~
    git clone --depth=1 -b v3.10.0 https://github.com/google/protobuf.git
    cd protobuf
    ./autogen.sh
    ./configure
    make -j1
    sudo make install
    cd python
    export LD_LIBRARY_PATH=../src/.libs
    python3 setup.py build --cpp_implementation 
    python3 setup.py test --cpp_implementation
    sudo python3 setup.py install --cpp_implementation
    export PROTOCOL_BUFFERS_PYTHON_IMPLEMENTATION=cpp
    export PROTOCOL_BUFFERS_PYTHON_IMPLEMENTATION_VERSION=3
    sudo ldconfig
    protoc --version

1.1. To install ``protobuf`` binaries:

.. code:: bash

    sudo apt install protobuf-c*
    sudo apt install python3-protobuf 

2. Install C++ parallelism library ``libtbb-dev``:

.. code:: bash

    cd ~
    wget https://github.com/PINTO0309/TBBonARMv7/raw/master/libtbb-dev_2018U2_armhf.deb
    sudo dpkg -i ~/libtbb-dev_2018U2_armhf.deb
    sudo ldconfig
    rm libtbb-dev_2018U2_armhf.deb

**RealSense SDK**

-  Install ``librealsense`` RealSense SDK:

.. code:: bash

    cd ~/librealsense
    mkdir  build  && cd build
    cmake .. -DBUILD_EXAMPLES=true -DCMAKE_BUILD_TYPE=Release -DFORCE_LIBUVC=true
    make -j1
    sudo make install

-  Let's just install Python bindings
   ``pyrealsense2 for``\ librealsense\`:

.. code:: bash

    cd ~/librealsense/build
    cmake .. -DBUILD_PYTHON_BINDINGS=bool:true -DPYTHON_EXECUTABLE=$(which python3)
    make -j1
    sudo make install

-  Modify the ``PYTHONPATH`` environment variable by adding the
   following line to \`.bashrc'

.. code:: bash

    export PYTHONPATH=$PYTHONPATH:/usr/local/lib

-  Apply the changes by:

.. code:: bash

    source ~/.bashrc

-  ``OpenGL`` installation:

.. code:: bash

    sudo apt-get install python-opengl
    sudo -H pip3 install pyopengl
    sudo -H pip3 install pyopengl_accelerate==3.1.3rc1

-  Change RPI settings to enable ``OpenGL``:

.. code:: bash

    sudo raspi-config
    # "7. Advanced Options" – "A8 GL Driver" – "G2 GL (Fake KMS)"

**ROS Driver**

RealSense ROS driver in RPI is not availble from binaies, so let's
install the driver from sources and its one depndency
``ddynamic_reconfiure``. Follow the steps below for the installation.

-  Create ``catkin_ws``

.. code:: bash

    mkdir -p ~/catkin_ws/src
    cd ~/catkin_ws/src/
    git clone https://github.com/IntelRealSense/realsense-ros.git
    git clone https://github.com/pal-robotics/ddynamic_reconfigure.git
    cd realsense-ros/
    git checkout `git tag | sort -V | grep -P "^2.\d+\.\d+" | tail -1`
    cd ..
    catkin_init_workspace
    cd ..
    catkin_make clean
    catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
    catkin_make install
    source ~/catkin_ws/devel/setup.bash

**Usage**

To use the installed driver run the following command:

.. code:: bash

    roslaunch realsense2_camera rs_camera.launch enable_sync:=true

Now you can run ``rviz`` or some other tools to see the depth camera
feed:

.. code:: bash

    rosrun rviz rviz

Add a new frame of RGB image or depth one to see the result.