moveitでsbplプランナーが使えるまで

OMPLとかCHOMPとかのプランナーでmoveitを使うチュートリアルがあるのにsbplプランナーを使うチュートリアルが存在しない！！のでせめて環境を作るところまでは書いておきたい。最終的にはpandaが動くようにしたい。また、moveitに自作のモーションプランナーを追加したい
# 環境
- ubuntu16.04
- kinetic
- moveitはソースでコンパイル済

#コケるポイント
- 英語でも文献がほとんど存在しない
- 他のプランナーはmoveitをsourceでコンパイルすれば済むがsbplはソースをcloneしないといけない
- READMEで肝心なrunnigがcomming soon (10 month ago)

# コケるまでの道のり
[sbpl公式](https://github.com/aurone/smpl)を参考に進めることにした。

```
cd ~/catkin_ws/src
git clone https://github.com/smpl/smpl
git clone https://github.com/sbpl/sbpl
cd sbpl && mkdir build && cd build && cmake .. && make && sudo make install
cd ~/catkin_ws/src
git clone https://github.com/aurone/leatherman
catkin build
```
すると以下のような問題に直面した。

# 問題点
### rosdep installした時に怒られる
ワークスペースのsrcディレクトリにsmplをクローンしてleathermanをクローンしてrosdep installした時に怒られる。
調べたところleathermanパッケージを見つけることができていないことが理由らしいが、私は同じディレクトリに配置している。何故だ・・・
これを無視した場合多分依存関係を解決できていないのでひとつづつ入れないといけない(多分)

```
$ rosdep install --from-paths smpl -i -y
ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
sbpl_collision_checking: Cannot locate rosdep definition for [leatherman]
sbpl_collision_checking_test: Cannot locate rosdep definition for [leatherman]
smpl_ros: Cannot locate rosdep definition for [leatherman]
sbpl_pr2_robot_model: Cannot locate rosdep definition for [leatherman]
sbpl_kdl_robot_model: Cannot locate rosdep definition for [leatherman]
smpl_test: Cannot locate rosdep definition for [leatherman]
```

### octomapが見つからないと怒られれる

```
CMake Error at /opt/ros/kinetic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "octomap_ros" with
  any of the following names:

    octomap_rosConfig.cmake
    octomap_ros-config.cmake

  Add the installation prefix of "octomap_ros" to CMAKE_PREFIX_PATH or set
  "octomap_ros_DIR" to a directory containing one of the above files.  If
  "octomap_ros" provides a separate development package or SDK, be sure it
  has been installed.
Call Stack (most recent call first):
  CMakeLists.txt:10 (find_package)
```

調べてみるとoctomapが無いということなのでapt-getしてみた

```
$ sudo apt-get install ros-kinetic-octomap
```
すると最新版が入っている???けれでも解決できない。
結局のところ必要だったのはこっちだった。違いはなんやねん。

```
$ sudo apt-get install ros-kinetic-octomap-ros
```

### pr2_kinematicsがないと怒られる
多分、rosdep installがうまいこと行けば怒られない。

```
git clone https://github.com/pr2/pr2_kinematics.git -b kinetic-devel
```

### used to generate cache
```
CMake Error: The source 
"/home/user/catkin_ws/src/leatherman/CMakeLists.txt" 
does not match the source 
"/home/user/catkin_ws/src/leatherman/CMakeLists.txt" 
used to generate cache.  Re-run cmake with a different source directory.
```
cacheのCMake.txtが邪魔をしているので削除する必要が有る

```
cd ~/catkin_ws/build/leatherman
rm CMakeCache.txt
```
### warning
とりあえず無視

```
CMake Warning at /opt/ros/kinetic/share/cmake_modules/cmake/Modules/FindEigen.cmake:62 (message):
  The FindEigen.cmake Module in the cmake_modules package is deprecated.

  Please use the FindEigen3.cmake Module provided with Eigen.  Change
  instances of find_package(Eigen) to find_package(Eigen3).  Check the
  FindEigen3.cmake Module for the resulting CMake variable names.

Call Stack (most recent call first):
  CMakeLists.txt:10 (find_package)


CMake Warning at /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:166 (message):
  catkin_package() DEPENDS on 'eigen' but neither 'eigen_INCLUDE_DIRS' nor
  'eigen_LIBRARIES' is defined.
Call Stack (most recent call first):
  /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:102 (_catkin_package)
  CMakeLists.txt:18 (catkin_package)
```
ここまででとりあえずワーニングは出るがbuildに成功した

#build成功後
### roslaunch smpl_test goal_pr2.launchができない
```
ResourceNotFound: sbpl_collision_checking_test
```
単純な話で```$ROS_PACKAGE_PATH```に```~/catkin_ws/src/smpl```が追加されていないため。```~/.bashrc```でexportすれば解決できる。

```~/.bashrc
export ROS_PACKAGE_PATH=~/catkin_ws/src/smpl:$ROS_PACKAGE_PATH
```
# 追記2019/08/10
https://github.com/aurone/pr2_sbpl_interface_config
これを動かしたいと思った。

```
cd ~/catkin_ws/src
git clone https://github.com/aurone/pr2_sbpl_interface_config.git
```
launchファイルを見るとpr2_moveit_configが必要そうだったので追加。

```
cd ~/catkin_ws/src
git clone https://github.com/ros-planning/moveit_pr2.git
catkin build
```
とりあえずbuildは成功

```
cd ~/catkin_ws/src/pr2_sbpl_interface_config/launch
roslaunch demo.launch
```
```demo.launch
ResourceNotFound: pr2_sbpl_interface_config
```
.bashrcに追加

```~/.bashrc
export ROS_PACKAGE_PATH=~/catkin_ws/src/pr2_sbpl_interface_config:$ROS_PACKAGE_PATH
```
もう一度、demo.launchを動かしてみる

```demo.launch
[ WARN] [1565429858.098507537]: Link 'head_mount_kinect_ir_link' is not known to URDF. Cannot disable collisons.
[ WARN] [1565429858.098572130]: Link 'head_mount_kinect_rgb_link' is not known to URDF. Cannot disable collisons.
[ WARN] [1565429858.098596310]: Link 'head_mount_link' is not known to URDF. Cannot disable collisons.
[ WARN] [1565429858.098603891]: Link 'head_mount_prosilica_link' is not known to URDF. Cannot disable collisons.
[ WARN] [1565429858.100868856]: Link 'head_mount_link' is not known to URDF. Cannot disable collisons.
[ WARN] [1565429858.424241902]: The root link base_footprint has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1565429858.615169947]: Manifest not found in folder '~/constraints_approximation_database'. Not loading constraint approximations.

...

[ INFO] [1565429862.261874614]: Listening to '/move_group/monitored_planning_scene'
TIFFFetchNormalTag: Warning, Incompatible type for "RichTIFFIPTC"; tag ignored.
TIFFFieldWithTag: Internal error, unknown tag 0x829a.
TIFFReadDirectory: Warning, Sum of Photometric type-related color channels and ExtraSamples doesn't match SamplesPerPixel. Defining non-color channels as ExtraSamples..
TIFFFetchNormalTag: Warning, Incompatible type for "RichTIFFIPTC"; tag ignored.

...

[ERROR] [1565429863.746551544]: Group 'base' does not have a parent link
[ WARN] [1565429863.746672912]: The root link base_footprint has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1565429864.747415366]: 
Deprecation warning: Trajectory execution service is deprecated (was replaced by an action).
Replace 'MoveGroupExecuteService' with 'MoveGroupExecuteTrajectoryAction' in move_group.launch
[ WARN] [1565429864.768311492]: Interactive marker 'EE:goal_r_wrist_roll_link' contains unnormalized quaternions. This warning will only be output once but may be true for others; enable DEBUG messages for ros.rviz.quaternions to see more details.

```
先は長い

# 追記2019/08/13
### 'head_mount_link' is not known to URDF. Cannot disable collisons.のworning
```~/catkin_ws/src/moveit_pr2/pr2_moveit_config/launch/planning_context.launch
<param if="$(arg load_robot_description)" name="robot_description" command="$(find xacro)/xacro '$(find pr2_description)/robots/pr2.urdf.xacro' --inorder" />
```
ここの部分を以下に変更(kinectのurdfを有効化?)

```~/catkin_ws/src/moveit_pr2/pr2_moveit_config/launch/planning_context.launch
<param if="$(arg load_robot_description)" name="robot_description" command="$(find xacro)/xacro '$(find pr2_description)/robots/pr2.urdf.xacro' KINECT1:='true' --inorder" />
```
すると、```'head_mount_link' is not known to URDF. Cannot disable collisons.```のエラーは解消された。[1]
あまりxacroファイルの中身を変更したくなかったのでパラメータを与える形にした。

### The root link base_footprint has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.のworning
```
roscd pr2_description
sudo vim urdf/base_v0/base.urdf.xacro
```
そして

```base.urdf.xacro
    <link name="${name}_footprint">
      <inertial>
        <mass value="1.0" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                 iyy="0.01" iyz="0.0" izz="0.01" />
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.01 0.01 0.01" />
        </geometry>
        
        <material name="White" />
      </visual>
      <collision>
        <!-- represent base collision with a simple rectangular model, positioned by base_size_z s.t. top
             surface of the collision box matches the top surface of the PR2 base -->
        <origin xyz="0 0 ${0.051+base_collision_size_z/2}" rpy="0 0 0" />
        <geometry>
          <box size="0.001 0.001 0.001" />
        </geometry>
      </collision>
    </link>
```
イナーシャの部分をコメントアウト

```base.urdf.xacro
    <link name="${name}_footprint">
      <!--
      <inertial>
        <mass value="1.0" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                 iyy="0.01" iyz="0.0" izz="0.01" />
      </inertial>
      -->
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.01 0.01 0.01" />
        </geometry>
        
        <material name="White" />
      </visual>
      <collision>
        <!-- represent base collision with a simple rectangular model, positioned by base_size_z s.t. top
             surface of the collision box matches the top surface of the PR2 base -->
        <origin xyz="0 0 ${0.051+base_collision_size_z/2}" rpy="0 0 0" />
        <geometry>
          <box size="0.001 0.001 0.001" />
        </geometry>
      </collision>
    </link>
```
多分、製作者もdummylinkのつもりでfootprint linkを作っていると判断した。よってイナーシャも考えなくても良いかなと(あとはオドメトリ関連か?)[2]
~~本来であればdummylinkを追加すればよいのだが、追加すると他のことで怒られるのでめんどくさくなった。~~
###TIFFFieldWithTag: Internal error
モデルの視覚化に関するエラーでロボットをシミュレートするときにはあまり問題はない。

```
[ WARN] [1565664107.508820196]: The STL file 'package://pr2_description/meshes/sensors/kinect_prosilica_v0/115x100_swept_back--coarse.STL' is malformed. It starts with the word 'solid', indicating that it's an ASCII STL file, but it does not contain the word 'endsolid' so it is either a malformed ASCII STL file or it is actually a binary STL file. Trying to interpret it as a binary STL file instead.
```
上記のWARNINGは[ここ](https://github.com/ros-industrial/robotiq/issues/84)を参照にすると解決した。
具体的には該当のSTLファイルをmeshlabで開き、保存しなおした。
###パッケージの追加
[ここ](https://github.com/ros-planning/moveit/issues/1535)に従って環境を整えてみる。

- sbpl_geometry_utils
- moveit_planners_sbpl
- pr2_common

pr2_commonを追加する際に追加したもの

- ivcon
- convex_decomposition


#参考文献
[1]  https://gist.github.com/davetcoleman/4256083
[2] https://answers.ros.org/question/192817/error-msg-the-root-link_base-has-an-inertia-specified-in-the-urdf-but-kdl/