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


nao_path_follower
Author(s): Armin Hornung, Stefan Osswald, Daniel Maier
autogenerated on Thu Jun 6 2019 21:07:26