FootstepPlannerNode.cpp

Go to the documentation of this file.

// SVN $HeadURL: https://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/src/FootstepPlannerNode.cpp $
// SVN $Id: FootstepPlannerNode.cpp 3298 2012-09-28 11:37:38Z hornunga@informatik.uni-freiburg.de $

/*
 * A footstep planner for humanoid robots
 *
 * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
 * http://www.ros.org/wiki/footstep_planner
 *
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <bipedRobin_footstep_planner/FootstepPlannerNode.h>
#include <tf/transform_listener.h>

//Erweiterung um neuen Service zum setzen der Startposition fuer dynamische Planung

namespace footstep_planner
{

        FootstepPlannerNodeNew::FootstepPlannerNodeNew()
        {
                ros::NodeHandle nh;     

                // provide callbacks to interact with the footstep planner:
                ivGridMapSub = nh.subscribe<nav_msgs::OccupancyGrid>("map", 1, &FootstepPlanner::mapCallback, &ivFootstepPlanner);
                ivGoalPoseSub = nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, &FootstepPlanner::goalPoseCallback, &ivFootstepPlanner);
                ivStartPoseSub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1, &FootstepPlanner::startPoseCallback, &ivFootstepPlanner);

                // service:
                ivFootstepPlanService = nh.advertiseService("plan_footsteps", &FootstepPlanner::planService, &ivFootstepPlanner);
                // neuer Service zum setzen der Startposition durch die Fuss Positionen
                initialsteps_srv = nh.advertiseService("initialSteps_srv", &FootstepPlannerNodeNew::initialStepsCallback, this);        
                //starten des Planners  
                ivFootstepPlanner.plan();
                
        }

        FootstepPlannerNodeNew::~FootstepPlannerNodeNew()
        {}
        
        //Service Callback zum setzen der Fuss Positionen
        bool    FootstepPlannerNodeNew::initialStepsCallback(bipedRobin_msgs::InitialStepsService::Request &req, bipedRobin_msgs::InitialStepsService::Response &res) {  
        
                double x, y, yaw, pitch, roll;

                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);    

                x = req.left.pose.position.x;
                y = req.left.pose.position.y;

                State left = State(x,y,yaw,LEFT); //State für linken Fuss

                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);        

                x = req.right.pose.position.x;
                y = req.right.pose.position.y;

                State right = State(x,y,yaw,RIGHT); //State für rechten Fuss           
                ivFootstepPlanner.setStart(left, right); //Setzen der Fuss Positionen   im Planner
                return true;
        }
        
}