T-RRT，RRTstar等のOptimization Objectiveが設定できるプランナーについて

[Ry0]

問題点

本家のmoveitリポジトリのｉｓsｕｅにも書いているんですが，どうやらOptimization Objectiveの設定がmoveit側から意図したものに設定できず困っています．
moveit/moveit#358

環境

• Ubuntu 14.04
• ROS Indigo
• motoman_project内のmotoman

このコミットf321cf5で
motoman_moveit/config/sia5_with_dhand/ompl_planning.yaml
等の装備ごとのompl_planning.yamlを読みに行くようにしています（いままではデフォルトのyamlしか呼んで行ってなかった）．

状況

このチュートリアルにあるようにyamlファイルにoptimization_objectiveの設定ができるようになりました．

OMPL Interface
OMPL Optimization Objective Tutorial
http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/ompl_interface_tutorial.html

しかし， TRRTクラスの親クラスであるompl::base::Plannerのメンバー変数pdef_に上手に反映できてないようです．RRTstarも同じ状況．

 93 void ompl::geometric::TRRT::setup()
 94 {
 95     Planner::setup();
 96     tools::SelfConfig selfConfig(si_, getName());
 97 
 98     if (!pdef_->hasOptimizationObjective())
 99     {
100         OMPL_INFORM("%s: No optimization objective specified.  Defaulting to mechanical work minimization.", getName().c_str());
101         opt_.reset(new base::MechanicalWorkOptimizationObjective(si_));
102     }

  104 void ompl::geometric::RRTstar::setup()
  105 {
  106     Planner::setup();
  107     tools::SelfConfig sc(si_, getName());
  108     sc.configurePlannerRange(maxDistance_);
  109     if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
  110     {
  111         OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
  112     }
  113 
  114     if (!nn_)
  115         nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
  116     nn_->setDistanceFunction(std::bind(&RRTstar::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
  117 
  118     // Setup optimization objective
  119     //
  120     // If no optimization objective was specified, then default to
  121     // optimizing path length as computed by the distance() function
  122     // in the state space.
  123     if (pdef_)
  124     {
  125         if (pdef_->hasOptimizationObjective())
  126             opt_ = pdef_->getOptimizationObjective();
  127         else
  128         {
  129             OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
  130             opt_.reset(new base::PathLengthOptimizationObjective(si_));
  131 
  132             // Store the new objective in the problem def'n
  133             pdef_->setOptimizationObjective(opt_);
  134         }
  135     }

T-RRTとRRTstarでpdef_の扱いが少し違います．
OMPL1.0.0のT-RRTではpdef_がヌルポインタの状況で98行目からのif文に入り，elseの方に入るので
コアダンプします．

  103     else
  104         opt_ = pdef_->getOptimizationObjective();

一方，RRTstarでは

  136     else
  137     {
  138         OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
  139         setup_ = false;
  140     }

となるので，ompl_planning.yamlで設定したOptimization Objectiveではないものの，動作はします．
これをどうにかしたいと思います！！
がんばります．．．

[Ry0]

根本的な解決ではありませんが，

Source Install For OMPL Within Catkin Workspace
https://github.com/ros-planning/moveit_planners/blob/kinetic-devel/ompl/doc/tutorial.rst#source-install-for-ompl-within-catkin-workspace

ここにOMPLをcatkin_ws内で自前ビルドする方法が書いてあったのでメモしておきます．
自前でビルドするのでデバッグ用のprintfも設置可能ですし，pdef_の箇所を無理やり修正することもできます（最悪Optimization ObjectiveもOMPL側で設定してあげることもできる）．

まずはcatkin_toolsの導入．

sudo apt-get install python-catkin-tools

OMPLのダウンロード．Indigoが対応するバージョンに合わせてダウンロードするバージョンはOMPL 1.0.0．

cd catkin_ws/src/
git clone https://github.com/ompl/ompl.git
cd ompl
git checkout refs/tags/1.0.0
git checkout -b 1.0.0
wget https://raw.githubusercontent.com/ros-gbp/ompl-release/debian/kinetic/xenial/ompl/package.xml

cd catkin_ws
catkin build

今までcatkin_makeを使っていた場合は，一旦srcとbuildを消す．
これでコンパイルします！
moveitをインストールする際の依存パッケージであるros-indigo-omplがインストールされている状況でも，
自前ビルドしたほうのOMPLが先に読まれているっぽいです．

もう少しがんばります．