navfn_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Bhaskara Marthi
00036 *********************************************************************/
00037 #include <navfn/navfn_ros.h>
00038 #include <navfn/SetCostmap.h>
00039 #include <navfn/MakeNavPlan.h>
00040 #include <boost/shared_ptr.hpp>
00041 
00042 namespace cm=costmap_2d;
00043 namespace rm=geometry_msgs;
00044 
00045 using std::vector;
00046 using rm::PoseStamped;
00047 using std::string;
00048 using cm::Costmap2D;
00049 using cm::Costmap2DROS;
00050 
00051 typedef boost::shared_ptr<Costmap2D> CmapPtr;
00052 
00053 namespace navfn {
00054 
00055 class NavfnWithLocalCostmap : public NavfnROS
00056 {
00057 public:
00058   NavfnWithLocalCostmap(string name, Costmap2DROS* cmap);
00059   bool setCostmap(SetCostmap::Request& req, SetCostmap::Response& resp);
00060   bool makePlanService(MakeNavPlan::Request& req, MakeNavPlan::Response& resp);
00061 
00062 protected:
00063   virtual void getCostmap(Costmap2D& cmap);
00064 
00065 private:
00066   CmapPtr cmap_;
00067   ros::ServiceServer set_costmap_service_;
00068   ros::ServiceServer make_plan_service_;
00069 };
00070 
00071 
00072 bool NavfnWithLocalCostmap::makePlanService(MakeNavPlan::Request& req, MakeNavPlan::Response& resp)
00073 {
00074   vector<PoseStamped> path;
00075 
00076   req.start.header.frame_id = "/map";
00077   req.goal.header.frame_id = "/map";
00078 
00079   bool success = makePlan(req.start, req.goal, path);
00080 
00081   resp.plan_found = success;
00082   if (success) {
00083     resp.path = path;
00084   }
00085 
00086   return true;
00087 }
00088 
00089 
00090 NavfnWithLocalCostmap::NavfnWithLocalCostmap(string name, Costmap2DROS* cmap) : 
00091   NavfnROS(name, cmap)
00092 {
00093   inscribed_radius_ = 0.0;
00094   circumscribed_radius_ = 0.0;
00095   inflation_radius_ = 0.0;
00096 
00097   ros::NodeHandle private_nh("~");
00098   set_costmap_service_ = private_nh.advertiseService("set_costmap", &NavfnWithLocalCostmap::setCostmap, this);
00099   make_plan_service_ = private_nh.advertiseService("make_plan", &NavfnWithLocalCostmap::makePlanService, this);
00100 }
00101 
00102 
00103 bool NavfnWithLocalCostmap::setCostmap(SetCostmap::Request& req, SetCostmap::Response& resp)
00104 {
00105   cmap_.reset(new Costmap2D(req.width, req.height, 1.0, 0.0, 0.0));
00106   unsigned ind=0;
00107   for (unsigned y=0; y<req.height; y++) 
00108     for (unsigned x=0; x<req.width; x++) 
00109       cmap_->setCost(x, y, req.costs[ind++]);
00110 
00111 
00112   for (unsigned y=0; y<req.height; y++) 
00113     for (unsigned x=0; x<req.width; x++) 
00114       ROS_DEBUG_NAMED ("node", "Cost of %u, %u is %u", x, y, cmap_->getCost(x,y));
00115 
00116   planner_.reset(new NavFn(cmap_->getSizeInCellsX(), cmap_->getSizeInCellsY()));
00117   ROS_DEBUG_STREAM_NAMED ("node", "Resetting planner object to have size " << cmap_->getSizeInCellsX() << ", " << cmap_->getSizeInCellsY());
00118 
00119   return true;
00120 }
00121 
00122 void NavfnWithLocalCostmap::getCostmap(Costmap2D& cmap)
00123 {
00124   cmap = *cmap_;
00125 }
00126 
00127 
00128 } // namespace
00129 
00130 int main (int argc, char** argv)
00131 {
00132   ros::init(argc, argv, "navfn_node");
00133   
00134 
00135   ros::NodeHandle n("~");
00136   tf::TransformListener tf;
00137 
00138   // Set params
00139   n.setParam("dummy_costmap/global_frame", "/map");
00140   n.setParam("dummy_costmap/robot_base_frame", "/map"); // Do this so it doesn't complain about unavailable transform 
00141   n.setParam("dummy_costmap/publish_frequency", 0.0);
00142   n.setParam("dummy_costmap/observation_sources", string(""));
00143   n.setParam("dummy_costmap/static_map", false);
00144 
00145   
00146   Costmap2DROS dummy_costmap("dummy_costmap", tf);
00147   navfn::NavfnWithLocalCostmap navfn("navfn_planner", &dummy_costmap);
00148 
00149   ros::spin();
00150   return 0;
00151 }
00152 
00153 
00154 
00155 
00156 
00157 
00158 
00159 
00160 
00161 
00162 
00163 
00164 
00165 
00166 
00167 


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein
autogenerated on Sat Dec 28 2013 17:14:13