FootstepPlannerNode.cpp
Go to the documentation of this file.
00001 // SVN $HeadURL: https://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/src/FootstepPlannerNode.cpp $
00002 // SVN $Id: FootstepPlannerNode.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 <bipedRobin_footstep_planner/FootstepPlannerNode.h>
00025 #include <tf/transform_listener.h>
00026 
00027 //Erweiterung um neuen Service zum setzen der Startposition fuer dynamische Planung
00028 
00029 namespace footstep_planner
00030 {
00031 
00032         FootstepPlannerNodeNew::FootstepPlannerNodeNew()
00033         {
00034                 ros::NodeHandle nh;     
00035 
00036                 // provide callbacks to interact with the footstep planner:
00037                 ivGridMapSub = nh.subscribe<nav_msgs::OccupancyGrid>("map", 1, &FootstepPlanner::mapCallback, &ivFootstepPlanner);
00038                 ivGoalPoseSub = nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, &FootstepPlanner::goalPoseCallback, &ivFootstepPlanner);
00039                 ivStartPoseSub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1, &FootstepPlanner::startPoseCallback, &ivFootstepPlanner);
00040 
00041                 // service:
00042                 ivFootstepPlanService = nh.advertiseService("plan_footsteps", &FootstepPlanner::planService, &ivFootstepPlanner);
00043                 // neuer Service zum setzen der Startposition durch die Fuss Positionen
00044                 initialsteps_srv = nh.advertiseService("initialSteps_srv", &FootstepPlannerNodeNew::initialStepsCallback, this);        
00045                 //starten des Planners  
00046                 ivFootstepPlanner.plan();
00047                 
00048         }
00049 
00050         FootstepPlannerNodeNew::~FootstepPlannerNodeNew()
00051         {}
00052         
00053         //Service Callback zum setzen der Fuss Positionen
00054         bool    FootstepPlannerNodeNew::initialStepsCallback(bipedRobin_msgs::InitialStepsService::Request &req, bipedRobin_msgs::InitialStepsService::Response &res) {  
00055         
00056                 double x, y, yaw, pitch, roll;
00057 
00058                 btMatrix3x3(btQuaternion(req.left.pose.orientation.x, req.left.pose.orientation.y, req.left.pose.orientation.z, req.left.pose.orientation.w)).getEulerYPR(yaw, pitch, roll);    
00059 
00060                 x = req.left.pose.position.x;
00061                 y = req.left.pose.position.y;
00062 
00063                 State left = State(x,y,yaw,LEFT); //State für linken Fuss
00064 
00065                 btMatrix3x3(btQuaternion(req.right.pose.orientation.x, req.right.pose.orientation.y, req.right.pose.orientation.z, req.right.pose.orientation.w)).getEulerYPR(yaw, pitch, roll);        
00066 
00067                 x = req.right.pose.position.x;
00068                 y = req.right.pose.position.y;
00069 
00070                 State right = State(x,y,yaw,RIGHT); //State für rechten Fuss           
00071                 ivFootstepPlanner.setStart(left, right); //Setzen der Fuss Positionen   im Planner
00072                 return true;
00073         }
00074         
00075 }


biped_robin_footstep_planner
Author(s): Johannes Mayr, Johannes Mayr
autogenerated on Mon Jan 6 2014 11:09:35