23 #include "forward_drive_mode.h" 24 #include <utils/math/common.h> 78 ForwardDriveModule::forward_curvature(
float dist_to_target,
float dist_to_trajec,
float alpha,
79 float cur_trans,
float cur_rot )
95 ForwardDriveModule::forward_translation(
float dist_to_target,
float dist_to_front,
float alpha,
96 float cur_trans,
float cur_rot,
float des_rot )
98 if( fabs(alpha) >= M_PI_2 ) {
103 float des_trans = 0.f;
135 float trans_target = 10000.f;
136 float trans_front = 10000.f;
142 if ( dist_to_front > 0.f && dist_to_front < dist_to_target )
146 des_trans = std::min( des_trans, std::min( trans_target, trans_front ) );
190 if ( dist_to_target >= 0.04 ) {
192 proposed_.
rot = forward_curvature( dist_to_target, dist_to_trajec, alpha,
195 proposed_.
x = forward_translation( dist_to_target, dist_to_trajec, alpha,
202 if( trans_correction < 1.f ) {
204 proposed_.
x *= trans_correction * trans_correction;
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
virtual void update()
Calculate here your desired settings.
float max_trans_
The maximum translation speed.
float y
Translation in y-direction.
~ForwardDriveModule()
Destructor.
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 guarantee_trans_stop(float distance, float current_trans, float desired_trans)
Get velocity that guarantees a stop for a given distance.
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.
ForwardDriveModule(Logger *logger, Configuration *config)
Constructor.
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.
cart_coord_2d_t local_trajec_
local trajectory