nao_path_follower.cpp
Go to the documentation of this file.
00001 // SVN $HeadURL$
00002 // SVN $Id$
00003 
00004 /*
00005  *
00006  * Copyright 2011 Armin Hornung & Stefan Osswald, University of Freiburg
00007  * http://www.ros.org/wiki/nao
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00034 #include <ros/ros.h>
00035 #include <tf/transform_datatypes.h>
00036 #include <tf/transform_listener.h>
00037 #include <geometry_msgs/Transform.h>
00038 #include <std_msgs/Bool.h>
00039 #include <std_srvs/Empty.h>
00040 #include <cmath>
00041 #include <geometry_msgs/Pose2D.h>
00042 #include <nav_msgs/Path.h>
00043 #include <geometry_msgs/PoseStamped.h>
00044 #include <move_base_msgs/MoveBaseAction.h>
00045 #include <nao_msgs/FollowPathAction.h>
00046 #include <actionlib/server/simple_action_server.h>
00047 #include <angles/angles.h>
00048 #include <assert.h>
00049 
00050 class PathFollower
00051 {
00052 public:
00053         PathFollower();
00054         ~PathFollower();
00055         void goalActionCB(const move_base_msgs::MoveBaseGoalConstPtr &goal);
00056         void pathActionCB(const nao_msgs::FollowPathGoalConstPtr &goal);
00057         void goalCB(const geometry_msgs::PoseStampedConstPtr& goal);
00058         void pathCB(const nav_msgs::PathConstPtr& goal);
00059         bool getRobotPose( tf::StampedTransform & globalToBase, const std::string & global_frame_id);
00060 
00061 private:
00062         bool getNextTarget(const nav_msgs::Path & path, const tf::Pose & robotPose,  const std::vector<geometry_msgs::PoseStamped>::const_iterator & currentPathPoseIt, tf::Stamped<tf::Transform> & targetPose, std::vector< geometry_msgs::PoseStamped>::const_iterator & targetPathPoseIt);
00063         void stopWalk();
00064         void setVelocity(const tf::Transform& relTarget);
00065         void publishTargetPoseVis(const tf::Stamped<tf::Pose>& targetPose);
00066         void footContactCallback(const std_msgs::BoolConstPtr& contact);
00067 
00068         ros::NodeHandle nh;
00069         ros::NodeHandle privateNh;
00070         ros::Subscriber m_simpleGoalSub, m_simplePathSub;
00071         ros::Publisher m_targetPub, m_velPub, m_actionGoalPub, m_visPub, m_actionPathPub;
00072         tf::TransformListener m_tfListener;
00073         actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> m_walkGoalServer;
00074         actionlib::SimpleActionServer<nao_msgs::FollowPathAction> m_walkPathServer;
00075         ros::ServiceClient m_inhibitWalkClient, m_uninhibitWalkClient;
00076         ros::Subscriber m_footContactSub;
00077 
00078         std::string m_baseFrameId;     
00079         double m_controllerFreq;       
00080         double m_targetDistThres;      
00081         double m_targetAngThres;       
00082         bool m_isJoystickInhibited;    
00083         bool m_useFootContactProtection;  
00084         bool m_footContact;            
00085         
00086         // Parameters for the velocity controller
00087         bool m_useVelocityController;  
00088         double m_maxVelFractionX;      
00089         double m_maxVelFractionY;      
00090         double m_maxVelFractionYaw;    
00091         double m_stepFreq;             
00092         double m_stepFactor;           
00093         const double m_maxVelXY;       
00094         const double m_maxVelYaw;      
00095         const double m_minStepFreq;    
00096         const double m_maxStepFreq;    
00097         double m_thresholdFar;         
00098         double m_thresholdRotate;      
00099         double m_thresholdDampXY;      
00100         double m_thresholdDampYaw;     
00101         // for path following
00102         double m_pathNextTargetDistance;     
00103 };
00104 
00105 PathFollower::PathFollower()
00106   : privateNh("~"),
00107     m_walkGoalServer(nh, "walk_target", boost::bind(&PathFollower::goalActionCB, this, _1), false),
00108     m_walkPathServer(nh, "walk_path", boost::bind(&PathFollower::pathActionCB, this, _1), false),
00109     m_baseFrameId("base_footprint"), m_controllerFreq(10),
00110     m_targetDistThres(0.05), m_targetAngThres(angles::from_degrees(5.)),
00111     m_isJoystickInhibited(false), 
00112     m_useFootContactProtection(true), m_footContact(true),
00113     m_useVelocityController(false),
00114     m_maxVelFractionX(0.7), m_maxVelFractionY(0.7), m_maxVelFractionYaw(0.7), m_stepFreq(0.5),
00115     m_maxVelXY(0.0952), m_maxVelYaw(angles::from_degrees(47.6)), m_minStepFreq(1.667), m_maxStepFreq(2.381),    
00116     m_thresholdFar(0.20), m_thresholdRotate(angles::from_degrees(45.)),
00117     m_thresholdDampXY(0.20), m_thresholdDampYaw(angles::from_degrees(30.)), m_pathNextTargetDistance(0.1)
00118 {
00119 
00120         privateNh.param("base_frame_id", m_baseFrameId, m_baseFrameId);
00121         m_baseFrameId = m_tfListener.resolve(m_baseFrameId);
00122         privateNh.param("controller_frequency", m_controllerFreq, m_controllerFreq);
00123         privateNh.param("target_distance_threshold", m_targetDistThres, m_targetDistThres);
00124         privateNh.param("target_angle_threshold", m_targetAngThres, m_targetAngThres);
00125         privateNh.param("max_vel_x", m_maxVelFractionX, m_maxVelFractionX);
00126         privateNh.param("max_vel_y", m_maxVelFractionY, m_maxVelFractionY);
00127         privateNh.param("max_vel_yaw", m_maxVelFractionYaw, m_maxVelFractionYaw);
00128         privateNh.param("step_freq", m_stepFreq, m_stepFreq);
00129         privateNh.param("use_vel_ctrl", m_useVelocityController, m_useVelocityController);
00130         privateNh.param("use_foot_contact_protection", m_useFootContactProtection, m_useFootContactProtection);
00131         privateNh.param("path_next_target_distance", m_pathNextTargetDistance, m_pathNextTargetDistance);
00132         privateNh.param("threshold_damp_xy", m_thresholdDampXY, m_thresholdDampXY);
00133         ROS_INFO("Param controller_frequency: %f",m_controllerFreq);
00134         ROS_INFO("Param target_distance_threshold: %f",m_targetDistThres);
00135         ROS_INFO("Param target_angle_threshold: %f",m_targetAngThres);
00136         ROS_INFO("Param max_vel_x: %f, max_vel_y: %f, max_vel_yaw: %f",m_maxVelFractionX,m_maxVelFractionY,m_maxVelFractionYaw);
00137         ROS_INFO("Param step_freq: %f",m_stepFreq);
00138         ROS_INFO("Param use_vel_ctrl: %d",m_useVelocityController);
00139         ROS_INFO("Param use_foot_contact_protection: %d",m_useFootContactProtection);
00140 
00141         m_stepFactor = ((m_maxStepFreq - m_minStepFreq) * m_stepFreq + m_minStepFreq) / m_maxStepFreq;
00142         ROS_INFO("Using step factor of %4.2f",m_stepFactor);
00143 
00144         if(m_useVelocityController) {
00145             ROS_WARN("Velocity controller is not working yet!");
00146             ROS_INFO("Using velocity controller");
00147             m_velPub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
00148         } else {
00149             ROS_INFO("Using target pose controller");
00150             m_targetPub = nh.advertise<geometry_msgs::Pose2D>("cmd_pose", 10);
00151         }
00152 
00153         m_actionGoalPub = nh.advertise<move_base_msgs::MoveBaseActionGoal>("walk_target/goal", 1);
00154         m_actionPathPub = nh.advertise<nao_msgs::FollowPathActionGoal>("walk_path/goal", 1);
00155         //we'll provide a mechanism for some people to send goals or paths as PoseStamped messages over a topic
00156         //they won't get any useful information back about its status, but this is useful for tools
00157         //like nav_view and rviz
00158         m_simpleGoalSub = nh.subscribe<geometry_msgs::PoseStamped>("walk_target_simple/goal", 1, boost::bind(&PathFollower::goalCB, this, _1));
00159         m_simplePathSub = nh.subscribe<nav_msgs::Path>("walk_path_simple/goal", 1, boost::bind(&PathFollower::pathCB, this, _1));
00160         
00161         if(m_useFootContactProtection) {
00162             m_footContactSub = nh.subscribe<std_msgs::Bool>("foot_contact", 1, &PathFollower::footContactCallback, this);
00163         }
00164 
00165         m_inhibitWalkClient = nh.serviceClient<std_srvs::Empty>("inhibit_walk");
00166         m_uninhibitWalkClient = nh.serviceClient<std_srvs::Empty>("uninhibit_walk");
00167 
00168         m_visPub = privateNh.advertise<geometry_msgs::PoseStamped>("target_pose", 1, true);
00169 
00170         // start up action server now:
00171         m_walkGoalServer.start();
00172         m_walkPathServer.start();
00173 }
00174 
00175 PathFollower::~PathFollower(){
00176    std::cerr << "Quitting PathFollower.." << std::endl;
00177    // TODO: stopWalk is not completed if ctrl+c is hit FIX THIS!
00178    stopWalk();
00179    std::cout << ".. stopWalk() called before quitting" << std::endl;
00180 
00181 }
00182 
00187 void PathFollower::publishTargetPoseVis(const tf::Stamped<tf::Pose>& targetPose) {
00188     geometry_msgs::PoseStamped targetPoseMsg;
00189     tf::poseStampedTFToMsg(targetPose, targetPoseMsg);
00190     m_visPub.publish(targetPoseMsg);
00191 }
00192 
00193 void PathFollower::goalCB(const geometry_msgs::PoseStampedConstPtr& goal) {
00194         ROS_DEBUG("In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
00195         move_base_msgs::MoveBaseActionGoal action_goal;
00196         action_goal.header.stamp = ros::Time::now();
00197         action_goal.goal.target_pose = *goal;
00198         m_actionGoalPub.publish(action_goal);
00199 }
00200 
00201 void PathFollower::pathCB(const nav_msgs::PathConstPtr& goal) {
00202    ROS_INFO("Simple Path callback");
00203         ROS_DEBUG("In ROS path callback, wrapping the Path in the action message and re-sending to the server.");
00204         nao_msgs::FollowPathActionGoal action_goal;
00205         action_goal.header = goal->header;
00206         action_goal.goal.path = *goal;
00207         m_actionPathPub.publish(action_goal);
00208 }
00209 
00210 void PathFollower::footContactCallback(const std_msgs::BoolConstPtr& contact) {
00211     m_footContact = contact->data;
00212 }
00213 
00214 
00215 bool hasOrientation(const geometry_msgs::Pose & pose)
00216 {
00217    double eps = 1e-5;
00218    if ( fabs(pose.orientation.x) < eps && fabs(pose.orientation.y) < eps && fabs(pose.orientation.z) < eps && fabs(pose.orientation.w) < eps )
00219       return false;
00220    geometry_msgs::Quaternion q = pose.orientation;
00221    if ( fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w -1 ) > eps)
00222          return false;
00223    return true;
00224          
00225 }
00226 
00227 bool PathFollower::getRobotPose( tf::StampedTransform & globalToBase, const std::string & global_frame_id)
00228 {
00229    try
00230    {
00231       // this causes a segfault when shutting down, move_base also doesn't have it?
00232       //                        m_tfListener.waitForTransform(targetPose.frame_id_, m_baseFrameId, ros::Time(), ros::Duration(0.1));
00233       m_tfListener.lookupTransform(global_frame_id, m_baseFrameId, ros::Time(), globalToBase);
00234       //ROS_INFO("ros::Time() is %f, ros::Time::now() is %f, delta is %f", globalToBase.stamp_.toSec(), ros::Time::now().toSec(), ros::Duration(ros::Time::now() - globalToBase.stamp_).toSec());
00235    }
00236    catch(const tf::TransformException& e)
00237    {
00238       ROS_WARN("Failed to obtain tf to base (%s)", e.what());
00239       return false;
00240    }
00241    //make sure base footprint is on the floor:
00242    globalToBase.getOrigin().setZ(0.0);
00243    double roll, pitch, yaw;
00244    globalToBase.getBasis().getRPY(roll, pitch, yaw);
00245    globalToBase.setRotation(tf::createQuaternionFromYaw(yaw));
00246    return true;
00247 }
00248 
00249 double distance (const tf::Pose & p1, const tf::Pose & p2)
00250 {
00251    return (p1.getOrigin() - p2.getOrigin()).length();
00252 }
00253 
00254 void print_transform(const tf::Transform & t)
00255 {
00256    geometry_msgs::Pose m;
00257    tf::poseTFToMsg(t, m);
00258    std::cout << m;
00259 }
00260 double getYawBetween(const tf::Transform & p1, const tf::Transform & p2)
00261 {
00262    tf::Vector3 o1 = p1.getOrigin();
00263    tf::Vector3 o2 = p2.getOrigin();
00264    return  std::atan2( o2.getY() - o1.getY(), o2.getX() - o1.getX());
00265 }
00266 tf::Quaternion getOrientationBetween(const tf::Transform & p1, const tf::Transform & p2)
00267 {
00268    /*
00269    tf::Quaternion q;
00270    tf::Transform t1 = p1;
00271    t1.setRotation(tf::createQuaternionFromYaw(0.0));
00272    tf::Transform t2 = p2;
00273    t2.setRotation(tf::createQuaternionFromYaw(0.0));
00274    print_transform(t1);
00275    print_transform(t2);
00276    std::cout << std::endl;
00277 
00278    tf::Transform relTarget = t1.inverseTimes(t2);
00279    print_transform(relTarget);
00280    std::cout << std::endl;
00281    relTarget.getBasis().getRPY(roll, pitch, yaw);
00282    */
00283    //ROS_INFO("Computed yaw %f ",  yaw);
00284    double yaw = getYawBetween(p1, p2);
00285    return (tf::createQuaternionFromYaw(yaw));
00286 }
00287 
00288 
00289 typedef std::vector< geometry_msgs::PoseStamped>::const_iterator PathIterator;
00290 
00291 double moveAlongPathByDistance( const PathIterator & start, PathIterator & end, double targetDistance)
00292 {
00293    PathIterator iter = start;
00294    tf::Stamped<tf::Transform> tfPose, tfPose2;
00295    double dist = 0.0;
00296    while(iter+1 != end )
00297    {
00298       tf::poseStampedMsgToTF(*(iter), tfPose);
00299       tf::poseStampedMsgToTF(*(iter+1), tfPose2);
00300       double d = distance( tfPose, tfPose2);
00301       if (dist + d > targetDistance )
00302          break;
00303       dist += d;
00304       ++iter;
00305    }
00306    end = iter;
00307    return dist;
00308 }
00309 
00310 // this function assumes that robotPose is close (< robot_start_path_threshold) to *currentPathPoseIt 
00311 bool PathFollower::getNextTarget(const nav_msgs::Path & path, const tf::Pose & robotPose,  const std::vector<geometry_msgs::PoseStamped>::const_iterator & currentPathPoseIt, tf::Stamped<tf::Transform> & targetPose, std::vector< geometry_msgs::PoseStamped>::const_iterator & targetPathPoseIt)
00312 {  
00313 
00314    // current state in  path unclear: cannot go further, end of path reached
00315    if( path.poses.empty() || currentPathPoseIt == path.poses.end() ) // past-the-end of path
00316    {
00317       ROS_ERROR("Path does not have successors for currentPathPoseIt (i.e. currentPathPoseIt pointing to past-the-end or is empty)");
00318       return true;
00319       // TODO: Throw something
00320    }
00321 
00322    
00323    // 0 successors possible from currentPathPoseIt
00324    if( currentPathPoseIt+1 == path.poses.end() )
00325    {
00326       // this can happen, if we are close to the final goal or someone send us a single goal wrapped as path (not allowed)
00327       // under first assumption it we just keep the current targetPose
00328       ROS_INFO("Goal almost reached! Keeping targetPose. ");
00329       return true;
00330    }
00331    // from here, currentPathPoseIt has at least one successor
00332 
00333    // rewrite
00334    std::vector< geometry_msgs::PoseStamped>::const_iterator iter = currentPathPoseIt+1;
00335    tf::Stamped<tf::Transform> tfPose, tfPose2;
00336    tf::poseStampedMsgToTF( *iter, tfPose);
00337    double dist = distance( robotPose, tfPose);
00338    bool targetIsEndOfPath = true;
00339    double targetDistance = m_pathNextTargetDistance; // target at most x cm ahead of robot
00340    while(iter+1 != path.poses.end() )
00341    {
00342       tf::poseStampedMsgToTF(*iter, tfPose);
00343       tf::poseStampedMsgToTF(*(iter+1), tfPose2);
00344       double d = distance( tfPose, tfPose2);
00345       if (dist + d > targetDistance )
00346       {
00347          targetIsEndOfPath = false;
00348          break;
00349       }
00350       dist += d;
00351       ++iter;
00352    }
00353 
00354    targetPathPoseIt = iter;
00355    tf::poseStampedMsgToTF(*iter, targetPose);
00356    
00357 
00358    // check if poses have orientation
00359    if ( !hasOrientation (iter->pose) )
00360    {
00361       //ROS_WARN("Target has no orientation. Computing orientation from other waypoints.");
00362 
00363       tf::Quaternion orientation;
00364       if( iter==currentPathPoseIt ) // path has only one element so get orientation from robot to poses[0]
00365       {
00366          ROS_ERROR("This case should never happend (anymore)");
00367          orientation = getOrientationBetween( robotPose, targetPose);
00368          assert(0); // produce seg fault // TODO: To remove this later
00369       }
00370       else if (targetIsEndOfPath) // path length >= 2 and iter points to end of path
00371       {
00372          tf::Stamped<tf::Transform> prevPose;
00373          tf::poseStampedMsgToTF( *(iter-1), prevPose );
00374          orientation = getOrientationBetween( prevPose, targetPose);
00375       }
00376       else // iter has at least one succesor
00377       {
00378          // set Orientation so that successor of targetPose are faced
00379          /*
00380          ++iter;
00381          tf::poseStampedMsgToTF( *iter, tfPose );
00382          dist = distance( targetPose, tfPose );
00383          while(iter+1 != path.poses.end() )
00384          {
00385             tf::poseStampedMsgToTF(*(iter), tfPose);
00386             tf::poseStampedMsgToTF(*(iter+1), tfPose2);
00387             double d = distance( tfPose, tfPose2);
00388             if (dist + d > targetDistance )
00389                break;
00390             dist += d;
00391             ++iter;
00392          }
00393          ROS_INFO("Found successor at %4.2f, %4.2f", tfPose.getOrigin().x(), tfPose.getOrigin().y());
00394          */
00395          //std::vector< tf::Stamped<tf::Transform> > poses;
00396          /*
00397          iter = targetPathPoseIt;
00398          double cumDist = 0.0;
00399          double sinSum = 0.0;
00400          double cosSum = 0.0;
00401          // first add robot orientation
00402          double yaw = getYawBetween( robotPose, targetPose); 
00403          sinSum += std::sin(yaw);
00404          cosSum += std::cos(yaw);
00405          ROS_INFO("Found target at %4.2f, %4.2f, %4.2f", targetPose.getOrigin().x(), targetPose.getOrigin().y(), yaw*180.0/M_PI);
00406          for(int i =0; i < 2 ; ++i )
00407          {
00408             PathIterator end = path.poses.end();
00409             cumDist += moveAlongPathByDistance( iter, end, targetDistance);
00410             tf::poseStampedMsgToTF(*(iter), tfPose2);
00411             tf::poseStampedMsgToTF(*(end), tfPose);
00412             double yaw = getYawBetween( tfPose2, tfPose); 
00413             sinSum += std::sin(yaw);
00414             cosSum += std::cos(yaw);
00415             //poses.push_back(tfPose);
00416             iter = end;
00417             ROS_INFO("Found successor at %4.2f, %4.2f, %4.2f", tfPose.getOrigin().x(), tfPose.getOrigin().y(), yaw*180.0/M_PI);
00418             if( iter+1 == path.poses.end() )
00419                break;
00420          }
00421          yaw = std::atan2( sinSum, cosSum );
00422          */
00423          tf::poseStampedMsgToTF(*(targetPathPoseIt+1), tfPose2 );
00424          double oldyaw = getYawBetween( targetPose, tfPose2);
00425          double yaw = oldyaw;
00426          if (targetPathPoseIt-1 != path.poses.begin() && targetPathPoseIt + 2 != path.poses.end() )
00427          {
00428 
00429             tf::poseStampedMsgToTF(*(targetPathPoseIt-2), tfPose);
00430             tf::poseStampedMsgToTF(*(targetPathPoseIt+2), tfPose2 );
00431             double yaw1 = getYawBetween(tfPose, targetPose);
00432             double yaw2 = getYawBetween(targetPose, tfPose2);
00433             yaw = atan2( sin(yaw1)+sin(yaw2), cos(yaw1) + cos(yaw2) );
00434          }
00435          ROS_INFO("Using yaw %4.2f instead of %4.2f (old method)", yaw*180.0/M_PI, oldyaw*180.0/M_PI);
00436          orientation = tf::createQuaternionFromYaw(yaw);
00437 
00438 
00439       }
00440       targetPose.setRotation(orientation);
00441 
00442       //ROS_INFO_STREAM("..computed orientation" << basis << std::endl);
00443    }
00444 
00445    return targetIsEndOfPath;
00446 
00447 }
00448 
00449 
00450 void PathFollower::pathActionCB(const nao_msgs::FollowPathGoalConstPtr &goal){
00451    ROS_INFO("path action starting...");
00452 
00453    nav_msgs::Path path = goal->path;
00454    nao_msgs::FollowPathFeedback feedback;
00455    if( path.poses.empty() )
00456    {
00457       ROS_INFO("Stop requested by sending empty path.");
00458       stopWalk();
00459       m_walkPathServer.setSucceeded(nao_msgs::FollowPathResult(),"Stop succeeed");
00460       return;
00461    }
00462    if(path.poses.size()==1)
00463    {
00464       ROS_INFO("Sending a single goal is not supported (use walk_target instead).");
00465       stopWalk();
00466       m_walkPathServer.setAborted(nao_msgs::FollowPathResult(),"Single goal stop");
00467       return;
00468    }
00469    ros::Rate r(m_controllerFreq);
00470 
00471    ros::Time lastTfSuccessTime = ros::Time::now();
00472    tf::StampedTransform globalToBase;
00473    double tfLookupTimeThresh = 1.0; // TODO: param
00474    std::string global_frame_id = path.header.frame_id;
00475    while (! getRobotPose(globalToBase, global_frame_id) )
00476    {
00477       if ( (ros::Time::now() - lastTfSuccessTime ).toSec() > tfLookupTimeThresh )
00478       {
00479          ROS_ERROR("Could not get robot pose. Stopping robot");
00480          stopWalk();
00481          m_walkPathServer.setAborted(nao_msgs::FollowPathResult(),"Aborted");
00482          return;
00483       }
00484       else
00485       {
00486          r.sleep();
00487          continue;
00488       }
00489    }
00490    //lastTfSuccessTime = globalToBase.stamp_;
00491    lastTfSuccessTime = ros::Time::now();
00492 
00493    // Check if start of path is consistent with current robot pose, otherwise abort
00494    std::vector< geometry_msgs::PoseStamped>::const_iterator currentPathPoseIt, targetPathPoseIt;
00495    currentPathPoseIt = path.poses.begin();
00496    tf::Stamped<tf::Transform> tmp;
00497    tf::poseStampedMsgToTF( *currentPathPoseIt, tmp);
00498    double robot_start_path_threshold = 0.05; // TODO: Param
00499    if (distance(tmp, globalToBase)> robot_start_path_threshold )
00500    {
00501       ROS_ERROR("Robot is too far away from start of plan. aborting");
00502       stopWalk();
00503       m_walkPathServer.setAborted(nao_msgs::FollowPathResult(),"Aborted");
00504       return;
00505    }
00506 
00507 
00508    // This is where Nao is currently walking to (current sub goal)
00509    tf::Stamped<tf::Transform> targetPose;
00510    bool targetIsEndOfPath = getNextTarget(path, globalToBase, currentPathPoseIt, targetPose, targetPathPoseIt );
00511    publishTargetPoseVis(targetPose);
00512    if(!m_isJoystickInhibited) {
00513       std_srvs::Empty e;
00514       if(m_inhibitWalkClient.call(e)) {
00515          ROS_INFO("Joystick inhibited");
00516          m_isJoystickInhibited = true;
00517       } else {
00518          ROS_WARN("Could not inhibit joystick walk");
00519       }
00520    }
00521    
00522    while(ros::ok()){
00523       if (! getRobotPose(globalToBase, global_frame_id) )
00524       {
00525          if ( (ros::Time::now() - lastTfSuccessTime ).toSec() > tfLookupTimeThresh )
00526          {
00527             ROS_ERROR("Could not get robot pose. Stopping robot");
00528             stopWalk();
00529             m_walkPathServer.setAborted(nao_msgs::FollowPathResult(),"Aborted");
00530             return;
00531          }
00532          else
00533          {
00534             r.sleep();
00535             continue;
00536          }
00537       }
00538       //lastTfSuccessTime = globalToBase.stamp_;
00539       lastTfSuccessTime = ros::Time::now();
00540       double dt = (lastTfSuccessTime - globalToBase.stamp_).toSec();
00541       if ( dt > tfLookupTimeThresh )
00542       {
00543          ROS_WARN("TF transforms are behind current clock by %4.2f", dt);
00544          //TODO do something here..
00545       }
00546 
00547       if (m_walkPathServer.isPreemptRequested()){
00548          if(m_walkPathServer.isNewGoalAvailable()){
00549             ROS_INFO("walk_path ActionServer: new goal available");
00550             nao_msgs::FollowPathGoal newGoal = *m_walkPathServer.acceptNewGoal();
00551             path = newGoal.path;
00552             // Check if path is not empty, otherwise abort
00553             if( path.poses.empty() )
00554             {
00555                ROS_INFO("Stop requested by sending empty path.");
00556                stopWalk();
00557                m_walkPathServer.setSucceeded(nao_msgs::FollowPathResult(),"Stop succeeed");
00558                return;
00559             }
00560             /*
00561             if(path.poses.size()==1)
00562             {
00563                ROS_INFO("Sending a single goal is not supported (use walk_target instead).");
00564                stopWalk();
00565                m_walkPathServer.setAborted(nao_msgs::FollowPathResult(),"Single goal stop");
00566                return;
00567             }
00568             */
00569 
00570             // Check if start of path is consistent with current robot pose, otherwise abort
00571             currentPathPoseIt = path.poses.begin();
00572             tf::Stamped<tf::Transform> tmp;
00573             tf::poseStampedMsgToTF( *currentPathPoseIt, tmp);
00574             if (distance(tmp, globalToBase)> robot_start_path_threshold )
00575             {
00576                ROS_ERROR("Robot is too far away from start of plan. aborting");
00577                stopWalk();
00578                m_walkPathServer.setAborted(nao_msgs::FollowPathResult(),"Aborted");
00579                return;
00580             }
00581 
00582             // set targetPose, this will not change until we reached it
00583             targetIsEndOfPath = getNextTarget(path, globalToBase, currentPathPoseIt, targetPose, targetPathPoseIt );
00584             publishTargetPoseVis(targetPose);
00585             if(!m_isJoystickInhibited) {
00586                std_srvs::Empty e;
00587                if(m_inhibitWalkClient.call(e)) {
00588                   ROS_INFO("Joystick inhibited");
00589                   m_isJoystickInhibited = true;
00590                } else {
00591                   ROS_WARN("Could not inhibit joystick walk");
00592                }
00593             }
00594          } else {
00595             ROS_INFO("walk_path ActionServer preempted");
00596             // anything to cleanup?e)
00597             stopWalk();
00598 
00599             m_walkPathServer.setPreempted();
00600             return;
00601          }
00602       }
00603 
00604       if(m_useFootContactProtection && !m_footContact) {
00605          ROS_WARN("Lost foot contact, abort walk");
00606          stopWalk();
00607          m_walkPathServer.setAborted(nao_msgs::FollowPathResult(), "Aborting on the goal because the robot lost foot contact with the ground");
00608          return;
00609       }
00610 
00611 
00612       /*
00613          move_base_msgs::MoveBaseFeedback feedback;
00614          feedback.base_position.header.stamp = globalToBase.stamp_;
00615          feedback.base_position.header.frame_id = globalToBase.frame_id_;
00616          tf::poseTFToMsg(globalToBase, feedback.base_position.pose);
00617          m_walkGoalServer.publishFeedback(feedback);
00618          */
00619 
00620 
00621       double roll, pitch, yaw;
00622       tf::Transform relTarget = globalToBase.inverseTimes(targetPose);
00623       relTarget.getBasis().getRPY(roll, pitch, yaw);
00624 
00625 
00626       // treat last targetPose different from other waypoints on path
00627       if (targetIsEndOfPath && relTarget.getOrigin().length()< m_targetDistThres && std::abs(yaw) < m_targetAngThres)
00628       {
00629          ROS_INFO("Target (%f %f %F) reached", targetPose.getOrigin().x(), targetPose.getOrigin().y(),
00630                tf::getYaw(targetPose.getRotation()));
00631 
00632          stopWalk();
00633 
00634          m_walkPathServer.setSucceeded(nao_msgs::FollowPathResult(), "Target reached");
00635          return;
00636       }
00637       // TODO: Use other thresholds for target != endOfPath
00638       else if ( relTarget.getOrigin().length()< robot_start_path_threshold && std::abs(yaw) < 45*M_PI/180.0)
00639       {
00640          ROS_INFO("Advancing targetPose..");
00641          // update currentPathPoseIt to point to waypoint corresponding to  targetPose
00642          currentPathPoseIt = targetPathPoseIt;
00643          // update targetPose, targetPathPoseIt
00644          targetIsEndOfPath = getNextTarget(path, globalToBase, currentPathPoseIt, targetPose, targetPathPoseIt );
00645          publishTargetPoseVis(targetPose);
00646          // update relTarget
00647          relTarget = globalToBase.inverseTimes(targetPose);
00648       }
00649 
00650       // walk to targetPose (relTarget)
00651       if(m_useVelocityController) {
00652          setVelocity(relTarget);
00653       } else {
00654          geometry_msgs::Pose2D target;
00655          // workaround for NaoQI API (stay on the straight line connection facing towards goal)
00656          if (relTarget.getOrigin().length() > 0.2){
00657             relTarget.setOrigin(0.2* relTarget.getOrigin().normalized());
00658             yaw = atan2(relTarget.getOrigin().y(), relTarget.getOrigin().x());
00659          }
00660          target.x = relTarget.getOrigin().x();
00661          target.y = relTarget.getOrigin().y();
00662          target.theta = yaw;
00663 
00664          m_targetPub.publish(target);
00665       }
00666 
00667 
00668       r.sleep();
00669       //make sure to sleep for the remainder of our cycle time
00670       if(r.cycleTime() > ros::Duration(1 / m_controllerFreq))
00671          ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", m_controllerFreq, r.cycleTime().toSec());
00672 
00673    }
00674 
00675    //if the node is killed then we'll abort and return
00676 
00677    // TODO: stopWalk is not completed if ctrl+c is hit FIX THIS!
00678    stopWalk();
00679    m_walkPathServer.setAborted(nao_msgs::FollowPathResult(), "Aborting on the goal because the node has been killed");
00680 
00681 
00682 
00683    std::cout << ("... path action ending") << std::endl;
00684 }
00685 
00686 
00687 void PathFollower::goalActionCB(const move_base_msgs::MoveBaseGoalConstPtr &goal){
00688         tf::Stamped<tf::Transform> targetPose;
00689         tf::poseStampedMsgToTF(goal->target_pose, targetPose);
00690 
00691         publishTargetPoseVis(targetPose);
00692 
00693         ros::Rate r(m_controllerFreq);
00694 
00695     if(!m_isJoystickInhibited) {
00696         std_srvs::Empty e;
00697         if(m_inhibitWalkClient.call(e)) {
00698             ROS_INFO("Joystick inhibited");
00699             m_isJoystickInhibited = true;
00700         } else {
00701             ROS_WARN("Could not inhibit joystick walk");
00702         }
00703     }
00704 
00705 
00706         while(nh.ok()){
00707                 if (m_walkGoalServer.isPreemptRequested()){
00708                         if(m_walkGoalServer.isNewGoalAvailable()){
00709                                 ROS_INFO("walk_target ActionServer: new goal available");
00710                                 move_base_msgs::MoveBaseGoal newGoal = *m_walkGoalServer.acceptNewGoal();
00711                                 tf::poseStampedMsgToTF(newGoal.target_pose, targetPose);
00712                                 publishTargetPoseVis(targetPose);
00713                                 if(!m_isJoystickInhibited) {
00714                                     std_srvs::Empty e;
00715                                         if(m_inhibitWalkClient.call(e)) {
00716                                             ROS_INFO("Joystick inhibited");
00717                                                 m_isJoystickInhibited = true;
00718                                         } else {
00719                                                 ROS_WARN("Could not inhibit joystick walk");
00720                                         }
00721                                 }
00722                         } else {
00723                                 ROS_INFO("walk_target ActionServer preempted");
00724                                 // anything to cleanup?e)
00725                                 stopWalk();
00726 
00727                                 m_walkGoalServer.setPreempted();
00728                                 return;
00729                         }
00730                 }
00731 
00732                 if(m_useFootContactProtection && !m_footContact) {
00733                     ROS_WARN("Lost foot contact, abort walk");
00734                     stopWalk();
00735                     m_walkGoalServer.setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the robot lost foot contact with the ground");
00736                     return;
00737                 }
00738 
00739 
00740                 //tf::Stamped<tf::Transform> odomToTarget;
00741                 tf::StampedTransform globalToBase;
00742                 try
00743                 {
00744                         // this causes a segfault when shutting down, move_base also doesn't have it?
00745 //                      m_tfListener.waitForTransform(targetPose.frame_id_, m_baseFrameId, ros::Time(), ros::Duration(0.1));
00746                         m_tfListener.lookupTransform(targetPose.frame_id_, m_baseFrameId, ros::Time(), globalToBase);
00747                 }
00748                 catch(const tf::TransformException& e)
00749                 {
00750                         ROS_WARN("Failed to obtain tf to base (%s)", e.what());
00751                         r.sleep();
00752                         continue;
00753                 }
00754                 //make sure base footprint is on the floor:
00755                 globalToBase.getOrigin().setZ(0.0);
00756                 double roll, pitch, yaw;
00757                 globalToBase.getBasis().getRPY(roll, pitch, yaw);
00758                 globalToBase.setRotation(tf::createQuaternionFromYaw(yaw));
00759 
00760                 move_base_msgs::MoveBaseFeedback feedback;
00761                 feedback.base_position.header.stamp = globalToBase.stamp_;
00762                 feedback.base_position.header.frame_id = globalToBase.frame_id_;
00763                 tf::poseTFToMsg(globalToBase, feedback.base_position.pose);
00764                 m_walkGoalServer.publishFeedback(feedback);
00765 
00766 
00767                 tf::Transform relTarget = globalToBase.inverseTimes(targetPose);
00768                 relTarget.getBasis().getRPY(roll, pitch, yaw);
00769 
00770         // target reached:
00771         if (relTarget.getOrigin().length()< m_targetDistThres && std::abs(yaw) < m_targetAngThres){
00772             ROS_INFO("Target (%f %f %F) reached", targetPose.getOrigin().x(), targetPose.getOrigin().y(),
00773                     tf::getYaw(targetPose.getRotation()));
00774 
00775             stopWalk();
00776 
00777             m_walkGoalServer.setSucceeded(move_base_msgs::MoveBaseResult(), "Target reached");
00778             return;
00779         } else {
00780             if(m_useVelocityController) {
00781                 setVelocity(relTarget);
00782             } else {
00783                 geometry_msgs::Pose2D target;
00784                 // workaround for NaoQI API (stay on the straight line connection facing towards goal)
00785                 if (relTarget.getOrigin().length() > 0.2){
00786                     relTarget.setOrigin(0.2* relTarget.getOrigin().normalized());
00787                     yaw = atan2(relTarget.getOrigin().y(), relTarget.getOrigin().x());
00788                 }
00789                 target.x = relTarget.getOrigin().x();
00790                 target.y = relTarget.getOrigin().y();
00791                 target.theta = yaw;
00792 
00793                 m_targetPub.publish(target);
00794             }
00795         }
00796 
00797             r.sleep();
00798             //make sure to sleep for the remainder of our cycle time
00799             if(r.cycleTime() > ros::Duration(1 / m_controllerFreq))
00800               ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", m_controllerFreq, r.cycleTime().toSec());
00801 
00802         }
00803 
00804         //if the node is killed then we'll abort and return
00805     stopWalk();
00806         m_walkGoalServer.setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
00807 
00808 }
00809 
00813 void PathFollower::stopWalk(){
00814         std_srvs::Empty::Request req;
00815         std_srvs::Empty::Response resp;
00816         if (!ros::service::call("stop_walk_srv", req, resp)){
00817            if(m_useVelocityController) {
00818               ROS_WARN("Call to stop_walk_srv Service failed, falling back to velocity=0,0,0");
00819               geometry_msgs::Twist twist;
00820               twist.linear.x = 0.0;
00821               twist.linear.y = 0.0;
00822               twist.angular.z = 0.0;
00823               m_velPub.publish(twist);
00824            }
00825            else
00826            {
00827               ROS_WARN("Call to stop_walk_srv Service failed, falling back to target=0,0,0");
00828               geometry_msgs::Pose2D target;
00829               target.x = 0.0;
00830               target.y = 0.0;
00831               target.theta = 0.0;
00832               m_targetPub.publish(target);
00833            }
00834         }
00835         if(m_isJoystickInhibited) {
00836             std_srvs::Empty e;
00837                 if(m_uninhibitWalkClient.call(e)) {
00838                     ROS_INFO("Joystick uninhibited");
00839                         m_isJoystickInhibited = false;
00840                 } else {
00841                         ROS_WARN("Could not uninhibit joystick walk");
00842                 }
00843         }
00844 }
00845 
00862 void PathFollower::setVelocity(const tf::Transform& relTarget) {
00863         double dx, dy, yaw;
00864         bool rotateOnSpot;
00865     const double dist = relTarget.getOrigin().length();
00866         if(dist > m_thresholdFar) {
00867             // Far away: Orient towards the target point
00868             yaw = atan2(relTarget.getOrigin().y(), relTarget.getOrigin().x());
00869             rotateOnSpot = (std::abs(yaw) > m_thresholdRotate);
00870         } else {
00871             // Near: Orient towards the final angle
00872             yaw = tf::getYaw(relTarget.getRotation());
00873         rotateOnSpot = (dist < m_targetDistThres);
00874         if(dist < m_targetDistThres && std::abs(yaw) < m_targetAngThres) {
00875             geometry_msgs::Twist twist;
00876             twist.linear.x = 0.0;
00877             twist.linear.y = 0.0;
00878             twist.angular.z = 0.0;
00879             m_velPub.publish(twist);
00880             return;
00881         }
00882         }
00883 
00884         // Normalize angle between -180° and +180°
00885         yaw = angles::normalize_angle(yaw);
00886 
00887         // Reduce velocity near target
00888         const double dampXY = std::min(dist / m_thresholdDampXY, 1.0);
00889         const double dampYaw = std::min(std::abs(yaw) / m_thresholdDampYaw, 1.0);
00890 
00891         if(rotateOnSpot) {
00892             dx = 0.0;
00893             dy = 0.0;
00894         } else {
00895             dx = relTarget.getOrigin().x();
00896             dy = relTarget.getOrigin().y();
00897         }
00898 
00899     const double times[] = {
00900         std::abs(dx)  / ((dampXY * m_maxVelFractionX)    * m_maxVelXY  * m_stepFactor),
00901         std::abs(dy)  / ((dampXY * m_maxVelFractionY)    * m_maxVelXY  * m_stepFactor),
00902         std::abs(yaw) / ((dampYaw * m_maxVelFractionYaw) * m_maxVelYaw * m_stepFactor),
00903         1. / m_controllerFreq
00904     };
00905     const double maxtime = *std::max_element(times, times + 4);
00906 
00907     geometry_msgs::Twist twist;
00908     twist.linear.x  = dx  / (maxtime * m_maxVelXY  * m_stepFactor);
00909     twist.linear.y  = dy  / (maxtime * m_maxVelXY  * m_stepFactor);
00910     twist.angular.z = yaw / (maxtime * m_maxVelYaw * m_stepFactor);
00911 
00912     ROS_DEBUG("setVelocity: target %f %f %f --> velocity %f %f %f",
00913           relTarget.getOrigin().x(), relTarget.getOrigin().y(), yaw,
00914           twist.linear.x, twist.linear.y, twist.angular.z);
00915     m_velPub.publish(twist);
00916 }
00917 
00918 int main(int argc, char** argv){
00919    ros::init(argc, argv, "nao_path_follower");
00920    PathFollower follower;
00921 
00922    ros::spin();
00923 
00924    return 0;
00925 }
00926 
00927 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


nao_remote
Author(s): Armin Hornung
autogenerated on Sat Oct 26 2013 11:02:42