SBPLPlanner2D.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Armin Hornung, University of Freiburg
00003  *
00004  * Redistribution and use in source and binary forms, with or without
00005  * modification, are permitted provided that the following conditions are met:
00006  *
00007  *     * Redistributions of source code must retain the above copyright
00008  *       notice, this list of conditions and the following disclaimer.
00009  *     * Redistributions in binary form must reproduce the above copyright
00010  *       notice, this list of conditions and the following disclaimer in the
00011  *       documentation and/or other materials provided with the distribution.
00012  *     * Neither the name of the University of Freiburg nor the names of its
00013  *       contributors may be used to endorse or promote products derived from
00014  *       this software without specific prior written permission.
00015  *
00016  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00017  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00018  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00019  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00020  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00021  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00022  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00023  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00024  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00025  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026  * POSSIBILITY OF SUCH DAMAGE.
00027  */
00028 
00029 #include "humanoid_planner_2d/SBPLPlanner2D.h"
00030 
00031 SBPLPlanner2D::SBPLPlanner2D()
00032   : nh_(),
00033   start_received_(false), goal_received_(false)
00034     //m_wayPointDistance(0.2)
00035 {
00036         // private NodeHandle for parameters:
00037         ros::NodeHandle nh_private("~");
00038   nh_private.param("planner_type", planner_type_, std::string("ARAPlanner"));
00039   nh_private.param("search_until_first_solution", search_until_first_solution_, false);
00040   nh_private.param("allocated_time", allocated_time_, 7.0);
00041   nh_private.param("forward_search", forward_search_, false);
00042   nh_private.param("initial_epsilon", initial_epsilon_, 3.0);
00043   nh_private.param("robot_radius", robot_radius_, 0.1);
00044 
00045   path_pub_ = nh_.advertise<nav_msgs::Path>("path", 0);
00046 
00047   // subscriptions in SBPLPlanner2DNode
00048 }
00049 
00050 SBPLPlanner2D::~SBPLPlanner2D() {
00051 
00052 }
00053 
00054 void SBPLPlanner2D::goalCallback(const geometry_msgs::PoseStampedConstPtr& goal_pose){
00055   // set goal:
00056   goal_pose_ = goal_pose->pose;
00057   goal_received_ = true;
00058   ROS_DEBUG("Received goal: %f %f", goal_pose_.position.x, goal_pose_.position.y);
00059 
00060   if (goal_pose->header.frame_id != map_->getFrameID()){
00061     ROS_WARN("Goal pose frame id \"%s\" different from map frame id \"%s\"", goal_pose->header.frame_id.c_str(), map_->getFrameID().c_str());
00062   }
00063   
00064   // planning?
00065   if (start_received_)
00066     plan();
00067 }
00068 
00069 void SBPLPlanner2D::startCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& start_pose){
00070   // set goal:
00071   start_pose_ = start_pose->pose.pose;
00072   start_received_ = true;
00073   ROS_DEBUG("Received start: %f %f", start_pose_.position.x, start_pose_.position.y);
00074 
00075   if (start_pose->header.frame_id != map_->getFrameID()){
00076     ROS_WARN("Start pose frame id \"%s\" different from map frame id \"%s\"", start_pose->header.frame_id.c_str(), map_->getFrameID().c_str());
00077   }
00078   
00079   // planning?
00080   if (goal_received_)
00081     plan();
00082 }
00083 
00084 bool SBPLPlanner2D::plan(const geometry_msgs::Pose& start, const geometry_msgs::Pose& goal){
00085   start_pose_ = start;
00086   goal_pose_ = goal;
00087 
00088   start_received_ = true;
00089   goal_received_ = true;
00090 
00091   return plan();
00092 }
00093 
00094 bool SBPLPlanner2D::plan(){
00095   path_.poses.clear();
00096 
00097   if (!map_){
00098     ROS_ERROR("Map not set");
00099     return false;
00100   }
00101   unsigned start_x, start_y, goal_x, goal_y;
00102   if (!map_->worldToMap(start_pose_.position.x, start_pose_.position.y, start_x, start_y)){
00103     ROS_ERROR("Start coordinates out of map bounds");
00104     return false;
00105   }
00106   if (!map_->worldToMap(goal_pose_.position.x, goal_pose_.position.y, goal_x, goal_y)){
00107     ROS_ERROR("Goal coordinates out of map bounds");
00108     return false;
00109   }
00110 
00111   int start_id = planner_environment_->SetStart(start_x, start_y);
00112   int goal_id = planner_environment_->SetGoal(goal_x, goal_y);
00113 
00114   if (start_id < 0 || planner_->set_start(start_id) == 0){
00115     ROS_ERROR("Failed to set start state");
00116     return false;
00117   }
00118 
00119   if (goal_id < 0 || planner_->set_goal(goal_id) == 0){
00120     ROS_ERROR("Failed to set goal state");
00121     return false;
00122   }
00123 
00124   // set planner params:
00125   planner_->set_initialsolution_eps(initial_epsilon_);
00126   planner_->set_search_mode(search_until_first_solution_);
00127   std::vector<int> solution_stateIDs;
00128   int solution_cost;
00129 
00130 
00131   if(planner_->replan(allocated_time_, &solution_stateIDs, &solution_cost))
00132     ROS_INFO("Solution found. Costs: %d;  final eps: %f", solution_cost, planner_->get_final_epsilon());
00133   else{
00134     ROS_INFO("Solution not found");
00135     return false;
00136   }
00137 
00138   // extract / publish path:
00139   path_.poses.reserve(solution_stateIDs.size());
00140   path_.header.frame_id = map_->getFrameID();
00141   path_.header.stamp = ros::Time::now();
00142 
00143   geometry_msgs::PoseStamped pose;
00144   pose.header = path_.header;
00145   for (size_t i = 0; i < solution_stateIDs.size(); i++) {
00146     int mx, my;
00147     planner_environment_->GetCoordFromState(solution_stateIDs[i], mx, my);
00148     //ROS_INFO("p: %d - [%d %d]", solution_stateIDs[i], mx, my);
00149     double wx,wy;
00150     map_->mapToWorld(mx,my,wx,wy);
00151 
00152 
00153     pose.pose.position.x = wx;
00154     pose.pose.position.y = wy;
00155     pose.pose.position.z = 0.0;
00156     path_.poses.push_back(pose);
00157   }
00158 
00159   path_pub_.publish(path_);
00160 
00161   return true;
00162 }
00163 
00164 void SBPLPlanner2D::mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_map){
00165   gridmap_2d::GridMap2DPtr map(new gridmap_2d::GridMap2D(occupancy_map));
00166   updateMap(map);
00167 }
00168 
00169 bool SBPLPlanner2D::updateMap(gridmap_2d::GridMap2DPtr map){
00170   planner_environment_.reset(new EnvironmentNAV2D());
00171   planner_environment_->InitializeEnv(int(map->getInfo().width), int(map->getInfo().height), 0, OBSTACLE_COST);
00172   // environment is set up, reset planner:
00173   setPlanner();
00174 
00175   map_ = map;
00176   map_->inflateMap(robot_radius_);
00177 
00178 
00179   for(unsigned int j = 0; j < map->getInfo().height; ++j){
00180     for(unsigned int i = 0; i < map->getInfo().width; ++i){
00181       if (map->isOccupiedAtCell(i,j))
00182         planner_environment_->UpdateCost(i, j, OBSTACLE_COST);
00183       else
00184         planner_environment_->UpdateCost(i,j,0);
00185 
00186     }
00187   }
00188 
00189   ROS_DEBUG("Map set");
00190 
00191   return true;
00192 }
00193 
00194 void SBPLPlanner2D::setPlanner(){
00195   if (planner_type_ == "ARAPlanner"){
00196     planner_.reset(new ARAPlanner(planner_environment_.get(),forward_search_));
00197   } else if (planner_type_ == "ADPlanner"){
00198     planner_.reset(new ADPlanner(planner_environment_.get(),forward_search_));
00199   } else if (planner_type_ == "RSTARPlanner"){
00200     planner_.reset(new RSTARPlanner(planner_environment_.get(),forward_search_));
00201   }
00202 }


humanoid_planner_2d
Author(s): Armin Hornung
autogenerated on Mon Oct 6 2014 00:42:04