RRT 随机森林全局路径规划

作为ROS系统全局路径规划的插件，编译可直接使用，取代原代码自带的全局路径规划包

/*********************************************************************
*
* Software License Agreement （BSD License）
*
*  Copyright （c） 2008 Willow Garage Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms with or without
*  modification are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  “AS IS“ AND ANY EXPRESS OR IMPLIED WARRANTIES INCLUDING BUT NOT
*  LIMITED TO THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT INDIRECT
*  INCIDENTAL SPECIAL EXEMPLARY OR CONSEQUENTIAL DAMAGES （INCLUDING
*  BUT NOT LIMITED TO PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE DATA OR PROFITS; OR BUSINESS INTERRUPTION） HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY WHETHER IN CONTRACT STRICT
*  LIABILITY OR TORT （INCLUDING NEGLIGENCE OR OTHERWISE） ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include 
#include 
#include 

using namespace std;
using namespace costmap_2d;

namespace rrt_star {
  CostmapModel::CostmapModel（const Costmap2D& ma） : costmap_（ma） {}

  double CostmapModel::footprintCost（const geometry_msgs::Point& position const std::vector& footprint
      double inscribed_radius double circumscribed_radius）{

    //used to put things into grid coordinates
    unsigned int cell_x cell_y;

    //get the cell coord of the center point of the robot
    if（!costmap_.worldToMap（position.x position.y cell_x cell_y））
      return 254.0;
    unsigned char cost = costmap_.getCost（cell_x cell_y）;
    //if number of points in the footprint is less than 3 we‘ll just assume a circular robot
    if（footprint.size（） < 3）{
      unsigned char cost = costmap_.getCost（cell_x cell_y）;
      //if（cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE）
      if（cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE）
        return 254.0;
      else if（cost == NO_INFORMATION）
        return -1.0;
      return cost;
    }

    //now we really have to l

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-06-08 12:26  RRT-Plugin-master\
     文件        7079  2018-06-08 12:26  RRT-Plugin-master\CMakeLists.txt
     目录           0  2018-06-08 12:26  RRT-Plugin-master\include\
     目录           0  2018-06-08 12:26  RRT-Plugin-master\include\rrt_star\
     文件        4337  2018-06-08 12:26  RRT-Plugin-master\include\rrt_star\costmap_model.h
     文件        4765  2018-06-08 12:26  RRT-Plugin-master\include\rrt_star\line_iterator.h
     文件        2324  2018-06-08 12:26  RRT-Plugin-master\include\rrt_star\planar_laser_scan.h
     文件        3173  2018-06-08 12:26  RRT-Plugin-master\include\rrt_star\rrt.h
     文件        5108  2018-06-08 12:26  RRT-Plugin-master\include\rrt_star\world_model.h
     目录           0  2018-06-08 12:26  RRT-Plugin-master\launch\
     文件        1783  2018-06-08 12:26  RRT-Plugin-master\launch\move_base.launch
     文件        2029  2018-06-08 12:26  RRT-Plugin-master\launch\navigation_stack.launch
     文件         126  2018-06-08 12:26  RRT-Plugin-master\launch\pcl.launch
     文件         797  2018-06-08 12:26  RRT-Plugin-master\launch\rrt.launch
     目录           0  2018-06-08 12:26  RRT-Plugin-master\map\
     文件         373  2018-06-08 12:26  RRT-Plugin-master\map\maze.png
     文件         108  2018-06-08 12:26  RRT-Plugin-master\map\maze.yaml
     文件        2047  2018-06-08 12:26  RRT-Plugin-master\package.xml
     目录           0  2018-06-08 12:26  RRT-Plugin-master\params\
     文件        1370  2018-06-08 12:26  RRT-Plugin-master\params\costmap.yaml
     目录           0  2018-06-08 12:26  RRT-Plugin-master\params\nav_params\
     文件         634  2018-06-08 12:26  RRT-Plugin-master\params\nav_params\costmap_common.yaml
     文件        1553  2018-06-08 12:26  RRT-Plugin-master\params\nav_params\costmap_converter_params.yaml
     文件         222  2018-06-08 12:26  RRT-Plugin-master\params\nav_params\costmap_global.yaml
     文件         196  2018-06-08 12:26  RRT-Plugin-master\params\nav_params\costmap_local.yaml
     文件         120  2018-06-08 12:26  RRT-Plugin-master\params\nav_params\plan.yaml
     文件        1867  2018-06-08 12:26  RRT-Plugin-master\params\nav_params\teb_local_planner_params.yaml
     文件          72  2018-06-08 12:26  RRT-Plugin-master\params\plan.yaml
     文件        4872  2018-06-08 12:26  RRT-Plugin-master\params\rrt_rviz.rviz
     文件         292  2018-06-08 12:26  RRT-Plugin-master\rrt_plugin.xml
     目录           0  2018-06-08 12:26  RRT-Plugin-master\src\
............此处省略3个文件信息