00001 /* 00002 * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 00003 * 00004 * Redistribution and use in source and binary forms, with or without 00005 * modification, are permitted provided that the following conditions 00006 * are met: 00007 * 00008 * 1. Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * 00011 * 2. Redistributions in binary form must reproduce the above 00012 * copyright notice, this list of conditions and the following 00013 * disclaimer in the documentation and/or other materials provided 00014 * with the distribution. 00015 * 00016 * 3. Neither the name of the copyright holder nor the names of its 00017 * contributors may be used to endorse or promote products derived 00018 * from this software without specific prior written permission. 00019 * 00020 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 * POSSIBILITY OF SUCH DAMAGE. 00032 * 00033 * wrapper_global_planner.cpp 00034 * 00035 * authors: 00036 * Sebastian Pütz <spuetz@uni-osnabrueck.de> 00037 * Jorge Santos Simón <santos@magazino.eu> 00038 * 00039 */ 00040 00041 #include "nav_core_wrapper/wrapper_global_planner.h" 00042 00043 namespace mbf_nav_core_wrapper 00044 { 00045 00046 uint32_t WrapperGlobalPlanner::makePlan(const geometry_msgs::PoseStamped &start, 00047 const geometry_msgs::PoseStamped &goal, 00048 double tolerance, 00049 std::vector<geometry_msgs::PoseStamped> &plan, 00050 double &cost, 00051 std::string &message) 00052 { 00053 #if ROS_VERSION_MINIMUM(1, 12, 0) // if current ros version is >= 1.12.0 00054 // Kinetic and beyond 00055 bool success = nav_core_plugin_->makePlan(start, goal, plan, cost); 00056 #else 00057 // Indigo 00058 bool success = nav_core_plugin_->makePlan(start, goal, plan); 00059 cost = 0; 00060 #endif 00061 message = success ? "Plan found" : "Planner failed"; 00062 return success ? 0 : 50; // SUCCESS | FAILURE 00063 } 00064 00065 bool WrapperGlobalPlanner::cancel() 00066 { 00067 return false; 00068 } 00069 00070 void WrapperGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros) 00071 { 00072 nav_core_plugin_->initialize(name, costmap_ros); 00073 } 00074 00075 WrapperGlobalPlanner::WrapperGlobalPlanner(boost::shared_ptr<nav_core::BaseGlobalPlanner> plugin) 00076 : nav_core_plugin_(plugin) 00077 {} 00078 00079 WrapperGlobalPlanner::~WrapperGlobalPlanner() 00080 {} 00081 00082 }; /* namespace mbf_abstract_core */