prefer_forward_cost_function.cpp
Go to the documentation of this file.
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 */


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:38