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


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Mon Oct 6 2014 00:41:41