/*********************************************************************
*
*?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（）???????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個文件信息