prefer_forward_cost_function.cpp
Go to the documentation of this file.
1 /*
2  * prefer_forward_cost_function.cpp
3  *
4  * Created on: Apr 4, 2012
5  * Author: tkruse
6  */
7 
9 
10 #include <math.h>
11 
12 namespace base_local_planner {
13 
14 
16  // backward motions bad on a robot without backward sensors
17  if (traj.xv_ < 0.0) {
18  return penalty_;
19  }
20  // strafing motions also bad on such a robot
21  if (traj.xv_ < 0.1 && fabs(traj.thetav_) < 0.2) {
22  return penalty_;
23  }
24  // the more we rotate, the less we progress forward
25  return fabs(traj.thetav_) * 10;
26 }
27 
28 } /* namespace base_local_planner */
double thetav_
The x, y, and theta velocities of the trajectory.
Definition: trajectory.h:60
Holds a trajectory generated by considering an x, y, and theta velocity.
Definition: trajectory.h:44


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49