hector_path_follower: hector_path_follower: hector_path

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