# Realsense D435i ROS package install guide

## Install Realsense driver and ROS package

1. Install [ROS](http://wiki.ros.org/melodic/Installation/Ubuntu) (就照著官方文件跑應該不用講啥吧 XD)
2. Install [Realsense driver for jetson nano](https://github.com/JetsonHacksNano/installLibrealsense) (可以跑下面的指令就好)

    $ cd ~
    $ git clone https://github.com/JetsonHacksNano/installLibrealsense.git
    $ cd installLibrealsense
    $ ./installLibrealsense.sh

3. Install driver for D435i
1. 把 D435i 插到板子上 (USB)
2. Run `realsense-viewer` on your terminal
3. 接著沒有出錯的話會跳出一個圖形介面 (realsense 官方的 viewer)
4. 接著測試，把左邊的 RGB Camera 打開 (右上角點選 2D)，如果能看見影像就成功了
5. 如果打開 `Motion Module` 有跳出 permission denied 的話，就是你 USER 的權限不夠存取 usb (不要問我為什麼我不知道==)，所以要先編輯 `/etc/udev/rules.d/99-nv-l4t-usb-host-config.rules` 檔案裡面的設定，並在有註解 `#Jetson Nano` 的下面那兩行加上 `GROUP="<your_user_name>"` (要長下面那個樣子)，接著在 Terminal 下 `sudo udevadm control --reload-rules && sudo udevadm trigger` 應該就可以跑了
4. Install realsense package for ROS (一樣跑下面的指令就可以了)

    $ cd ~
    $ mkdir catkin_ws
    $ cd catkin_ws
    $ mkdir src
    $ catkin_make
    $ cd ~
    $ git clone https://github.com/JetsonHacksNano/installRealSenseROS.git
    $ cd installRealSenseROS/
    $ ./installRealSenseROS.sh

然後在 `catkin_make` 的時候應該會噴錯：`find_if() was not declared in this file`，就去那個檔案找到那行然後在 `find_if` 前面加上 `std::` 就可以了。
5. 這樣應該就裝好了，想要測試的話可以跑下面的指令：`roslaunch realsense2_camera rs_camera.launch filters:=pointcloud depth_fps:=6 color_fps:=6 infra_fps:=6`，這邊特別要注意的是因為 nano 比較廢，fps 太高的話會跑不起來，所以設成 6 比較保險 (預設好像是 30，根本跑不動==)
6. 打開你的 rviz，照理來說可以看到下面這些東西 (Fixed Frame 選 camera_link，接著照著 topic 選就可以了)：

## SWAP

在裝以下那些套件之前建議你各位先切 swap 出來，不然你們 compile 的時候會卡爆，我自己是切了 8G (RAM \* 2) 看你們要切幾 G，下面指令照著跑就好 (也可以參考 [這篇](https://chtseng.wordpress.com/2019/05/01/nvida-jetson-nano-%E5%88%9D%E9%AB%94%E9%A9%97%EF%BC%9A%E5%AE%89%E8%A3%9D%E8%88%87%E6%B8%AC%E8%A9%A6/))，

$ sudo swapoff # (先把 swap 的設定關掉)
$ sudo fallocate -l 8G /swapfile
$ sudo chmod 600 /swapfile
$ sudo mkswap /swapfile
$ sudo swapon /swapfile
$ sudo swapon –show
###### 下面是開機設定 ######
$ sudo cp /etc/fstab /etc/fstab.bak
$ echo '/swapfile none swap sw 0 0' | sudo tee -a /etc/fstab

接著可以用 `free -h` 看一下有沒有你已經設定好的 swap 空間，有的話就可以ㄌ。

## Install ROS rtabmap package and save octomap file

1. Install [rtabmap for ROS](https://github.com/introlab/rtabmap_ros)，因為 Nvidia Jetson 系列沒辦法直接用 `apt-get install` 安裝所以要直接 compile source code，一樣照著下面的指令下就好：

    $ sudo apt-get update -y
    $ sudo apt-get install ros-melodic-move-base-msgs # 這個套件要先裝
    $ sudo apt-get purge ros-melodic-rtabmap ros-melodic-tabmap-ros ros-melodic-cv-bridge # 先全部刪乾淨再裝比較安全
    $ cd ~
    $ git clone https://github.com/introlab/rtabmap.git rtabmap
    $ cd rtabmap/build
    $ cmake ..  # .. 要加，要加，要加
    $ make -j1  # 這要跑大概一到兩小時 XD
    $ sudo make install
    $ cd ~/catkin_ws
    $ mkdir src
    $ cd src
    $ git clone https://github.com/BrutusTT/vision_opencv.git
    $ git clone https://github.com/ros-perception/image_transport_plugins
    $ git clone https://github.com/introlab/rtabmap_ros.git
    $ catkin_make -j1  # 這也差不多要一到兩小

> 這邊盡量都用 `-j1` 比較不會跑到一半就整個卡死還要全部重來==
> [name=家好]
2. 接著因為它 imu 需要 filter，所以要裝一個套件來用，應該兩種方法都可以。要裝這個：[imu_filter_madgwick](https://github.com/ccny-ros-pkg/imu_tools/tree/melodic)。我是直接 `sudo apt-get install ros-melodic-imu-filter-madgwick`
3. 接著就可以測試了，分別在三個 terminal 下這三個指令 (要照順序喔)，這邊要特別注意的是因為 Qt 的關係所以不要用它原生的圖形介面 (八成是什麼地圖都看不到只看得到影像而已XD) 直接用 rviz 會比較快，所以 `rabmapviz:=false` 跟 `rviz:=true` 要記得下。啊有東西的話就是成功了喔 \>\<。

    $ roslaunch realsense2_camera rs_camera.launch \
    align_depth:=true \
    unite_imu_method:="linear_interpolation" \
    enable_gyro:=true \
    enable_accel:=true \
    depth_fps:=6 \
    color_fps:=6 \
    infra_fps:=6

    $ rosrun imu_filter_madgwick imu_filter_node \
    _use_mag:=false \
    _publish_tf:=false \
    _world_frame:="enu" \
    /imu/data_raw:=/camera/imu \
    /imu/data:=/rtabmap/imu

    $ roslaunch rtabmap_ros rtabmap.launch \
    rtabmap_args:="--delete_db_on_start --Optimizer/GravitySigma 0.3" \
    depth_topic:=/camera/aligned_depth_to_color/image_raw \
    rgb_topic:=/camera/color/image_raw \
    camera_info_topic:=/camera/color/camera_info \
    approx_sync:=false \
    wait_imu_to_init:=true \
    imu_topic:=/rtabmap/imu \
    rviz:=true \
    rtabmapviz:=false

4. 最後就是存 octomap 的檔案了，要先裝 [octomap_server](https://github.com/OctoMap/octomap_mapping)，不過不要用 source code compile 因為它 source code 只到 kinetic 版本而已，直接跑 `sudo apt-get install ros-melodic-octomap-server` 就好。另外如果想要在 rviz 上面看 octomap 的相關 topic 的話要裝套件 [octomap_rviz_plugins](https://github.com/OctoMap/octomap_rviz_plugins)，一樣 `sudo apt-get install ros-melodic-octomap-rviz-plugins` 就可以了。
5. 最後就是照著 3 的測試指令下去執行 rtabmap，覺得地圖建的差不多之後就可以下 `rosrun octomap_server octomap_saver mapfile.bt octomap_full:=/rtabmap/octomap_full` 來存檔 (rtabmap 的 node 不要關掉喔，關掉就收不到 topic 了 XD)，不過這邊要注意的是它是存 `.bt` 檔 (黑白的)，如果要存彩色的 `.ot` 檔的話要跑 `rosrun octomap_server octomap_saver -f mapfile.ot octomap_full:=/rtabmap/octomap_full` 才行。
6. 存完之後就可以跑 `rosrun octomap_server octomap_server_node <your .bt/.ot file>` 來看有沒有存成功。也可以跑 [國師的扣](https://github.com/kuoshih/octomap_code) 來看檔案，不過要做一些前處理 (你們自己去找問題 XD)。