23 #ifndef _PLUGINS_COLLI_ABSTRACT_DRIVE_MODE_H_
24 #define _PLUGINS_COLLI_ABSTRACT_DRIVE_MODE_H_
26 #include "../common/types.h"
28 #include <config/config.h>
29 #include <interfaces/NavigatorInterface.h>
30 #include <logging/logger.h>
81 float lin_interpol(
float x,
float left,
float right,
float bot,
float top);
109 float max_trans_acc_;
110 float max_trans_dec_;
115 float stopping_distance_;
116 float stopping_factor_;
128 : logger_(logger), config_(config)
135 max_trans_acc_ =
config_->
get_float(
"/plugins/colli/motor_instruct/trans_acc");
136 max_trans_dec_ =
config_->
get_float(
"/plugins/colli/motor_instruct/trans_dec");
141 config_->
get_float(
"/plugins/colli/drive_mode/stopping_adjustment/distance_addition");
143 config_->
get_float(
"/plugins/colli/drive_mode/stopping_adjustment/deceleration_factor");
144 stopping_factor_ = std::min(1.f, std::max(0.f, stopping_factor_));
290 return (((x - x1) * (y2 - y1)) / (x2 - x1) + y1);
303 current_trans = fabs(current_trans);
308 if (current_trans < 0.05f)
309 return desired_trans;
312 float trans_tmp = current_trans;
313 float distance_to_stop = stopping_distance_;
314 for (
int loops_to_stop = 0; trans_tmp > 0; loops_to_stop++) {
315 distance_to_stop += trans_tmp / frequency_;
316 trans_tmp -= max_trans_dec_ * stopping_factor_;
323 return desired_trans;
This is the base class which calculates drive modes.
bool stop_at_target_
flag if stopping on or after target
virtual ~AbstractDriveMode()
Desctructor.
void set_current_target(float x, float y, float ori)
Sets the current target.
void set_current_colli_mode(NavigatorInterface::OrientationMode orient, bool stop)
Set the colli mode values for each drive mode.
float max_trans_
The maximum translation speed.
float get_proposed_trans_x()
Returns the proposed x translation.
float get_proposed_rot()
Returns the proposed rotatio.
NavigatorInterface::OrientationMode orient_mode_
orient mode of nav if
cart_coord_2d_t local_trajec_
local trajectory
float robot_speed_
current robo translation velocity
field_pos_t target_
current target
field_pos_t robot_
current robot pos
colli_trans_rot_t robot_vel_
current robot velocity
AbstractDriveMode(Logger *logger, Configuration *config)
Constructor.
NavigatorInterface::DriveMode get_drive_mode_name()
Returns the drive modes name.
float max_rot_
The maximum rotation speed.
Configuration * config_
The fawkes configuration.
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
float get_proposed_trans_y()
Returns the proposed y translation.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
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.
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
cart_coord_2d_t local_target_
local target
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.
Interface for configuration handling.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
DriveMode
Drive modes enum.
@ MovingNotAllowed
Moving not allowed constant.
OrientationMode
Orientation mode enum.
Fawkes library namespace.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Cartesian coordinates (2D).
Storing Translation and rotation.
float x
Translation in x-direction.
float y
Translation in y-direction.
float rot
Rotation around z-axis.
float y
y coordinate in meters
float x
x coordinate in meters