使用ORBSLAM2进行kineticV2稠密建图，实时转octomap建图以及导航（下）-阿里云开发者社区

之后存在几种方法去实现导航：

1、octomap_server是ROS中的一个基于octomap的功能包。

我在查阅资料的时候，发现所有的介绍、博客等资料都是在介绍其将点云地图转化为基于Octree的OctoMap的功能。由于之前一直在查找三维点云地图转化为二维地图的方法，所以之前试过这个包的三维转换功能后就没有在继续使用，由于之前使用其他方法将二维占据栅格地图生成了，然后准备回过头来再看一下octomap_server的三维概率地图，然后在不经意间就发现了它也有转化为二维地图的功能

首先简单介绍下octomap_server【设置】的安装。

打开一个终端.(ctrl+alt+T)输入下面指令安装octomap.
sudo apt-get install ros-kinetic-octomap-ros #安装octomap
sudo apt-get install ros-kinetic-octomap-msgs
sudo apt-get install ros-kinetic-octomap-server
安装octomap 在 rviz 中的插件
sudo apt-get install ros-kinetic-octomap-rviz-plugins

安装上这个插件以后你可以启动 rviz ,这时候点开Add选项，会多一个octomap_rviz_plugins模组.如下图所示:

其中的OccupancyGrid是显示三维概率地图，也就是octomap地图。OccupancyMap是显示二维占据栅格地图。

可以通过一个launch文件启动octomap_server节点，如下：

其中的param都是可以修改的，具体的修改细节见这里。/octotree_map修改为自己的PointCloud2点云即可。

运行此launch文件会有如下话题：

打开rviz，在里面添加OccupancyGrid，OccupancyMap，Map显示选项,显示话题选择octomap_full或者octomap_binary。

注意：param中的frame_id要和rviz的Fixed Frame一致。

Map:

OccupancyMap:

OccupancyGrid:

对应的参考代码：https://github.com/306327680/PointCloud-to-grid-map【参考注释】

或者北达科他大学( North Dakota State University)cloud_to_map学习代码：https://download.csdn.net/download/sru_alo/12277545

2、使用3D稠密点云图，并使用octomap进行压缩滤除地面信息。

然后通过2D投影生成占据格地图最后利用costmap进行全局和局部路径规划导航实时避障。【参考代码】

然后参考OctoMap中3D-RRT路径规划

#include "ros/ros.h"
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap_ros/conversions.h>
#include <octomap/octomap.h>
#include <message_filters/subscriber.h>
#include "visualization_msgs/Marker.h"
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/base/OptimizationObjective.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
// #include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/rrt/InformedRRTstar.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/config.h>
#include <iostream>
#include "fcl/config.h"
#include "fcl/octree.h"
#include "fcl/traversal/traversal_node_octree.h"
#include "fcl/collision.h"
#include "fcl/broadphase/broadphase.h"
#include "fcl/math/transform.h"
namespace ob = ompl::base;
namespace og = ompl::geometric;
// Declear some global variables
//ROS publishers
ros::Publisher vis_pub;
ros::Publisher traj_pub;
class planner {
public:
  void setStart(double x, double y, double z)
  {
    ob::ScopedState<ob::SE3StateSpace> start(space);
    start->setXYZ(x,y,z);
    start->as<ob::SO3StateSpace::StateType>(1)->setIdentity();
    pdef->clearStartStates();
    pdef->addStartState(start);
  }
  void setGoal(double x, double y, double z)
  {
    ob::ScopedState<ob::SE3StateSpace> goal(space);
    goal->setXYZ(x,y,z);
    goal->as<ob::SO3StateSpace::StateType>(1)->setIdentity();
    pdef->clearGoal();
    pdef->setGoalState(goal);
    std::cout << "goal set to: " << x << " " << y << " " << z << std::endl;
  }
  void updateMap(std::shared_ptr<fcl::CollisionGeometry> map)
  {
    tree_obj = map;
  }
  // Constructor
  planner(void)
  {
    //四旋翼的障碍物几何形状
    Quadcopter = std::shared_ptr<fcl::CollisionGeometry>(new fcl::Box(0.8, 0.8, 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,0);
    bounds.setHigh(2,3);
    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();
    // start.random();
    goal->setXYZ(0,0,0);
    goal->as<ob::SO3StateSpace::StateType>(1)->setIdentity();
    // goal.random();
      // 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));
    std::cout << "Initialized: " << std::endl;
  }
  // Destructor
  ~planner()
  {
  }
  void replan(void)
  {
    std::cout << "Total Points:" << path_smooth->getStateCount () << std::endl;
    if(path_smooth->getStateCount () <= 2)
      plan();
    else
    {
      for (std::size_t idx = 0; idx < path_smooth->getStateCount (); idx++)
      {
        if(!replan_flag)
          replan_flag = !isStateValid(path_smooth->getState(idx));
        else
          break;
      }
      if(replan_flag)
        plan();
      else
        std::cout << "Replanning not required" << std::endl;
    }
  }
  void plan(void)
  {
      // create a planner for the defined space
    og::InformedRRTstar* rrt = new og::InformedRRTstar(si);
    //设置rrt的参数range
    rrt->setRange(0.2);
    ob::PlannerPtr plan(rrt);
      // set the problem we are trying to solve for the planner
    plan->setProblemDefinition(pdef);
      // perform setup steps for the planner
    plan->setup();
      // print the settings for this space
    si->printSettings(std::cout);
    std::cout << "problem setting\n";
      // print the problem settings
    pdef->print(std::cout);
      // attempt to solve the problem within one second of planning time
    ob::PlannerStatus solved = plan->solve(1);
    if (solved)
    {
          // get the goal representation from the problem definition (not the same as the goal state)
          // and inquire about the found path
      std::cout << "Found solution:" << std::endl;
      ob::PathPtr path = pdef->getSolutionPath();
      og::PathGeometric* pth = pdef->getSolutionPath()->as<og::PathGeometric>();
      pth->printAsMatrix(std::cout);
          // print the path to screen
          // path->print(std::cout);
      nav_msgs::Path msg;
      msg.header.stamp = ros::Time::now();
      msg.header.frame_id = "map";
      for (std::size_t path_idx = 0; path_idx < pth->getStateCount (); path_idx++)
      {
        const ob::SE3StateSpace::StateType *se3state = pth->getState(path_idx)->as<ob::SE3StateSpace::StateType>();
              // extract the first component of the state and cast it to what we expect
        const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
              // extract the second component of the state and cast it to what we expect
        const ob::SO3StateSpace::StateType *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
        geometry_msgs::PoseStamped pose;
//        pose.header.frame_id = "/world"
        pose.pose.position.x = pos->values[0];
        pose.pose.position.y = pos->values[1];
        pose.pose.position.z = pos->values[2];
        pose.pose.orientation.x = rot->x;
        pose.pose.orientation.y = rot->y;
        pose.pose.orientation.z = rot->z;
        pose.pose.orientation.w = rot->w;
        msg.poses.push_back(pose);
      }
      traj_pub.publish(msg);
          //Path smoothing using bspline
      //B样条曲线优化
      og::PathSimplifier* pathBSpline = new og::PathSimplifier(si);
      path_smooth = new og::PathGeometric(dynamic_cast<const og::PathGeometric&>(*pdef->getSolutionPath()));
      pathBSpline->smoothBSpline(*path_smooth,3);
      // std::cout << "Smoothed Path" << std::endl;
      // path_smooth.print(std::cout);
      //Publish path as markers
      nav_msgs::Path smooth_msg;
      smooth_msg.header.stamp = ros::Time::now();
      smooth_msg.header.frame_id = "map";
      for (std::size_t idx = 0; idx < path_smooth->getStateCount (); idx++)
      {
                  // cast the abstract state type to the type we expect
        const ob::SE3StateSpace::StateType *se3state = path_smooth->getState(idx)->as<ob::SE3StateSpace::StateType>();
              // extract the first component of the state and cast it to what we expect
        const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
              // extract the second component of the state and cast it to what we expect
        const ob::SO3StateSpace::StateType *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
        geometry_msgs::PoseStamped point;
//        pose.header.frame_id = "/world"
        point.pose.position.x = pos->values[0];
        point.pose.position.y = pos->values[1];
        point.pose.position.z = pos->values[2];
        point.pose.orientation.x = rot->x;
        point.pose.orientation.y = rot->y;
        point.pose.orientation.z = rot->z;
        point.pose.orientation.w = rot->w;
        smooth_msg.poses.push_back(point);
        std::cout << "Published marker: " << idx << std::endl;
      }
      vis_pub.publish(smooth_msg);
      // ros::Duration(0.1).sleep();
      // Clear memory
      pdef->clearSolutionPaths();
      replan_flag = false;
    }
    else
      std::cout << "No solution found" << std::endl;
  }
private:
  // construct the state space we are planning in
  ob::StateSpacePtr space;
  // construct an instance of  space information from this state space
  ob::SpaceInformationPtr si;
  // create a problem instance
  ob::ProblemDefinitionPtr pdef;
  og::PathGeometric* path_smooth;
  bool replan_flag = false;
  std::shared_ptr<fcl::CollisionGeometry> Quadcopter;
  std::shared_ptr<fcl::CollisionGeometry> tree_obj;
  bool isStateValid(const ob::State *state)
  {
      // cast the abstract state type to the type we expect
    const ob::SE3StateSpace::StateType *se3state = state->as<ob::SE3StateSpace::StateType>();
      // extract the first component of the state and cast it to what we expect
    const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
      // extract the second component of the state and cast it to what we expect
    const ob::SO3StateSpace::StateType *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
    fcl::CollisionObject treeObj((tree_obj));
    fcl::CollisionObject aircraftObject(Quadcopter);
      // check validity of state defined by pos & rot
    fcl::Vec3f translation(pos->values[0],pos->values[1],pos->values[2]);
    fcl::Quaternion3f rotation(rot->w, rot->x, rot->y, rot->z);
    aircraftObject.setTransform(rotation, translation);
    fcl::CollisionRequest requestType(1,false,1,false);
    fcl::CollisionResult collisionResult;
    fcl::collide(&aircraftObject, &treeObj, requestType, collisionResult);
    return(!collisionResult.isCollision());
  }
  // Returns a structure representing the optimization objective to use
  // for optimal motion planning. This method returns an objective which
  // attempts to minimize the length in configuration space of computed
  // paths.
  ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si)
  {
    ob::OptimizationObjectivePtr obj(new ob::PathLengthOptimizationObjective(si));
    // obj->setCostThreshold(ob::Cost(1.51));
    return obj;
  }
  ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si)
  {
    ob::OptimizationObjectivePtr obj(new ob::PathLengthOptimizationObjective(si));
    obj->setCostToGoHeuristic(&ob::goalRegionCostToGo);
    return obj;
  }
};
void octomapCallback(const octomap_msgs::Octomap::ConstPtr &msg, planner* planner_ptr)
{
    //loading octree from binary
  // const std::string filename = "/home/xiaopeng/dense.bt";
  // octomap::OcTree temp_tree(0.1);
  // temp_tree.readBinary(filename);
  // fcl::OcTree* tree = new fcl::OcTree(std::shared_ptr<const octomap::OcTree>(&temp_tree));
  // convert octree to collision object
  octomap::OcTree* tree_oct = dynamic_cast<octomap::OcTree*>(octomap_msgs::msgToMap(*msg));
  fcl::OcTree* tree = new fcl::OcTree(std::shared_ptr<const octomap::OcTree>(tree_oct));
  // Update the octree used for collision checking
  planner_ptr->updateMap(std::shared_ptr<fcl::CollisionGeometry>(tree));
  planner_ptr->replan();
}
void odomCb(const nav_msgs::Odometry::ConstPtr &msg, planner* planner_ptr)
{
  planner_ptr->setStart(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
}
void startCb(const geometry_msgs::PointStamped::ConstPtr &msg, planner* planner_ptr)
{
  planner_ptr->setStart(msg->point.x, msg->point.y, msg->point.z);
}
void goalCb(const geometry_msgs::PointStamped::ConstPtr &msg, planner* planner_ptr)
{
  planner_ptr->setGoal(msg->point.x, msg->point.y, msg->point.z);
  planner_ptr->plan();
}
int main(int argc, char **argv)
{
  ros::init(argc, argv, "octomap_planner");
  ros::NodeHandle n;
  planner planner_object;
  ros::Subscriber octree_sub = n.subscribe<octomap_msgs::Octomap>("/octomap_binary", 1, boost::bind(&octomapCallback, _1, &planner_object));
  // ros::Subscriber odom_sub = n.subscribe<nav_msgs::Odometry>("/rovio/odometry", 1, boost::bind(&odomCb, _1, &planner_object));
  ros::Subscriber goal_sub = n.subscribe<geometry_msgs::PointStamped>("/goal/clicked_point", 1, boost::bind(&goalCb, _1, &planner_object));
  ros::Subscriber start_sub = n.subscribe<geometry_msgs::PointStamped>("/start/clicked_point", 1, boost::bind(&startCb, _1, &planner_object));
//  vis_pub = n.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
  vis_pub = n.advertise<nav_msgs::Path>( "visualization_marker", 0 );
//  traj_pub = n.advertise<trajectory_msgs::MultiDOFJointTrajectory>("waypoints",1);
  traj_pub = n.advertise<nav_msgs::Path>("waypoints",1);
  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
  ros::spin();
  return 0;
}