DJI TelloでOMPLを用いた自律飛行ドローンのナビゲーション - 7.1. 状態空間環境のセットアップ

DJI Tello等ドローンの自律飛行を実現するには、3D環境でナビゲーション経路を計画としてOMPL（Open Motion Planning Library）を試してみます。

# 目次
1. [OMPLのインストール](https://qiita.com/k-koh/items/4f7e441d1225e6580f82)
2. [OMPLの基礎](https://qiita.com/k-koh/items/9933cd29599c859cac45)
3. [OMPL.appの入門](https://qiita.com/k-koh/items/fe1f05313045ed729c8b)
4. [OMPLの使用](https://qiita.com/k-koh/items/3a8b372537aa29e17863)
5. DJI Telloの使用
    1. [DJI Telloのフロントカメラのcamera_calibration](https://qiita.com/k-koh/items/094631646931ea5bc2bb)
    2. [Visual SLAM ORB_SLAM2 用のカメラキャリブレーションYamlファイルの作成](https://qiita.com/k-koh/items/b524fc175198e701bdff)
    3. [DJI Telloのカメラ でVisual-SLAMのORB-SLAM2 を動かしてみた](https://qiita.com/k-koh/items/98b5f079f784b22dcd01)
6. navigationスタックの使い方
    1. [Turtlebot3 NavigationをGazebo Simulationで動かしてみた](https://qiita.com/k-koh/items/1a4fbc175ca37be6206b)
    2. [Turtlebot3 Navigationでmap_fileの代わりにSLAMを使用](https://qiita.com/k-koh/items/1ec9097e4afedd0b43ca)
    3. [ARdroneのシミュレーターをGazeboで動かす](https://qiita.com/k-koh/items/feccbaee7c11c43c3ab4)
    4. [AirSimのシミュレーターを動かす](https://qiita.com/k-koh/items/52122fb5e96336ac8d33)
    5. [octomapのインストール](https://qiita.com/k-koh/items/c9a3a87e043934456842)
    6. [単眼カメラでORB_SLAM2から3D point clouodの生成](https://qiita.com/k-koh/items/1e069c9dda43b85e2fa0)
    7. [point_cloud2から3D octomapの生成](https://qiita.com/k-koh/items/01e34f353487eb8a6e26)
7. OMPLで3D 経路計画
    1. [状態空間環境のセットアップ](https://qiita.com/k-koh/items/e19ac08bb0a16975081e)　← いまココ

## 動作環境
- Ubuntu 18.04
- ROS Melodic
- Tello EDU

## 状態空間環境のセットアップ

```c++
#include "ros/ros.h"
#include "visualization_msgs/Marker.h"
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <message_filters/subscriber.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap_ros/conversions.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

#include <ompl/base/OptimizationObjective.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/spaces/SE3StateSpace.h>
// #include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/InformedRRTstar.h>

#include <iostream>
#include <ompl/config.h>

#include "fcl/broadphase/broadphase.h"
#include "fcl/collision.h"
#include "fcl/config.h"
#include "fcl/math/transform.h"
#include "fcl/octree.h"
#include "fcl/traversal/traversal_node_octree.h"

namespace ob = ompl::base;
namespace og = ompl::geometric;

class planner
{
public:
  // Constructor
  planner(void)
  {
    //ドローンの衝突幾何形状
    Quadcopter =
        std::shared_ptr<fcl::CollisionGeometry>(new fcl::Box(0.5, 0.5, 0.3));
    //解像度パラメータ
    fcl::OcTree* tree = new fcl::OcTree(
        std::shared_ptr<const octomap::OcTree>(new octomap::OcTree(0.15)));
    tree_obj = std::shared_ptr<fcl::CollisionGeometry>(tree);

    //経路計画の状態空間
    space = ob::StateSpacePtr(new ob::SE3StateSpace());

    // create a start state
    ob::ScopedState<ob::SE3StateSpace> start(space);

    // create a goal state
    ob::ScopedState<ob::SE3StateSpace> goal(space);

    // set the bounds for the R^3 part of SE(3)
    ob::RealVectorBounds bounds(3);
    bounds.setLow(0, -5);
    bounds.setHigh(0, 5);
    bounds.setLow(1, -5);
    bounds.setHigh(1, 5);
    bounds.setLow(2, -3);
    bounds.setHigh(2, 2);
    space->as<ob::SE3StateSpace>()->setBounds(bounds);

    // construct an instance of  space information from this state space
    si = ob::SpaceInformationPtr(new ob::SpaceInformation(space));

    start->setXYZ(0, 0, 0);
    start->as<ob::SO3StateSpace::StateType>(1)->setIdentity();

    goal->setXYZ(0, 0, 0);
    goal->as<ob::SO3StateSpace::StateType>(1)->setIdentity();

    // set state validity checking for this space
    si->setStateValidityChecker(
        std::bind(&planner::isStateValid, this, std::placeholders::_1));

    // create a problem instance
    pdef = ob::ProblemDefinitionPtr(new ob::ProblemDefinition(si));

    // set the start and goal states
    pdef->setStartAndGoalStates(start, goal);

    // set Optimizattion objective
    pdef->setOptimizationObjective(planner::getPathLengthObjWithCostToGo(si));
  } // end of Constructor

}; // end of planner
```

### OMPLで経路計画を実行

OMPLで経路計画を実行のYouTube動画

[![OMPLで経路計画を実行](http://img.youtube.com/vi/haAlmmXKHhA/0.jpg)](http://www.youtube.com/watch?v=haAlmmXKHhA)


Prev: 6.7. [point_cloud2から3D octomapの生成](https://qiita.com/k-koh/items/01e34f353487eb8a6e26)