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