CHOMPプランナーにおけるFailed to call service get_planning_sceneについて

CHOMPをmotoman_projectで使おうとしてますが，うまく動かないので困ってます．

条件

indigo
moveitは自分でビルド．apt-get経由でインストールしたmoveitは消してない（catkin_wsの方から読みにいくため）．
moveitのソースからのビルド方法はここから

方法

まず動作例から moveit_resourcesの中にあるfanuc_moveit_configにあるdemo_chomp.launchを起動します． moveit_resourcesはmoveitの依存パッケージとしてwstool経由でcloneされます．

roslaunch moveit_resources demo_chomp.launch

これは動く． screenshot from 2016-11-17 20 15 32

これをmotoman_projectのchomp-testブランチの中で再現したいと思いfanuc_moveit_configを真似しながらmove_group.launch等を変更しています．

roslaunch motoman_gazebo sia5_with_dhand.launch
roslaunch motoman_moveit sia5_with_dhand_moveit_planning_execution.launch

sia5_with_dhand_moveit_planning_execution.launchにて，

[ WARN] [1479373292.289313752, 2908.422000000]: Failed to call service get_planning_scene, have you launched move_group? at /home/ry0/Workspace/ROS/motoman_ws/src/moveit/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp:480

のようにget_planning_sceneの呼び出しに失敗して，CHOMPプランナーの読み込みができません．

研究とはあまり関係ないのですが，motoman_projectでもOMPL以外のプランナーを動かせるようにしておきたいのでｉｓｓｕｅを立てました．

moveit/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cppの299行目

    try
    {
      move_group_.reset(new moveit::planning_interface::MoveGroup(opt, context_->getFrameManager()->getTFClientPtr(),
                                                                  ros::WallDuration(30, 0))); //←299行目
      if (planning_scene_storage_)
        move_group_->setConstraintsDatabase(ui_->database_host->text().toStdString(), ui_->database_port->value());
    }
    catch (std::runtime_error &ex)
    {
      ROS_ERROR("%s", ex.what());
    }

ros::WallDuration(30, 0)));を60に修正しました．これでタイムアウトせずに正常にCHOMPプランナーを呼び出すことができました．この時間設定は moveit/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cppの480行目

[ WARN] [1479373292.289313752, 2908.422000000]: Failed to call service get_planning_scene, have you launched move_group? at /home/ry0/Workspace/ROS/motoman_ws/src/moveit/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp:480

と moveit/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cppの208行目

[ERROR] [1479894852.011495790, 61.761000000]: Unable to connect to move_group action server 'move_group' within allotted time (30s)

に影響しています．（特に下のエラーについて）これを修正するとなぜうまく行くのかは調査中ですが，ロボットのモデルが複雑すぎて読み込みに時間がかかり過ぎているせいなのかな〜と思ってます．

これで一応，このmotoman_projectはOMPL以外のプランナーであるCHOMPとSTOMPを読み込むことができるようになったので，もう少しメンテナンスしてからmasterにマージしたいと思います．

masterブランチにてOMPL、CHOMP、STOMPの3種類のプランナーを試せるようになりました。プランナーの変更はいつもmoveitを立ち上げる末端のlaunchファイルであるsia5_with_ほにゃらら_moveit_planning_execution.launchのパラメーターを変えてください。

Nishida-Lab / motoman_project

CHOMPプランナーにおけるFailed to call service get_planning_sceneについて #78

条件

方法