oscillation_cost_function.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, 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: TKruse
00036  *********************************************************************/
00037 
00038 #include <base_local_planner/oscillation_cost_function.h>
00039 
00040 #include <cmath>
00041 
00042 namespace base_local_planner {
00043 
00044 OscillationCostFunction::OscillationCostFunction() {
00045 }
00046 
00047 OscillationCostFunction::~OscillationCostFunction() {
00048   prev_stationary_pos_ = Eigen::Vector3f::Zero();
00049 }
00050 
00051 void OscillationCostFunction::setOscillationResetDist(double dist, double angle) {
00052   oscillation_reset_dist_ = dist;
00053   oscillation_reset_angle_ = angle;
00054 }
00055 
00056 void OscillationCostFunction::updateOscillationFlags(Eigen::Vector3f pos, base_local_planner::Trajectory* traj, double min_vel_trans) {
00057   if (traj->cost_ >= 0) {
00058     if (setOscillationFlags(traj, min_vel_trans)) {
00059       prev_stationary_pos_ = pos;
00060     }
00061     //if we've got restrictions... check if we can reset any oscillation flags
00062     if(forward_pos_only_ || forward_neg_only_
00063         || strafe_pos_only_ || strafe_neg_only_
00064         || rot_pos_only_ || rot_neg_only_){
00065       resetOscillationFlagsIfPossible(pos, prev_stationary_pos_);
00066     }
00067   }
00068 }
00069 
00070 void OscillationCostFunction::resetOscillationFlagsIfPossible(const Eigen::Vector3f& pos, const Eigen::Vector3f& prev) {
00071   double x_diff = pos[0] - prev[0];
00072   double y_diff = pos[1] - prev[1];
00073   double sq_dist = x_diff * x_diff + y_diff * y_diff;
00074 
00075   double th_diff = pos[2] - prev[2];
00076 
00077   //if we've moved far enough... we can reset our flags
00078   if (sq_dist > oscillation_reset_dist_ * oscillation_reset_dist_ ||
00079       fabs(th_diff) > oscillation_reset_angle_) {
00080     resetOscillationFlags();
00081   }
00082 }
00083 
00084 void OscillationCostFunction::resetOscillationFlags() {
00085   strafe_pos_only_ = false;
00086   strafe_neg_only_ = false;
00087   strafing_pos_ = false;
00088   strafing_neg_ = false;
00089 
00090   rot_pos_only_ = false;
00091   rot_neg_only_ = false;
00092   rotating_pos_ = false;
00093   rotating_neg_ = false;
00094 
00095   forward_pos_only_ = false;
00096   forward_neg_only_ = false;
00097   forward_pos_ = false;
00098   forward_neg_ = false;
00099 }
00100 
00101 bool OscillationCostFunction::setOscillationFlags(base_local_planner::Trajectory* t, double min_vel_trans) {
00102   bool flag_set = false;
00103   //set oscillation flags for moving forward and backward
00104   if (t->xv_ < 0.0) {
00105     if (forward_pos_) {
00106       forward_neg_only_ = true;
00107       flag_set = true;
00108     }
00109     forward_pos_ = false;
00110     forward_neg_ = true;
00111   }
00112   if (t->xv_ > 0.0) {
00113     if (forward_neg_) {
00114       forward_pos_only_ = true;
00115       flag_set = true;
00116     }
00117     forward_neg_ = false;
00118     forward_pos_ = true;
00119   }
00120 
00121   //we'll only set flags for strafing and rotating when we're not moving forward at all
00122   if (fabs(t->xv_) <= min_vel_trans) {
00123     //check negative strafe
00124     if (t->yv_ < 0) {
00125       if (strafing_pos_) {
00126         strafe_neg_only_ = true;
00127         flag_set = true;
00128       }
00129       strafing_pos_ = false;
00130       strafing_neg_ = true;
00131     }
00132 
00133     //check positive strafe
00134     if (t->yv_ > 0) {
00135       if (strafing_neg_) {
00136         strafe_pos_only_ = true;
00137         flag_set = true;
00138       }
00139       strafing_neg_ = false;
00140       strafing_pos_ = true;
00141     }
00142 
00143     //check negative rotation
00144     if (t->thetav_ < 0) {
00145       if (rotating_pos_) {
00146         rot_neg_only_ = true;
00147         flag_set = true;
00148       }
00149       rotating_pos_ = false;
00150       rotating_neg_ = true;
00151     }
00152 
00153     //check positive rotation
00154     if (t->thetav_ > 0) {
00155       if (rotating_neg_) {
00156         rot_pos_only_ = true;
00157         flag_set = true;
00158       }
00159       rotating_neg_ = false;
00160       rotating_pos_ = true;
00161     }
00162   }
00163   return flag_set;
00164 }
00165 
00166 double OscillationCostFunction::scoreTrajectory(Trajectory &traj) {
00167   if ((forward_pos_only_ && traj.xv_ < 0.0) ||
00168       (forward_neg_only_ && traj.xv_ > 0.0) ||
00169       (strafe_pos_only_  && traj.yv_ < 0.0) ||
00170       (strafe_neg_only_  && traj.yv_ > 0.0) ||
00171       (rot_pos_only_     && traj.thetav_ < 0.0) ||
00172       (rot_neg_only_     && traj.thetav_ > 0.0)) {
00173     return -5.0;
00174   }
00175   return 0.0;
00176 }
00177 
00178 } /* namespace base_local_planner */


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:08