23 #include "backward_drive_mode.h" 25 #include <utils/math/angle.h> 26 #include <utils/math/common.h> 81 BackwardDriveModule::backward_curvature(
float dist_to_target,
float dist_to_trajec,
float alpha,
82 float cur_trans,
float cur_rot )
98 BackwardDriveModule::backward_translation(
float dist_to_target,
float dist_to_front,
float alpha,
99 float cur_trans,
float cur_rot,
float des_rot)
101 float des_trans = 0.f;
103 if ( fabs( des_rot ) >= 0.f && fabs( des_rot ) <= 1.f )
106 else if ( fabs( des_rot ) > 1.f )
107 des_trans =
lin_interpol( fabs( des_rot ), M_PI, 1.f, 0.f, 0.7f );
110 if ( des_trans > 0.f ) des_trans = 0.f;
128 float trans_target = 10000.f;
129 float trans_front = 10000.f;
135 if ( dist_to_front < dist_to_target )
139 des_trans = std::min( des_trans, std::min( trans_target, trans_front ) );
182 proposed_.
rot = backward_curvature( dist_to_target, dist_to_trajec, alpha,
186 if ( fabs( alpha ) <= M_PI_2+0.1f )
187 proposed_.
x = backward_translation( dist_to_target, dist_to_trajec, alpha,
192 if ( dist_to_target >= 0.04f ) {
float x
Translation in x-direction.
cart_coord_2d_t local_target_
local target
Logger * logger_
The fawkes logger.
Fawkes library namespace.
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
bool stop_at_target_
flag if stopping on or after target
float max_trans_
The maximum translation speed.
void update()
Calculate here your desired settings.
BackwardDriveModule(Logger *logger, Configuration *config)
Constructor.
float y
Translation in y-direction.
float rot
Rotation around z-axis.
double sqr(double x)
Fast square multiplication.
float max_rot_
The maximum rotation speed.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
colli_trans_rot_t robot_vel_
current robot velocity
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
float guarantee_trans_stop(float distance, float current_trans, float desired_trans)
Get velocity that guarantees a stop for a given distance.
Moving backward constant.
float lin_interpol(float x, float left, float right, float bot, float top)
Perform linear interpolation.
Moving not allowed constant.
Configuration * config_
The fawkes configuration.
This is the base class which calculates drive modes.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
float robot_speed_
current robo translation velocity
Interface for configuration handling.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
~BackwardDriveModule()
Destruct your local values here!
cart_coord_2d_t local_trajec_
local trajectory