Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00046 privateNh.param("footstep_wall_dist", ivFootstepWallDist, 0.15);
00047
00048
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
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
00064
00065
00066
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 }