24 #include "act_thread.h" 25 #include "colli_thread.h" 27 #include <interfaces/NavigatorInterface.h> 29 #include <utils/math/coord.h> 46 :
Thread(
"ColliActThread",
Thread::OPMODE_WAITFORWAKEUP),
48 thread_colli_( colli_thread )
60 std::string cfg_prefix =
"/plugins/colli/";
61 cfg_security_distance_ =
config->
get_float((cfg_prefix +
"security_distance").c_str());
62 cfg_max_velocity_ =
config->
get_float((cfg_prefix +
"max_velocity").c_str());
63 cfg_max_rotation_ =
config->
get_float((cfg_prefix +
"max_rotation").c_str());
64 cfg_escaping_enabled_ =
config->
get_bool((cfg_prefix +
"escaping_enabled").c_str());
65 cfg_stop_at_target_ =
config->
get_bool((cfg_prefix +
"stop_at_target").c_str());
67 std::string cfg_orient_mode =
config->
get_string((cfg_prefix +
"orient_mode/default").c_str());
68 if ( cfg_orient_mode ==
"OrientAtTarget" ) {
69 cfg_orient_mode_ = fawkes::NavigatorInterface::OrientationMode::OrientAtTarget;
70 }
else if ( cfg_orient_mode ==
"OrientDuringTravel" ) {
71 cfg_orient_mode_ = fawkes::NavigatorInterface::OrientationMode::OrientDuringTravel;
73 cfg_orient_mode_ = fawkes::NavigatorInterface::OrientationMode::OrientAtTarget;
77 std::string cfg_drive_mode =
config->
get_string((cfg_prefix +
"drive_mode/default").c_str());
78 if ( cfg_drive_mode ==
"MovingNotAllowed" ) {
79 cfg_drive_mode_ = NavigatorInterface::MovingNotAllowed;
80 }
else if ( cfg_drive_mode ==
"Forward" ) {
81 cfg_drive_mode_ = NavigatorInterface::Forward;
82 }
else if ( cfg_drive_mode ==
"AllowBackward" ) {
83 cfg_drive_mode_ = NavigatorInterface::AllowBackward;
84 }
else if ( cfg_drive_mode ==
"Backward" ) {
85 cfg_drive_mode_ = NavigatorInterface::Backward;
86 }
else if ( cfg_drive_mode ==
"ESCAPE" ) {
87 cfg_drive_mode_ = NavigatorInterface::ESCAPE;
89 cfg_drive_mode_ = NavigatorInterface::MovingNotAllowed;
95 cfg_iface_navi_ =
config->
get_string((cfg_prefix +
"interface/navigator").c_str());
97 cfg_frame_odom_ =
config->
get_string((cfg_prefix +
"frame/odometry").c_str());
229 float cart_x = 0, cart_y = 0;
249 ColliActThread::colli_final()
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
virtual ~ColliActThread()
Desctructor.
void set_stop_at_target(const bool new_stop_at_target)
Set stop_at_target value.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
unsigned int id() const
Get message ID.
bool msgq_empty()
Check if queue is empty.
void unref()
Decrement reference count and conditionally delete this instance.
float y() const
Get y value.
void set_escaping_enabled(const bool new_escaping_enabled)
Set escaping_enabled value.
float orientation() const
Get orientation value.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void set_final(const bool new_final)
Set final value.
SetSecurityDistanceMessage Fawkes BlackBoard Interface Message.
bool is_escaping_enabled() const
Get escaping_enabled value.
float max_rotation() const
Get max_rotation value.
void colli_stop()
Sends a stop-command.
PolarGotoMessage Fawkes BlackBoard Interface Message.
float max_velocity() const
Get max_velocity value.
Thread class encapsulation of pthreads.
SetOrientationModeMessage Fawkes BlackBoard Interface Message.
void write()
Write from local copy into BlackBoard memory.
bool is_stop_at_target() const
Get stop_at_target value.
Logger * logger
This is the Logger member used to access the logger.
const char * tostring_DriveMode(DriveMode value) const
Convert DriveMode constant to string.
float orientation() const
Get orientation value.
void set_security_distance(const float new_security_distance)
Set security_distance value.
float dist() const
Get dist value.
SetStopAtTargetMessage Fawkes BlackBoard Interface Message.
SetEscapingMessage Fawkes BlackBoard Interface Message.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Thread that performs the navigation and collision avoidance algorithms.
SetDriveModeMessage Fawkes BlackBoard Interface Message.
Base class for exceptions in Fawkes.
Message * msgq_first()
Get the first message from the message queue.
SetMaxRotationMessage Fawkes BlackBoard Interface Message.
void colli_relgoto(float x, float y, float ori, fawkes::NavigatorInterface *iface)
Sends a goto-command, using relative coordinates.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
DriveMode drive_mode() const
Get drive_mode value.
void ref()
Increment reference count.
ColliActThread(ColliThread *colli_thread)
Constructor.
bool is_final() const
Checks if the colli is final.
const char * name() const
Get name of thread.
void set_drive_mode(const DriveMode new_drive_mode)
Set drive_mode value.
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
bool msgq_first_is()
Check if first message has desired type.
float security_distance() const
Get security_distance value.
virtual void loop()
Code to execute in the thread.
bool is_of_type()
Check if message has desired type.
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
void set_dest_dist(const float new_dest_dist)
Set dest_dist value.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
void set_max_rotation(const float new_max_rotation)
Set max_rotation value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void set_dest_x(const float new_dest_x)
Set dest_x value.
float phi() const
Get phi value.
const char * tostring_OrientationMode(OrientationMode value) const
Convert OrientationMode constant to string.
OrientationMode orientation_mode() const
Get orientation_mode value.
void set_orientation_mode(const OrientationMode new_orientation_mode)
Set orientation_mode value.
float x() const
Get x value.
ResetParametersMessage Fawkes BlackBoard Interface Message.
void set_dest_y(const float new_dest_y)
Set dest_y value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Configuration * config
This is the Configuration member used to access the configuration.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
virtual void close(Interface *interface)=0
Close interface.