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_local_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_local_planner.h" 00042 00043 namespace mbf_nav_core_wrapper 00044 { 00045 00046 uint32_t WrapperLocalPlanner::computeVelocityCommands( 00047 const geometry_msgs::PoseStamped& robot_pose, 00048 const geometry_msgs::TwistStamped& robot_velocity, 00049 geometry_msgs::TwistStamped &cmd_vel, 00050 std::string &message) 00051 { 00052 bool success = nav_core_plugin_->computeVelocityCommands(cmd_vel.twist); 00053 message = success ? "Goal reached" : "Controller failed"; 00054 return success ? 0 : 100; // SUCCESS | FAILURE 00055 } 00056 00057 bool WrapperLocalPlanner::isGoalReached() 00058 { 00059 return nav_core_plugin_->isGoalReached(); 00060 } 00061 00062 bool WrapperLocalPlanner::isGoalReached(double xy_tolerance, double yaw_tolerance) 00063 { 00064 return isGoalReached(); 00065 } 00066 00067 bool WrapperLocalPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) 00068 { 00069 return nav_core_plugin_->setPlan(plan); 00070 } 00071 00072 bool WrapperLocalPlanner::cancel() 00073 { 00074 return false; 00075 } 00076 00077 void WrapperLocalPlanner::initialize(std::string name, 00078 TF *tf, 00079 costmap_2d::Costmap2DROS *costmap_ros) 00080 { 00081 nav_core_plugin_->initialize(name, tf, costmap_ros); 00082 } 00083 00084 WrapperLocalPlanner::WrapperLocalPlanner(boost::shared_ptr<nav_core::BaseLocalPlanner> plugin) 00085 : nav_core_plugin_(plugin) 00086 {} 00087 00088 WrapperLocalPlanner::~WrapperLocalPlanner() 00089 {} 00090 00091 }; /* namespace mbf_abstract_core */