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 */