pose_follower.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2009, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  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
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 #include <nav_msgs/Path.h>
00038 #include <pose_follower/pose_follower.h>
00039 #include <pluginlib/class_list_macros.hpp>
00040 
00041 PLUGINLIB_EXPORT_CLASS(pose_follower::PoseFollower, nav_core::BaseLocalPlanner)
00042 
00043 namespace pose_follower {
00044   PoseFollower::PoseFollower(): tf_(NULL), costmap_ros_(NULL) {}
00045 
00046   void PoseFollower::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros){
00047     tf_ = tf;
00048     costmap_ros_ = costmap_ros;
00049     current_waypoint_ = 0;
00050     goal_reached_time_ = ros::Time::now();
00051     ros::NodeHandle node_private("~/" + name);
00052 
00053     collision_planner_.initialize(name, tf_, costmap_ros_);
00054 
00055     node_private.param("k_trans", K_trans_, 2.0);
00056     node_private.param("k_rot", K_rot_, 2.0);
00057 
00058     //within this distance to the goal, finally rotate to the goal heading (also, we've reached our goal only if we're within this dist)
00059     node_private.param("tolerance_trans", tolerance_trans_, 0.02); 
00060 
00061     //we've reached our goal only if we're within this angular distance
00062     node_private.param("tolerance_rot", tolerance_rot_, 0.04);
00063 
00064     //we've reached our goal only if we're within range for this long and stopped
00065     node_private.param("tolerance_timeout", tolerance_timeout_, 0.5);
00066 
00067     //set this to true if you're using a holonomic robot
00068     node_private.param("holonomic", holonomic_, true);
00069 
00070     //number of samples (scaling factors of our current desired twist) to check the validity of 
00071     node_private.param("samples", samples_, 10);
00072 
00073     //go no faster than this
00074     node_private.param("max_vel_lin", max_vel_lin_, 0.9);
00075     node_private.param("max_vel_th", max_vel_th_, 1.4);
00076 
00077     //minimum velocities to keep from getting stuck
00078     node_private.param("min_vel_lin", min_vel_lin_, 0.1);
00079     node_private.param("min_vel_th", min_vel_th_, 0.0);
00080 
00081     //if we're rotating in place, go at least this fast to avoid getting stuck
00082     node_private.param("min_in_place_vel_th", min_in_place_vel_th_, 0.0);
00083 
00084     //when we're near the end and would be trying to go no faster than this translationally, just rotate in place instead
00085     node_private.param("in_place_trans_vel", in_place_trans_vel_, 0.0);
00086 
00087     //we're "stopped" if we're going slower than these velocities
00088     node_private.param("trans_stopped_velocity", trans_stopped_velocity_, 1e-4);
00089     node_private.param("rot_stopped_velocity", rot_stopped_velocity_, 1e-4);
00090 
00091     //if this is true, we don't care whether we go backwards or forwards
00092     node_private.param("allow_backwards", allow_backwards_, false);
00093 
00094     //if this is true, turn in place to face the new goal instead of arcing toward it
00095     node_private.param("turn_in_place_first", turn_in_place_first_, false);
00096 
00097     //if turn_in_place_first is true, turn in place if our heading is more than this far from facing the goal location
00098     node_private.param("max_heading_diff_before_moving", max_heading_diff_before_moving_, 0.17);
00099 
00100     global_plan_pub_ = node_private.advertise<nav_msgs::Path>("global_plan", 1);
00101 
00102     ros::NodeHandle node;
00103     odom_sub_ = node.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&PoseFollower::odomCallback, this, _1));
00104     vel_pub_ = node.advertise<geometry_msgs::Twist>("cmd_vel", 10);
00105 
00106     ROS_DEBUG("Initialized");
00107   }
00108 
00109   void PoseFollower::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
00110     //we assume that the odometry is published in the frame of the base
00111     boost::mutex::scoped_lock lock(odom_lock_);
00112     base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
00113     base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
00114     base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
00115     ROS_DEBUG("In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
00116         base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
00117   }
00118 
00119   double PoseFollower::headingDiff(double x, double y, double pt_x, double pt_y, double heading)
00120   {
00121     double v1_x = x - pt_x;
00122     double v1_y = y - pt_y;
00123     double v2_x = cos(heading);
00124     double v2_y = sin(heading);
00125 
00126     double perp_dot = v1_x * v2_y - v1_y * v2_x;
00127     double dot = v1_x * v2_x + v1_y * v2_y;
00128 
00129     //get the signed angle
00130     double vector_angle = atan2(perp_dot, dot);
00131 
00132     return -1.0 * vector_angle;
00133   }
00134 
00135   bool PoseFollower::stopped(){
00136     //copy over the odometry information
00137     nav_msgs::Odometry base_odom;
00138     {
00139       boost::mutex::scoped_lock lock(odom_lock_);
00140       base_odom = base_odom_;
00141     }
00142 
00143     return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity_
00144       && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity_
00145       && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity_;
00146   }
00147 
00148   void PoseFollower::publishPlan(const std::vector<geometry_msgs::PoseStamped> &path,
00149                                const ros::Publisher &pub) {
00150     // given an empty path we won't do anything
00151     if (path.empty())
00152       return;
00153 
00154     // create a path message
00155     nav_msgs::Path gui_path;
00156     gui_path.poses.resize(path.size());
00157     gui_path.header.frame_id = path[0].header.frame_id;
00158     gui_path.header.stamp = path[0].header.stamp;
00159 
00160     // Extract the plan in world co-ordinates, we assume the path is all in the same frame
00161     for (unsigned int i = 0; i < path.size(); i++) {
00162       gui_path.poses[i] = path[i];
00163     }
00164     pub.publish(gui_path);
00165   }
00166 
00167   bool PoseFollower::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){
00168     //get the current pose of the robot in the fixed frame
00169     tf::Stamped<tf::Pose> robot_pose;
00170     if(!costmap_ros_->getRobotPose(robot_pose)){
00171       ROS_ERROR("Can't get robot pose");
00172       geometry_msgs::Twist empty_twist;
00173       cmd_vel = empty_twist;
00174       return false;
00175     }
00176 
00177     //we want to compute a velocity command based on our current waypoint
00178     tf::Stamped<tf::Pose> target_pose;
00179     tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
00180 
00181     ROS_DEBUG("PoseFollower: current robot pose %f %f ==> %f", robot_pose.getOrigin().x(), robot_pose.getOrigin().y(), tf::getYaw(robot_pose.getRotation()));
00182     ROS_DEBUG("PoseFollower: target robot pose %f %f ==> %f", target_pose.getOrigin().x(), target_pose.getOrigin().y(), tf::getYaw(target_pose.getRotation()));
00183 
00184     //get the difference between the two poses
00185     geometry_msgs::Twist diff = diff2D(target_pose, robot_pose);
00186     ROS_DEBUG("PoseFollower: diff %f %f ==> %f", diff.linear.x, diff.linear.y, diff.angular.z);
00187 
00188     geometry_msgs::Twist limit_vel = limitTwist(diff);
00189 
00190     geometry_msgs::Twist test_vel = limit_vel;
00191     bool legal_traj = collision_planner_.checkTrajectory(test_vel.linear.x, test_vel.linear.y, test_vel.angular.z, true);
00192 
00193     double scaling_factor = 1.0;
00194     double ds = scaling_factor / samples_;
00195 
00196     //let's make sure that the velocity command is legal... and if not, scale down
00197     if(!legal_traj){
00198       for(int i = 0; i < samples_; ++i){
00199         test_vel.linear.x = limit_vel.linear.x * scaling_factor;
00200         test_vel.linear.y = limit_vel.linear.y * scaling_factor;
00201         test_vel.angular.z = limit_vel.angular.z * scaling_factor;
00202         test_vel = limitTwist(test_vel);
00203         if(collision_planner_.checkTrajectory(test_vel.linear.x, test_vel.linear.y, test_vel.angular.z, false)){
00204           legal_traj = true;
00205           break;
00206         }
00207         scaling_factor -= ds;
00208       }
00209     }
00210 
00211     if(!legal_traj){
00212       ROS_ERROR("Not legal (%.2f, %.2f, %.2f)", limit_vel.linear.x, limit_vel.linear.y, limit_vel.angular.z);
00213       geometry_msgs::Twist empty_twist;
00214       cmd_vel = empty_twist;
00215       return false;
00216     }
00217 
00218     //if it is legal... we'll pass it on
00219     cmd_vel = test_vel;
00220 
00221     bool in_goal_position = false;
00222     while(fabs(diff.linear.x) <= tolerance_trans_ &&
00223           fabs(diff.linear.y) <= tolerance_trans_ &&
00224           fabs(diff.angular.z) <= tolerance_rot_)
00225     {
00226       if(current_waypoint_ < global_plan_.size() - 1)
00227       {
00228         current_waypoint_++;
00229         tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
00230         diff = diff2D(target_pose, robot_pose);
00231       }
00232       else
00233       {
00234         ROS_INFO("Reached goal: %d", current_waypoint_);
00235         in_goal_position = true;
00236         break;
00237       }
00238     }
00239 
00240     //if we're not in the goal position, we need to update time
00241     if(!in_goal_position)
00242       goal_reached_time_ = ros::Time::now();
00243 
00244     //check if we've reached our goal for long enough to succeed
00245     if(goal_reached_time_ + ros::Duration(tolerance_timeout_) < ros::Time::now()){
00246       geometry_msgs::Twist empty_twist;
00247       cmd_vel = empty_twist;
00248     }
00249 
00250     return true;
00251   }
00252 
00253   bool PoseFollower::setPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan){
00254     current_waypoint_ = 0;
00255     goal_reached_time_ = ros::Time::now();
00256     if(!transformGlobalPlan(*tf_, global_plan, *costmap_ros_, costmap_ros_->getGlobalFrameID(), global_plan_)){
00257       ROS_ERROR("Could not transform the global plan to the frame of the controller");
00258       return false;
00259     }
00260 
00261     ROS_DEBUG("global plan size: %lu", global_plan_.size());
00262     publishPlan(global_plan_, global_plan_pub_);
00263     return true;
00264   }
00265 
00266   bool PoseFollower::isGoalReached(){
00267     if(goal_reached_time_ + ros::Duration(tolerance_timeout_) < ros::Time::now() && stopped()){
00268       return true;
00269     }
00270     return false;
00271   }
00272 
00273   geometry_msgs::Twist PoseFollower::diff2D(const tf::Pose& pose1, const tf::Pose& pose2)
00274   {
00275     geometry_msgs::Twist res;
00276     tf::Pose diff = pose2.inverse() * pose1;
00277     res.linear.x = diff.getOrigin().x();
00278     res.linear.y = diff.getOrigin().y();
00279     res.angular.z = tf::getYaw(diff.getRotation());
00280 
00281     if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
00282       return res;
00283 
00284     //in the case that we're not rotating to our goal position and we have a non-holonomic robot
00285     //we'll need to command a rotational velocity that will help us reach our desired heading
00286     
00287     //we want to compute a goal based on the heading difference between our pose and the target
00288     double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(), 
00289         pose2.getOrigin().x(), pose2.getOrigin().y(), tf::getYaw(pose2.getRotation()));
00290 
00291     //we'll also check if we can move more effectively backwards
00292     if (allow_backwards_) 
00293     {
00294       double neg_yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(), 
00295                                         pose2.getOrigin().x(), pose2.getOrigin().y(), M_PI + tf::getYaw(pose2.getRotation()));
00296 
00297       //check if its faster to just back up
00298       if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
00299         ROS_DEBUG("Negative is better: %.2f", neg_yaw_diff);
00300         yaw_diff = neg_yaw_diff;
00301       }
00302     }
00303 
00304     //compute the desired quaterion
00305     tf::Quaternion rot_diff = tf::createQuaternionFromYaw(yaw_diff);
00306     tf::Quaternion rot = pose2.getRotation() * rot_diff;
00307     tf::Pose new_pose = pose1;
00308     new_pose.setRotation(rot);
00309 
00310     diff = pose2.inverse() * new_pose;
00311     res.linear.x = diff.getOrigin().x();
00312     res.linear.y = diff.getOrigin().y();
00313     res.angular.z = tf::getYaw(diff.getRotation());
00314     return res;
00315   }
00316 
00317 
00318   geometry_msgs::Twist PoseFollower::limitTwist(const geometry_msgs::Twist& twist)
00319   {
00320     geometry_msgs::Twist res = twist;
00321     res.linear.x *= K_trans_;
00322     if(!holonomic_)
00323       res.linear.y = 0.0;
00324     else    
00325       res.linear.y *= K_trans_;
00326     res.angular.z *= K_rot_;
00327 
00328     //if turn_in_place_first is true, see if we need to rotate in place to face our goal first
00329     if (turn_in_place_first_ && fabs(twist.angular.z) > max_heading_diff_before_moving_)
00330     {
00331       res.linear.x = 0;
00332       res.linear.y = 0;
00333       if (fabs(res.angular.z) > max_vel_th_) res.angular.z = max_vel_th_ * sign(res.angular.z);
00334       if (fabs(res.angular.z) < min_in_place_vel_th_) res.angular.z = min_in_place_vel_th_ * sign(res.angular.z);
00335       return res;
00336     }
00337 
00338     //make sure to bound things by our velocity limits
00339     double lin_overshoot = sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y) / max_vel_lin_;
00340     double lin_undershoot = min_vel_lin_ / sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y);
00341     if (lin_overshoot > 1.0) 
00342     {
00343       res.linear.x /= lin_overshoot;
00344       res.linear.y /= lin_overshoot;
00345     }
00346 
00347     //we only want to enforce a minimum velocity if we're not rotating in place
00348     if(lin_undershoot > 1.0)
00349     {
00350       res.linear.x *= lin_undershoot;
00351       res.linear.y *= lin_undershoot;
00352     }
00353 
00354     if (fabs(res.angular.z) > max_vel_th_) res.angular.z = max_vel_th_ * sign(res.angular.z);
00355     if (fabs(res.angular.z) < min_vel_th_) res.angular.z = min_vel_th_ * sign(res.angular.z);
00356     if (std::isnan(res.linear.x))
00357         res.linear.x = 0.0;
00358     if (std::isnan(res.linear.y))
00359         res.linear.y = 0.0;
00360 
00361     //we want to check for whether or not we're desired to rotate in place
00362     if(sqrt(twist.linear.x * twist.linear.x + twist.linear.y * twist.linear.y) < in_place_trans_vel_){
00363       if (fabs(res.angular.z) < min_in_place_vel_th_) res.angular.z = min_in_place_vel_th_ * sign(res.angular.z);
00364       res.linear.x = 0.0;
00365       res.linear.y = 0.0;
00366     }
00367 
00368     ROS_DEBUG("Angular command %f", res.angular.z);
00369     return res;
00370   }
00371 
00372   bool PoseFollower::transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan, 
00373       const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
00374       std::vector<geometry_msgs::PoseStamped>& transformed_plan){
00375     const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
00376 
00377     transformed_plan.clear();
00378 
00379     try{
00380       if (global_plan.empty())
00381       {
00382         ROS_ERROR("Recieved plan with zero length");
00383         return false;
00384       }
00385 
00386       tf::StampedTransform transform;
00387       tf.lookupTransform(global_frame, ros::Time(), 
00388           plan_pose.header.frame_id, plan_pose.header.stamp, 
00389           plan_pose.header.frame_id, transform);
00390 
00391       tf::Stamped<tf::Pose> tf_pose;
00392       geometry_msgs::PoseStamped newer_pose;
00393       //now we'll transform until points are outside of our distance threshold
00394       for(unsigned int i = 0; i < global_plan.size(); ++i){
00395         const geometry_msgs::PoseStamped& pose = global_plan[i];
00396         poseStampedMsgToTF(pose, tf_pose);
00397         tf_pose.setData(transform * tf_pose);
00398         tf_pose.stamp_ = transform.stamp_;
00399         tf_pose.frame_id_ = global_frame;
00400         poseStampedTFToMsg(tf_pose, newer_pose);
00401 
00402         transformed_plan.push_back(newer_pose);
00403       }
00404     }
00405     catch(tf::LookupException& ex) {
00406       ROS_ERROR("No Transform available Error: %s\n", ex.what());
00407       return false;
00408     }
00409     catch(tf::ConnectivityException& ex) {
00410       ROS_ERROR("Connectivity Error: %s\n", ex.what());
00411       return false;
00412     }
00413     catch(tf::ExtrapolationException& ex) {
00414       ROS_ERROR("Extrapolation Error: %s\n", ex.what());
00415       if (!global_plan.empty())
00416         ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
00417 
00418       return false;
00419     }
00420 
00421     return true;
00422   }
00423 };


pose_follower
Author(s): Eitan Marder-Eppstein
autogenerated on Thu Mar 28 2019 03:37:40