PX4 SITL simulation with ROS melodic

この記事はROS melodicでPX4シミュレーションを使用してみたときに詰まったところなどを記した備忘録です。
※公開時期が2021年になっていますが，執筆は2020年8月です．ご注意下さい．

# 実装環境
Macbook Pro 2018(VMWare Fusion) + ROS melodic on ubuntu 18.04.5 
のほか，dual boot, ubuntu機でも動作確認できてます．


# 環境構築
まずは[Development Environment on Linux](https://dev.px4.io/master/en/setup/dev_env_linux.html)で環境構築

[こちらのサイト](https://uenota.github.io/dronedoc/ja/px4sim/px4sim.html#fcu-url)も大変参考になります。
バージョンの違いだけ要注意

## Development Toolchainのセットアップ
```
sudo usermod -a -G dialout $USER
```

実行後はログアウトし再度ログインし設定を反映させる


ビルド時にエラーがでることがあるので以下を実行しておくと安心

```
sudo apt update -y 
```

```
sudo apt install  python3-pip -y
```

```
sudo pip3 install -U rosdep
sudo rosdep init
rosdep update
```

```
sudo apt install python-pip -y
```

```
pip3 install --user empy
pip3 install --user toml
pip3 install --user packaging
pip3 install --user numpy
pip3 install --user jinja2
sudo apt-get install libgstreamer1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-libav gstreamer1.0-doc gstreamer1.0-tools gstreamer1.0-x gstreamer1.0-alsa gstreamer1.0-gl gstreamer1.0-gtk3 gstreamer1.0-qt5 gstreamer1.0-pulseaudio -y
```

```
sudo apt install python-rosdep2 -y
```

```
sudo apt install python-wstool -y
sudo apt install python-rosinstall-generator -y
```

gstreamer

```
sudo apt-get install libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev libgstreamer-plugins-bad1.0-dev gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-libav gstreamer1.0-doc gstreamer1.0-tools gstreamer1.0-x gstreamer1.0-alsa gstreamer1.0-gl gstreamer1.0-gtk3 gstreamer1.0-qt5 gstreamer1.0-pulseaudio  
```

## Gazebo with ROS Melodic
https://docs.px4.io/master/en/simulation/gazebo.html#ros-gazebo
[ubuntu_sim_ros_melodic.sh](https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh)をダウンロードし、bash shellで以下を実行する.
少し時間がかかる可能性あり

```
wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh

source ubuntu_sim_ros_melodic.sh
```

[Development Environment on Ubuntu LTS / Debian Linux (ROS/Gazebo)](https://dev.px4.io/master/en/setup/dev_env_linux_ubuntu.html)



次に，Downloading PX4 Source Codeをダウンロードする
尚，ここではdrone_projectディレクトリを作成し，その下に入れた

```
mkdir -p ~/drone_project
cd drone_project
```

[Building PX4 Software](https://dev.px4.io/master/en/setup/building_px4.html)
上記サイトの手順に従ってソースコードをクローンする

```
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
```

これまでの設定が問題ないか確認

```
cd ~/drone_project/PX4-Autopilot
make px4_sitl gazebo
```

たまにエラーを吐くので随時以下のサイトを参考にする
https://qiita.com/kkoba84/items/dfa23f87b0c94d0a6935
https://qiita.com/k-koh/items/d99ab9b0964c7f994cf1

久しぶりに起動したらGazebo起動時にエラーが出たので上記を一通りやり直したら直った。

## MAVROSパッケージのインストール
[MAVROS](https://dev.px4.io/master/en/ros/mavros_installation.html)
ただし[公式のインストールガイド](https://github.com/mavlink/mavros/tree/master/mavros#installation)があるのでそちらに従う

### Binary installation (deb)
こちらは環境構築が容易な一方で手を加えにくい(e.g. ethzのmav_control_rwパッケージとの互換性)問題点もある．
まずはこちらを推奨．

```
sudo apt-get install ros-melodic-mavros ros-melodic-mavros-extras
```

```
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
./install_geographiclib_datasets.sh
```

./install_geographiclib_datasets.shで「許可がありません」と出る場合は以下を実行する

```
sudo bash ./install_geographiclib_datasets.sh
```

また、"OS not supported!" と心臓に悪いエラーが出る場合があったが、依存パッケージが足りなかっただった

```
$ sudo bash ./install_geographiclib_datasets.sh
./install_geographiclib_datasets.sh: line 33: hash: geographiclib-get-geoids: not found
./install_geographiclib_datasets.sh: line 37: hash: geographiclib-datasets-download: not found
OS not supported! Check GeographicLib page for supported OS and lib versions.
```

### Source installation

```
sudo apt-get install python-catkin-tools python-rosinstall-generator -y

# 1. Create the workspace: unneeded if you already has workspace
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin init
wstool init src

# 2. Install MAVLink
#    we use the Kinetic reference for all ROS distros as it's not distro-specific and up to date
rosinstall_generator --rosdistro kinetic mavlink | tee /tmp/mavros.rosinstall

# 3. Install MAVROS: get source (upstream - released)
rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall
# alternative: latest source
# rosinstall_generator --upstream-development mavros | tee -a /tmp/mavros.rosinstall
# For fetching all the dependencies into your catkin_ws, just add '--deps' to the above scripts
# ex: rosinstall_generator --upstream mavros --deps | tee -a /tmp/mavros.rosinstall


# 4. Create workspace & deps
wstool merge -t src /tmp/mavros.rosinstall -y
wstool update -t src -j4
rosdep install --from-paths src --ignore-src -y

# 5. Install GeographicLib datasets:
./src/mavros/mavros/scripts/install_geographiclib_datasets.sh

# 6. Build source
catkin build

# 7. Make sure that you use setup.bash or setup.zsh from workspace.
#    Else rosrun can't find nodes from this workspace.
source devel/setup.bash

```

### パスを通す

旧バージョンではposix_sitl_defaultが使用されていましたが、~/Firmware(現PX4-Autopilotディレクトリ)/buildディレクトリを見るとpx4_sitl_defaultが置かれているのでその部分を変更し、~/.bashrcに以下の内容を書き込みます。
その際，自分の環境でのPX4-Autopilotの位置に従って$HOME以下のパスを変更すること．今回は，drone_projectディレクトリ以下にあるため，以下のように記述する

```
source $HOME/drone_project/PX4-Autopilot/Tools/setup_gazebo.bash $HOME/drone_project/PX4-Autopilot $HOME/drone_project/PX4-Autopilot/build/px4_sitl_default > /dev/null
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$HOME/drone_project/PX4-Autopilot
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$HOME/drone_project/PX4-Autopilot/Tools/sitl_gazebo
```

```
sudo apt-get install geographiclib-* ros-kinetic-geographic-*
```

## 自作ノードの作成・実行
参照：[MAVROS Offboard control example](https://docs.px4.io/master/en/ros/mavros_offboard.html)

### ROSパッケージの作成

```
cd ~/catkin_ws/src
#catkin_create_pkg px4_sim_pkg roscpp geometry_msgs mavros_msgs
catkin create pkg px4_sim_pkg --catkin-deps roscpp geometry_msgs mavros_msgs
code px4_sim_pkg/src/offboard_sample.cpp
```

### ノードの作成

```offboard_sample.cpp
/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}
```



### CMakeLists.txtの編集
以下の内容を末尾に追加する

```~/catkin_ws/src/px4_sim_pkg/CMakeLists.txt
add_executable(offboard_sample src/offboard_sample.cpp)
target_link_libraries(offboard_sample ${catkin_LIBRARIES})
```

### ビルドする
```
cd ~/catkin_ws
catkin build
```

### ノードの起動
１つ目のターミナルで

```
roslaunch px4 mavros_posix_sitl.launch
```

2つ目のターミナルを起動し以下のような画面が見えれば完了です。

```
rosrun px4_sim_pkg offboard_sample
```

![スクリーンショット 2020-08-24 22.58.41.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/452637/b5450527-07ad-b55e-547c-dc0b359ea963.png)

### launchファイルを書く
```~/catkin_ws/src/px4_sim_pkg/launch/cpp_offb_sample.launch
<launch>

    <include file="$(find px4)/launch/mavros_posix_sitl.launch" />
    <node name="offb_node" pkg="px4_sim_pkg" type="offboard_sample" />

</launch>
```

ターミナルを開き以下を実行してOffboardノードを起動

```
roslaunch px4_sim_pkg cpp_offb_sample.launch
```

## Debug

```
ln -s ~/src/PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/ ~/.ros/etc/
ln -s ~/src/PX4-Autopilot/ROMFS/px4fmu_common/init.d ~/.ros/etc/
ln -s ~/src/PX4-Autopilot/ROMFS/px4fmu_common/mixers ~/.ros/etc/
ln -s ~/src/PX4-Autopilot/ROMFS/px4fmu_common/mixers-sitl/ ~/.ros/etc/
ln -s ~/src/PX4-Autopilot/ROMFS/px4fmu_common/CMakeLists.txt ~/.ros/etc/
```




## 小ネタ
https://docs.px4.io/v1.12/en/simulation/#run-simulation-faster-than-realtime

https://docs.px4.io/v1.12/en/simulation/gazebo.html


# References
https://uenota.github.io/dronedoc/ja/appendix/trouble_shooting/subpages/ros/startup_script_not_found.html

https://dev.px4.io/master/en/