23 #ifndef __PLUGINS_COLLI_ABSTRACT_DRIVE_MODE_H_ 24 #define __PLUGINS_COLLI_ABSTRACT_DRIVE_MODE_H_ 26 #include "../common/types.h" 28 #include <interfaces/NavigatorInterface.h> 29 #include <logging/logger.h> 30 #include <config/config.h> 88 float lin_interpol(
float x,
float left,
float right,
float bot,
float top );
117 float max_trans_acc_;
118 float max_trans_dec_;
123 float stopping_distance_;
124 float stopping_factor_;
147 max_trans_acc_ =
config_->
get_float(
"/plugins/colli/motor_instruct/trans_acc");
148 max_trans_dec_ =
config_->
get_float(
"/plugins/colli/motor_instruct/trans_dec");
152 stopping_distance_ =
config_->
get_float(
"/plugins/colli/drive_mode/stopping_adjustment/distance_addition");
153 stopping_factor_ =
config_->
get_float(
"/plugins/colli/drive_mode/stopping_adjustment/deceleration_factor");
154 stopping_factor_ = std::min(1.f, std::max(0.f, stopping_factor_));
306 return ( ((x-x1)*(y2-y1))/(x2-x1) + y1 );
318 float desired_trans )
320 distance = fabs( distance );
321 current_trans = fabs( current_trans );
323 if ( distance < 0.05f )
326 if ( current_trans < 0.05f )
327 return desired_trans;
330 float trans_tmp = current_trans;
331 float distance_to_stop = stopping_distance_;
332 for (
int loops_to_stop = 0; trans_tmp > 0; loops_to_stop++) {
333 distance_to_stop += trans_tmp / frequency_;
334 trans_tmp -= max_trans_dec_ * stopping_factor_;
338 if (distance_to_stop >= distance) {
341 return desired_trans;
float x
Translation in x-direction.
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Cartesian coordinates (2D).
cart_coord_2d_t local_target_
local target
float get_proposed_trans_x()
Returns the proposed x translation.
Logger * logger_
The fawkes logger.
Fawkes library namespace.
NavigatorInterface::DriveMode get_drive_mode_name()
Returns the drive modes name.
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
NavigatorInterface::OrientationMode orient_mode_
orient mode of nav if
OrientationMode
Orientation mode enum.
void set_current_target(float x, float y, float ori)
Sets the current target.
bool stop_at_target_
flag if stopping on or after target
float max_trans_
The maximum translation speed.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Storing Translation and rotation.
float y
Translation in y-direction.
float rot
Rotation around z-axis.
float max_rot_
The maximum rotation speed.
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
field_pos_t target_
current target
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.
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
float x
x coordinate in meters
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
field_pos_t robot_
current robot pos
float get_proposed_trans_y()
Returns the proposed y translation.
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.
void set_current_colli_mode(NavigatorInterface::OrientationMode orient, bool stop)
Set the colli mode values for each drive mode.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
AbstractDriveMode(Logger *logger, Configuration *config)
Constructor.
float robot_speed_
current robo translation velocity
float get_proposed_rot()
Returns the proposed rotatio.
DriveMode
Drive modes enum.
virtual ~AbstractDriveMode()
Desctructor.
float y
y coordinate in meters
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