$search
00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/src/footstep_planner_walls.cpp $ 00002 // SVN $Id: footstep_planner_walls.cpp 3298 2012-09-28 11:37:38Z hornunga@informatik.uni-freiburg.de $ 00003 00004 /* 00005 * A footstep planner for humanoid robots 00006 * 00007 * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg 00008 * http://www.ros.org/wiki/footstep_planner 00009 * 00010 * 00011 * This program is free software: you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation, version 3. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU General Public License for more details. 00019 * 00020 * You should have received a copy of the GNU General Public License 00021 * along with this program. If not, see <http://www.gnu.org/licenses/>. 00022 */ 00023 00024 #include <ros/ros.h> 00025 #include <footstep_planner/FootstepPlanner.h> 00026 #include <nav_msgs/OccupancyGrid.h> 00027 #include <gridmap_2d/GridMap2D.h> 00028 00029 00030 using namespace footstep_planner; 00031 using gridmap_2d::GridMap2D; 00032 using gridmap_2d::GridMap2DPtr; 00033 00040 class FootstepPlannerWallsNode { 00041 public: 00042 FootstepPlannerWallsNode(){ 00043 ros::NodeHandle privateNh("~"); 00044 // params: 00045 privateNh.param("footstep_wall_dist", ivFootstepWallDist, 0.15); 00046 00047 // provide callbacks to interact with the footstep planner: 00048 ivGridMapSub = ivNh.subscribe<nav_msgs::OccupancyGrid>("map", 1, &FootstepPlannerWallsNode::mapCallback, this); 00049 ivGoalPoseSub = ivNh.subscribe<geometry_msgs::PoseStamped>("goal", 1, &FootstepPlanner::goalPoseCallback, &ivFootstepPlanner); 00050 ivStartPoseSub = ivNh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1, &FootstepPlanner::startPoseCallback, &ivFootstepPlanner); 00051 00052 // service: 00053 ivFootstepPlanService = ivNh.advertiseService("plan_footsteps", &FootstepPlanner::planService, &ivFootstepPlanner); 00054 00055 } 00056 00057 virtual ~FootstepPlannerWallsNode(){}; 00058 00059 void mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancyMap){ 00060 00061 ROS_INFO("Obstacle map received, now waiting for wall map."); 00062 ivGridMap = GridMap2DPtr(new GridMap2D(occupancyMap)); 00063 // don't set wall => wait for wall map! 00064 //ivFootstepPlanner.setMap(ivGridMap); 00065 00066 // now subscribe to walls, so that they arrive in order: 00067 ivWallMapSub = ivNh.subscribe<nav_msgs::OccupancyGrid>("map_walls", 1, &FootstepPlannerWallsNode::wallMapCallback, this); 00068 } 00069 00070 void wallMapCallback(const nav_msgs::OccupancyGridConstPtr& occupancyMap){ 00071 ROS_INFO("Wall / Obstacle map received"); 00072 assert(ivGridMap); 00073 GridMap2DPtr wallMap(new GridMap2D(occupancyMap)); 00074 00075 GridMap2DPtr enlargedWallMap(new GridMap2D(occupancyMap)); 00076 cv::Mat binaryMap = (enlargedWallMap->distanceMap() > ivFootstepWallDist); 00077 bitwise_and(binaryMap, ivGridMap->binaryMap(), binaryMap); 00078 00079 enlargedWallMap->setMap(binaryMap); 00080 00081 ivFootstepPlanner.updateMap(enlargedWallMap); 00082 00083 } 00084 protected: 00085 ros::NodeHandle ivNh; 00086 footstep_planner::FootstepPlanner ivFootstepPlanner; 00087 GridMap2DPtr ivGridMap; 00088 double ivFootstepWallDist; 00089 ros::Subscriber ivGoalPoseSub, ivGridMapSub, ivWallMapSub, ivStartPoseSub, ivRobotPoseSub; 00090 ros::ServiceServer ivFootstepPlanService; 00091 }; 00092 00093 int main(int argc, char** argv){ 00094 ros::init(argc, argv, "footstep_planner"); 00095 00096 FootstepPlannerWallsNode planner; 00097 00098 ros::spin(); 00099 00100 return 0; 00101 }