ROS2(rclcpp) + VSCode + Dockerのデバッグ付き開発環境

# この記事について
- ROS2でC++(rclcpp)を使った開発環境の構築方法です。ROS歴2週間なので間違っていたり、もっといい方法があれば教えて下さいmm
- Docker上にROS2環境を用意します
- ホストPCのVSCodeからDocker内のワークスペースをコーディングできるようにします。IntelliSense付きで
- ホストPCのVSCodeからDocker内のワークスペースをデバッグできるようにします
- 以下の3環境で確認済みです
    - Ubuntu 20.04
    - Ubuntu 20.04 + Docker
    - Windows11 + WSL2 (Ubuntu 20.04) + Docker

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/214268/c6fb3ce3-4ed2-75bf-e026-6856f0cd7ac7.png)


## 環境
- ROS2: Foxy (恐らく別バージョンでも同様)
- Host PC:
    - Ubuntu 20.04
    - Windows11 + WSL2 (Ubuntu 20.04)
- Host PC SW:
    - Docker
    - Visual Studio Code
        - ms-vscode-remote.vscode-remote-extensionpack
        - ms-iot.vscode-ros

- コーディングとデバッグはVSCodeを使いますが、ビルドはターミナルから行います
- 以下の説明はWindows + WSL2をベースに行います
     - Ubuntu + Dockerの場合は、VSCodeの操作でHost -> WSLへの接続が不要です
     - Ubuntu上で直接開発する場合は、VSCodeの操作でContainerへのAttachが不要です

### メモ: Dockerインストール on Ubuntu
- https://docs.docker.com/engine/install/ubuntu/
- `sudo gpasswd -a iwatake docker`


# ROS2開発用コンテナを用意する
- ROS2開発用のコンテナを作ります。今回はオフィシャルのfoxyイメージをそのまま使います
- カスタマイズしたい場合は自分でdockerfileを用意してください
- その後動作確認用にtopicを送受信してみます
    - 受信用に、tmuxなどでターミナルを分割するか、別途ターミナルを開いて同じコンテナに入ってください

```sh
xhost local:    # 必要に応じて
docker run -it --name foxytest1 -e DISPLAY=$DISPLAY osrf/ros:foxy-desktop
# docker run -it --name foxytest1 -e DISPLAY=$DISPLAY ros:foxy
# docker run -it --name foxytest1 -e DISPLAY=$DISPLAY --net=host -v /dev:/dev -v /home/iwatake/devel/study_ros/:/study_ros --privileged ros:foxy
# docker start foxytest1
# docker exec -it foxytest1 /ros_entrypoint.sh bash

### 以下、Docker 内のコマンド ###
# export DISPLAY=192.168.1.2:0
ros2 topic pub /chatter std_msgs/String "data: Hello World"
ros2 topic echo /chatter
```

# Workspaceの用意とサンプルコード
## Workspaceを用意する
- ここではDocker内で新規にworkspaceを用意します
- ホスト側と共有したい場合は、docker runの時にマウントした場所を使ってください (上記のコメントアウトしているコマンドを参照)

```sh:Workspaceの作成とひとまずビルドと実行
mkdir -p ~/dev_ws/src
cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake --dependencies rclcpp rclcpp_components std_msgs --node-name my_talker my_package
cd ..
rosdep install -i --from-path src --rosdistro foxy -y
colcon build

# ビルドと実行は別のターミナルでやった方がよいです(環境変数の設定がごっちゃになるので)
cd ~/dev_ws
. install/setup.bash
ros2 run my_package my_talker
```

```sh:必要なソースコードを作っておく
touch src/my_package/src/my_listener.cpp
mkdir -p src/my_package/launch
touch src/my_package/launch/my_launch.py
```

## Coding
- Host PCから、Docker内のworkspaceを開きます
    - Host PCでVSCodeを開く
    - Remote Explorer -> WSL Targets -> Ubuntu-20.04 -> Connect -> 新しいWindowが開く
        - Windows + WSL2の場合のみ
    - Remote Explorer -> Containers -> ros:foxy oxytest -> Attach to Container -> 新しいWindowが開く
        - Dockerを使う場合のみ
    - Explorer -> Open Folder -> `/root/dev_ws/`
    - Extensionsから`ms-iot.vscode-ros` (`ms-vscode.cpptools` , `ms-python.python` ) をインストールする
- サンプルとして簡単なtalker/listenerとlaunchを実装します
    - 以下、コードは折りたたんでいます
    - DLL用のexport設定は省略しています

<details><summary>ソースコード ★折り畳み★</summary><div>

```cmake:CMakeLists.txt
cmake_minimum_required(VERSION 3.5)
project(my_package)

set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(std_msgs REQUIRED)

add_library(my_talker SHARED src/my_talker.cpp)
target_include_directories(my_talker PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
# target_compile_definitions(my_talker PRIVATE "MY_PACKAGE_BUILDING_DLL")
ament_target_dependencies(my_talker
  "rclcpp"
  "rclcpp_components"
  "std_msgs"
)
rclcpp_components_register_node(my_talker PLUGIN "my_package::MyTalker" EXECUTABLE my_talker_exe)

add_library(my_listener SHARED src/my_listener.cpp)
target_include_directories(my_listener PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
# target_compile_definitions(my_listener PRIVATE "MY_PACKAGE_BUILDING_DLL")
ament_target_dependencies(my_listener
  "rclcpp"
  "rclcpp_components"
  "std_msgs"
)
rclcpp_components_register_node(my_listener PLUGIN "my_package::MyListener" EXECUTABLE my_listener_exe)

install(TARGETS
  my_talker my_listener
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

# install(TARGETS
#   manual_composition
#   DESTINATION lib/${PROJECT_NAME}
# )

install(
  DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

ament_package()
```

```cpp:my_talker.cpp
#include <cstdlib>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"

namespace my_package
{
class MyTalker : public rclcpp::Node
{
public:
  MyTalker(const rclcpp::NodeOptions & options)
  : Node("my_talker", options)
  {
    publisher_ = this->create_publisher<std_msgs::msg::Int32>("my_topic", 10);
    timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&MyTalker::timer_callback, this));
  }

private:
  void timer_callback()
  {
    auto msg = std_msgs::msg::Int32();
    msg.data = count_++;
    RCLCPP_INFO(this->get_logger(), "data = %d", msg.data);
    publisher_->publish(msg);
  }

private:
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  int count_;
};
}

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(my_package::MyTalker)
```

```cpp:my_listener.cpp
#include <cstdlib>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"

namespace my_package
{
class MyListener : public rclcpp::Node
{
public:
  MyListener(const rclcpp::NodeOptions & options)
  : Node("my_listener", options)
  {
    subscription_ = this->create_subscription<std_msgs::msg::Int32>("my_topic", 10, std::bind(&MyListener::topic_callback, this, std::placeholders::_1));
  }

private:
  void topic_callback(const std_msgs::msg::Int32::SharedPtr msg)
  {
    RCLCPP_INFO(this->get_logger(), "data = %d", msg->data);
  }

private:
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_;
};
}

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(my_package::MyListener)
```

```py:my_launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package="my_package",
            executable="my_talker_exe",
            output="screen",
            emulate_tty=True,
        ),
        Node(
            package="my_package",
            executable="my_listener_exe",
            output="screen",
            emulate_tty=True,
        ),
    ])
```

</div></details>


# ビルドする
```sh
cd ~/dev_ws
colcon build
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Debug    # Debug用
```

# 実行する
ビルドとは別のターミナルで

```sh
cd ~/dev_ws
. install/setup.bash
ros2 launch my_package my_launch.py
```

# InteliSenseの設定
- `ms-iot.vscode-ros` をインストールしたことで、自動的にC++, Python用のInteliSense(コードスニペット)設定がされるはずです
- 必要に応じて以下を実行して更新してください
    - ctrl + hift + p -> `ROS: Update C++ Properties` , `ROS: Updat Python path`
- これによって、C++コード、Pythonコードどちらもコード補完や関数ジャンプが使えるはずです

## うまくいかない場合
- 基本的には自動生成される設定でうまくいくはずです。が、うまくいかなかったり不十分な指定がある場合は以下を参考に設定してください
- C++用の設定は2つ示します (c_cpp_properties.json)
    - 1つ目はパスを直接指定しています。自動生成されたファイルと同等です。必要に応じてパスを編集してください
    - 2つ目は`colcon build` 時に出力される`compile_commands.json` からどのパスを参照するかを取得しています
        - `compile_commands.json` を出力するために、CMakeLists.txt内に `set(CMAKE_EXPORT_COMPILE_COMMANDS ON)` を記載しています
        - そのため、インテリセンスを有効にするためには一度colcon buildする必要があります
        - ROSに限らず、一般的なC/CPP開発でも使えるテクです

```json:.vscode/settings.json
{
    "cmake.configureOnOpen": false,
    "python.autoComplete.extraPaths": [
        "/opt/ros/foxy/lib/python3.8/site-packages/"
    ],
    "python.analysis.extraPaths": [
        "/opt/ros/foxy/lib/python3.8/site-packages"
    ]
}
```

```json:.vscode/c_cpp_properties.json
{
  "configurations": [
    {
      "includePath": [
        "/opt/ros/foxy/include/**",
        "/root/dev_ws/src/my_package/include/**",
        "/usr/include/**"
      ],
      "name": "ROS",
      "intelliSenseMode": "gcc-x64",
      "compilerPath": "/usr/bin/gcc",
      "cStandard": "gnu11",
      "cppStandard": "c++14"
    }
  ],
  "version": 4
}
```

```json:.vscode/c_cpp_properties.json
{
  "configurations": [
    {
      "includePath": [
      ],
      "compileCommands": "${workspaceFolder}/build/compile_commands.json",
      "name": "ROS",
      "intelliSenseMode": "gcc-x64",
      "compilerPath": "/usr/bin/gcc",
      "cStandard": "gnu11",
      "cppStandard": "c++14"
    }
  ],
  "version": 4
}
```

# デバッグする
- ここまでで、コード補完付きのコーディング環境と実行環境が整いました。次は、デバッグ環境を整えます
- Docker内にgdbをインストールします
    - `apt update && apt install -y gdb`
- 新しく`.vscode/launch.json` ファイルを作り以下のように編集します

```json:.vscode/launch.json
{
    "version": "0.2.0",
    "configurations": [
        {
            "name": "ROS: Launch",
            "type": "ros",
            "request": "launch",
            "target": "${workspaceFolder}/src/my_package/launch/my_launch.py"
        }
    ]
}
```

- デバッグ情報を付けてビルドします
    - `colcon build --cmake-args -DCMAKE_BUILD_TYPE=Debug`
- F5キーを押すことでデバッグが開始されます
    - ここではノードを2つ生成しているので、別々にターミナル表示、実行制御が出来ます

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/214268/7700162a-19bb-d8c1-ab3f-1c4d22ffec23.png)


# トラブルシューティング
- 遭遇した問題
    - InteliSenseの設定が自動でされない
    - F5キーを押してデバッグしようとしたら、ROS coreが見つかりません的なエラーが出る
- 確認事項
    - ctrl + shift + p -> `ROS: show Status` が「online」になっているか確認する
    - 「offline」だったら以下を試す
- 対処法
    - ctrl + shift + p -> `ROS: start` してみる
    - ターミナル上でsetup.bashを実行してから、`code ./ &` する
        - ネイティブ環境の場合
    - `echo "source /opt/ros/foxy/setup.bash" >> ~/.bashrc` 設定をしておく
        - Dockerの場合
        - ターミナルからこの設定をしたら、一度VSCodeを閉じて、再度VSCodeからAttach to Containerする
    - VSCodeのROS用Extensionの設定を手動で行う
        - File -> Preferences -> Settings
            - `Ros: Distro` = `foxy` (または `/opt/ros/foxy` )
            - `Ros: Ros Setup Script` = `${workspaceFolder}/install/setup.bash`