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 */
base_local_planner::Trajectory::xv_
double xv_
Definition: trajectory.h:92
base_local_planner::PreferForwardCostFunction::penalty_
double penalty_
Definition: prefer_forward_cost_function.h:130
prefer_forward_cost_function.h
base_local_planner::PreferForwardCostFunction::scoreTrajectory
double scoreTrajectory(Trajectory &traj)
Definition: prefer_forward_cost_function.cpp:15
base_local_planner::Trajectory
Holds a trajectory generated by considering an x, y, and theta velocity.
Definition: trajectory.h:76
base_local_planner
Definition: costmap_model.h:44
base_local_planner::Trajectory::thetav_
double thetav_
The x, y, and theta velocities of the trajectory.
Definition: trajectory.h:92


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:24