oscillation_cost_function.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: TKruse
36  *********************************************************************/
37 
38 #ifndef OSCILLATION_COST_FUNCTION_H_
39 #define OSCILLATION_COST_FUNCTION_H_
40 
42 #include <Eigen/Core>
43 
44 namespace base_local_planner {
45 
46 class OscillationCostFunction: public base_local_planner::TrajectoryCostFunction {
47 public:
49  virtual ~OscillationCostFunction();
50 
51  double scoreTrajectory(Trajectory &traj);
52 
53  bool prepare() {return true;};
54 
58  void resetOscillationFlags();
59 
60 
61  void updateOscillationFlags(Eigen::Vector3f pos, base_local_planner::Trajectory* traj, double min_vel_trans);
62 
63  void setOscillationResetDist(double dist, double angle);
64 
65 private:
66 
67  void resetOscillationFlagsIfPossible(const Eigen::Vector3f& pos, const Eigen::Vector3f& prev);
68 
75  bool setOscillationFlags(base_local_planner::Trajectory* t, double min_vel_trans);
76 
77  // flags
81 
82  // param
84 
85  Eigen::Vector3f prev_stationary_pos_;
86 };
87 
88 } /* namespace base_local_planner */
89 #endif /* OSCILLATION_COST_FUNCTION_H_ */
base_local_planner::OscillationCostFunction::strafing_pos_
bool strafing_pos_
Definition: oscillation_cost_function.h:148
base_local_planner::OscillationCostFunction::setOscillationFlags
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.
Definition: oscillation_cost_function.cpp:136
base_local_planner::OscillationCostFunction::strafe_pos_only_
bool strafe_pos_only_
Definition: oscillation_cost_function.h:148
base_local_planner::OscillationCostFunction::forward_neg_only_
bool forward_neg_only_
Definition: oscillation_cost_function.h:150
base_local_planner::OscillationCostFunction::updateOscillationFlags
void updateOscillationFlags(Eigen::Vector3f pos, base_local_planner::Trajectory *traj, double min_vel_trans)
Definition: oscillation_cost_function.cpp:91
base_local_planner::OscillationCostFunction::rotating_neg_
bool rotating_neg_
Definition: oscillation_cost_function.h:149
base_local_planner::OscillationCostFunction::rot_neg_only_
bool rot_neg_only_
Definition: oscillation_cost_function.h:149
base_local_planner::OscillationCostFunction::oscillation_reset_angle_
double oscillation_reset_angle_
Definition: oscillation_cost_function.h:153
base_local_planner::OscillationCostFunction::forward_neg_
bool forward_neg_
Definition: oscillation_cost_function.h:150
base_local_planner::OscillationCostFunction::prepare
bool prepare()
Definition: oscillation_cost_function.h:123
base_local_planner::OscillationCostFunction::scoreTrajectory
double scoreTrajectory(Trajectory &traj)
Definition: oscillation_cost_function.cpp:201
base_local_planner::TrajectoryCostFunction
Provides an interface for critics of trajectories During each sampling run, a batch of many trajector...
Definition: trajectory_cost_function.h:87
base_local_planner::OscillationCostFunction::strafing_neg_
bool strafing_neg_
Definition: oscillation_cost_function.h:148
base_local_planner::OscillationCostFunction::rot_pos_only_
bool rot_pos_only_
Definition: oscillation_cost_function.h:149
base_local_planner::OscillationCostFunction::~OscillationCostFunction
virtual ~OscillationCostFunction()
Definition: oscillation_cost_function.cpp:82
base_local_planner::OscillationCostFunction::oscillation_reset_dist_
double oscillation_reset_dist_
Definition: oscillation_cost_function.h:153
base_local_planner::OscillationCostFunction::prev_stationary_pos_
Eigen::Vector3f prev_stationary_pos_
Definition: oscillation_cost_function.h:155
base_local_planner::OscillationCostFunction::rotating_pos_
bool rotating_pos_
Definition: oscillation_cost_function.h:149
base_local_planner::OscillationCostFunction::strafe_neg_only_
bool strafe_neg_only_
Definition: oscillation_cost_function.h:148
base_local_planner::OscillationCostFunction::OscillationCostFunction
OscillationCostFunction()
Definition: oscillation_cost_function.cpp:79
base_local_planner::OscillationCostFunction::resetOscillationFlags
void resetOscillationFlags()
Reset the oscillation flags for the local planner.
Definition: oscillation_cost_function.cpp:119
base_local_planner::OscillationCostFunction::forward_pos_
bool forward_pos_
Definition: oscillation_cost_function.h:150
base_local_planner::OscillationCostFunction::setOscillationResetDist
void setOscillationResetDist(double dist, double angle)
Definition: oscillation_cost_function.cpp:86
base_local_planner::Trajectory
Holds a trajectory generated by considering an x, y, and theta velocity.
Definition: trajectory.h:76
base_local_planner::OscillationCostFunction::forward_pos_only_
bool forward_pos_only_
Definition: oscillation_cost_function.h:150
base_local_planner
Definition: costmap_model.h:44
base_local_planner::OscillationCostFunction::resetOscillationFlagsIfPossible
void resetOscillationFlagsIfPossible(const Eigen::Vector3f &pos, const Eigen::Vector3f &prev)
Definition: oscillation_cost_function.cpp:105
trajectory_cost_function.h


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