24#include "act_thread.h"
26#include "colli_thread.h"
28#include <interfaces/NavigatorInterface.h>
29#include <utils/math/coord.h>
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;
94 "Default drive_mode: %i (%s)",
98 cfg_iface_navi_ =
config->
get_string((cfg_prefix +
"interface/navigator").c_str());
100 cfg_frame_odom_ =
config->
get_string((cfg_prefix +
"frame/odometry").c_str());
188 "setting orient_at_target to %s",
197 "setting drive_mode to %s",
228 "CartesianGotoMessage received, x:%f y:%f ori:%f",
246 "PolarGotoMessage received, phi:%f dist:%f",
251 float cart_x = 0, cart_y = 0;
271ColliActThread::colli_final()
virtual void finalize()
Finalize the thread.
ColliActThread(ColliThread *colli_thread)
Constructor.
virtual void init()
Initialize the thread.
virtual ~ColliActThread()
Desctructor.
virtual void loop()
Code to execute in the thread.
Thread that performs the navigation and collision avoidance algorithms.
void colli_relgoto(float x, float y, float ori, fawkes::NavigatorInterface *iface)
Sends a goto-command, using relative coordinates.
void colli_stop()
Sends a stop-command.
bool is_final() const
Checks if the colli is final.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
bool msgq_first_is()
Check if first message has desired type.
void msgq_pop()
Erase first message from queue.
Message * msgq_first()
Get the first message from the message queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Logger * logger
This is the Logger member used to access the logger.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
bool is_of_type()
Check if message has desired type.
unsigned int id() const
Get message ID.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
float x() const
Get x value.
float orientation() const
Get orientation value.
float y() const
Get y value.
PolarGotoMessage Fawkes BlackBoard Interface Message.
float phi() const
Get phi value.
float orientation() const
Get orientation value.
float dist() const
Get dist value.
ResetParametersMessage Fawkes BlackBoard Interface Message.
SetDriveModeMessage Fawkes BlackBoard Interface Message.
DriveMode drive_mode() const
Get drive_mode value.
SetEscapingMessage Fawkes BlackBoard Interface Message.
bool is_escaping_enabled() const
Get escaping_enabled value.
SetMaxRotationMessage Fawkes BlackBoard Interface Message.
float max_rotation() const
Get max_rotation value.
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
float max_velocity() const
Get max_velocity value.
SetOrientationModeMessage Fawkes BlackBoard Interface Message.
OrientationMode orientation_mode() const
Get orientation_mode value.
SetSecurityDistanceMessage Fawkes BlackBoard Interface Message.
float security_distance() const
Get security_distance value.
SetStopAtTargetMessage Fawkes BlackBoard Interface Message.
bool is_stop_at_target() const
Get stop_at_target value.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
void set_max_rotation(const float new_max_rotation)
Set max_rotation value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_stop_at_target(const bool new_stop_at_target)
Set stop_at_target value.
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
void set_drive_mode(const DriveMode new_drive_mode)
Set drive_mode value.
void set_security_distance(const float new_security_distance)
Set security_distance value.
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.
void set_dest_y(const float new_dest_y)
Set dest_y value.
const char * tostring_DriveMode(DriveMode value) const
Convert DriveMode constant to string.
void set_dest_x(const float new_dest_x)
Set dest_x value.
const char * tostring_OrientationMode(OrientationMode value) const
Convert OrientationMode constant to string.
void set_final(const bool new_final)
Set final value.
void set_orientation_mode(const OrientationMode new_orientation_mode)
Set orientation_mode value.
void set_escaping_enabled(const bool new_escaping_enabled)
Set escaping_enabled value.
void unref()
Decrement reference count and conditionally delete this instance.
void ref()
Increment reference count.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Fawkes library namespace.
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.