Go to the documentation of this file.
57 if (traj->
cost_ >= 0) {
71 double x_diff = pos[0] - prev[0];
72 double y_diff = pos[1] - prev[1];
73 double sq_dist = x_diff * x_diff + y_diff * y_diff;
75 double th_diff = pos[2] - prev[2];
102 bool flag_set =
false;
122 if (fabs(
t->xv_) <= min_vel_trans) {
144 if (
t->thetav_ < 0) {
154 if (
t->thetav_ > 0) {
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
bool setOscillationFlags(base_local_planner::Trajectory *t, double min_vel_trans)
Given a trajectory that's selected, set flags if needed to prevent the robot from oscillating.
void updateOscillationFlags(Eigen::Vector3f pos, base_local_planner::Trajectory *traj, double min_vel_trans)
double oscillation_reset_angle_
double scoreTrajectory(Trajectory &traj)
virtual ~OscillationCostFunction()
double oscillation_reset_dist_
double cost_
The cost/score of the trajectory.
Eigen::Vector3f prev_stationary_pos_
OscillationCostFunction()
void resetOscillationFlags()
Reset the oscillation flags for the local planner.
void setOscillationResetDist(double dist, double angle)
Holds a trajectory generated by considering an x, y, and theta velocity.
void resetOscillationFlagsIfPossible(const Eigen::Vector3f &pos, const Eigen::Vector3f &prev)
geometry_msgs::TransformStamped t
base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:24