Fawkes API
Fawkes Development Version
|
24 #include "act_thread.h"
26 #include "colli_thread.h"
28 #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;
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;
271 ColliActThread::colli_final()
void unref()
Decrement reference count and conditionally delete this instance.
void set_security_distance(const float new_security_distance)
Set security_distance value.
void colli_stop()
Sends a stop-command.
bool msgq_first_is()
Check if first message has desired type.
void msgq_pop()
Erase first message from queue.
bool msgq_empty()
Check if queue is empty.
DriveMode drive_mode() const
Get drive_mode value.
void set_dest_dist(const float new_dest_dist)
Set dest_dist value.
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
float x() const
Get x value.
OrientationMode orientation_mode() const
Get orientation_mode value.
float security_distance() const
Get security_distance value.
void set_drive_mode(const DriveMode new_drive_mode)
Set drive_mode value.
void set_dest_x(const float new_dest_x)
Set dest_x value.
bool is_escaping_enabled() const
Get escaping_enabled value.
virtual bool get_bool(const char *path)=0
const char * tostring_OrientationMode(OrientationMode value) const
Convert OrientationMode constant to string.
float orientation() const
Get orientation value.
float phi() const
Get phi value.
void colli_relgoto(float x, float y, float ori, fawkes::NavigatorInterface *iface)
Sends a goto-command, using relative coordinates.
void set_max_rotation(const float new_max_rotation)
Set max_rotation value.
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
const char * name() const
void set_dest_y(const float new_dest_y)
Set dest_y value.
float max_velocity() const
Get max_velocity value.
void set_escaping_enabled(const bool new_escaping_enabled)
Set escaping_enabled value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_orientation_mode(const OrientationMode new_orientation_mode)
Set orientation_mode value.
float y() const
Get y value.
void ref()
Increment reference count.
virtual void close(Interface *interface)=0
float max_rotation() const
Get max_rotation value.
void set_stop_at_target(const bool new_stop_at_target)
Set stop_at_target value.
void set_final(const bool new_final)
Set final value.
const char * tostring_DriveMode(DriveMode value) const
Convert DriveMode constant to string.
virtual void loop()
Code to execute in the thread.
float orientation() const
Get orientation value.
bool is_final() const
Checks if the colli is final.
bool is_stop_at_target() const
Get stop_at_target value.
virtual float get_float(const char *path)=0
virtual std::string get_string(const char *path)=0
unsigned int id() const
Get message ID.
bool is_of_type()
Check if message has desired type.
Message * msgq_first()
Get the first message from the message queue.
virtual void finalize()
Finalize the thread.
virtual void log_debug(const char *component, const char *format,...)=0
void write()
Write from local copy into BlackBoard memory.
virtual ~ColliActThread()
Desctructor.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
ColliActThread(ColliThread *colli_thread)
Constructor.
virtual void init()
Initialize the thread.
float dist() const
Get dist value.