footstep_planner_walls.cpp
Go to the documentation of this file.
00001 // SVN $HeadURL$
00002 // SVN $Id$
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   {
00044     ros::NodeHandle privateNh("~");
00045     // params:
00046     privateNh.param("footstep_wall_dist", ivFootstepWallDist, 0.15);
00047 
00048     // provide callbacks to interact with the footstep planner:
00049     ivGridMapSub = ivNh.subscribe<nav_msgs::OccupancyGrid>("map", 1, &FootstepPlannerWallsNode::mapCallback, this);
00050     ivGoalPoseSub = ivNh.subscribe<geometry_msgs::PoseStamped>("goal", 1, &FootstepPlanner::goalPoseCallback, &ivFootstepPlanner);
00051     ivStartPoseSub = ivNh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1, &FootstepPlanner::startPoseCallback, &ivFootstepPlanner);
00052 
00053     // service:
00054     ivFootstepPlanService = ivNh.advertiseService("plan_footsteps", &FootstepPlanner::planService, &ivFootstepPlanner);
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>(
00068       "map_walls", 1, &FootstepPlannerWallsNode::wallMapCallback, this);
00069   }
00070 
00071   void wallMapCallback(const nav_msgs::OccupancyGridConstPtr& occupancyMap)
00072   {
00073     ROS_INFO("Wall / Obstacle map received");
00074     assert(ivGridMap);
00075     GridMap2DPtr wallMap(new GridMap2D(occupancyMap));
00076 
00077     GridMap2DPtr enlargedWallMap(new GridMap2D(occupancyMap));
00078     cv::Mat binaryMap =  (enlargedWallMap->distanceMap() > ivFootstepWallDist);
00079     bitwise_and(binaryMap, ivGridMap->binaryMap(), binaryMap);
00080 
00081     enlargedWallMap->setMap(binaryMap);
00082 
00083     ivFootstepPlanner.updateMap(enlargedWallMap);
00084   }
00085 
00086 protected:
00087   ros::NodeHandle ivNh;
00088   footstep_planner::FootstepPlanner ivFootstepPlanner;
00089   GridMap2DPtr ivGridMap;
00090   double ivFootstepWallDist;
00091   ros::Subscriber ivGoalPoseSub, ivGridMapSub, ivWallMapSub, ivStartPoseSub, ivRobotPoseSub;
00092   ros::ServiceServer ivFootstepPlanService;
00093 };
00094 
00095 
00096 int main(int argc, char** argv)
00097 {
00098   ros::init(argc, argv, "footstep_planner");
00099 
00100   FootstepPlannerWallsNode planner;
00101 
00102   ros::spin();
00103 
00104   return 0;
00105 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Tue Oct 15 2013 10:06:51