00001 /* 00002 * prefer_forward_cost_function.cpp 00003 * 00004 * Created on: Apr 4, 2012 00005 * Author: tkruse 00006 */ 00007 00008 #include <base_local_planner/prefer_forward_cost_function.h> 00009 00010 #include <math.h> 00011 00012 namespace base_local_planner { 00013 00014 00015 double PreferForwardCostFunction::scoreTrajectory(Trajectory &traj) { 00016 // backward motions bad on a robot without backward sensors 00017 if (traj.xv_ < 0.0) { 00018 return penalty_; 00019 } 00020 // strafing motions also bad on such a robot 00021 if (traj.xv_ < 0.1 && fabs(traj.thetav_) < 0.2) { 00022 return penalty_; 00023 } 00024 // the more we rotate, the less we progress forward 00025 return fabs(traj.thetav_) * 10; 00026 } 00027 00028 } /* namespace base_local_planner */