improve octomap

[Pine-Pine]

• Depthカメラで見えないものはoctomapからも消す
• 計算リソースの消費削減
  • 床・天井付近や遠方はoctomapの生成対象から除外する
  • Depthカメラの画素を圧縮してoctomap生成処理を行う
• 台車移動時に遠方のoctomapは削除する

[jsupratman13]

追記： octomapを任意のエリア (bounding box) のみに生成するように修正．bounding boxの位置は以下のoccupancy_*パラメータで提議する：

• occupancy_frame : 生成するbounding boxの原点とするframe_id. TFに存在するframe_idであること
• occupancy_min_x, occupancy_max_x, occupancy_max_y occupancy_min_y occupancy_max_z occupancy_min_z: occupancy_frameを原点としたbounding boxの大きさ

実機で動作確認できたらdraft解除します．

[jsupratman13]

@nyxrobotics 実機で動作確認取れましたが，ロボットアーム本体のoctomapが除去されない現象が見えましたが，何かこころあたりありますか？

[nyxrobotics]

self_filterがcollisionのオフセット情報を含んでいないため処理されるメッシュの位置がずれているのが原因だと思っているのですがデバッグ情報が可視化できておらず確証がありません https://github.com/sbgisen/moveit/blob/322728ee63c5b2cf8f9bfebe81cab97cd4996d20/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp#L237-L244

このaddMeshesはリンク原点にメッシュ原点が来る動作をしており、link->getCollisionOriginTransforms();で取れる「リンク原点からメッシュ原点のオフセット」が考慮されていません depth_self_filter_nodeletはデバッグ用にdepth imageが出るのでわかりやすいです この点をなんとかしようとして作業していたのが下記ですがaddMeshがいろいろな場所で使われており追いきれませんでした 参考にしてください https://github.com/sbgisen/moveit/commit/6efb0299a05aa81c8ee820c3bd6a797b3c0bbeec

[nyxrobotics]

mapからoctomap_frameへの変換も必要そうです（参考：下記はlink->collisionの変換が足りない）

mesh_filter::MeshHandle DepthImageOctomapUpdater::excludeShape(const shapes::ShapeConstPtr& shape)
{
  geometry_msgs::TransformStamped tf_world2octomap;
  Eigen::Isometry3d isometry_world2octomap = Eigen::Isometry3d::Identity();
  try
  {
    tf_world2octomap = tf_buffer_->lookupTransform(monitor_->getMapFrame(), monitor_->getOctomapFrame(), ros::Time(0));
    isometry_world2octomap = tf2::transformToEigen(tf_world2octomap);
  }
  catch (tf2::TransformException& ex)
  {
    isometry_world2octomap = Eigen::Isometry3d::Identity();
  }

  mesh_filter::MeshHandle h = 0;
  if (mesh_filter_)
  {
    if (shape->type == shapes::MESH)
      h = mesh_filter_->addMesh(static_cast<const shapes::Mesh&>(*shape), isometry_world2octomap);
    else
    {
      std::unique_ptr<shapes::Mesh> m(shapes::createMeshFromShape(shape.get()));
      if (m)
        h = mesh_filter_->addMesh(*m, isometry_world2octomap);
    }
  }
  else
    ROS_ERROR_NAMED(LOGNAME, "Mesh filter not yet initialized!");
  return h;
}

[jsupratman13]

一応アームキャリブレーションしたURDFを確認してみましたが，キャリブレーション前と変わらずself-meshできていない部分があります．

[jsupratman13]

一旦mesh filterのコミットを外し issueを立てました (#15)