Fawkes API
Fawkes Development Version
|
Virtual base class for thread that communicates with a Robotino. More...
#include "com_thread.h"
Classes | |
class | SensorData |
Struct to exchange data between com and sensor thread. More... | |
Public Member Functions | |
RobotinoComThread (const char *thread_name) | |
Constructor. More... | |
virtual | ~RobotinoComThread () |
Destructor. More... | |
virtual bool | is_connected ()=0 |
Check if we are connected to OpenRobotino. More... | |
virtual void | set_gripper (bool opened)=0 |
Open or close gripper. More... | |
virtual bool | is_gripper_open ()=0 |
Check if gripper is open. More... | |
virtual void | set_speed_points (float s1, float s2, float s3)=0 |
Set speed points for wheels. More... | |
virtual void | get_act_velocity (float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)=0 |
Get actual velocity. More... | |
virtual void | get_odometry (double &x, double &y, double &phi)=0 |
Get latest odometry value. More... | |
virtual void | reset_odometry ()=0 |
Reset odometry to zero. More... | |
virtual void | set_bumper_estop_enabled (bool enabled)=0 |
Enable or disable emergency stop on bumper contact. More... | |
virtual void | set_motor_accel_limits (float min_accel, float max_accel)=0 |
Set acceleration limits of motors. More... | |
virtual void | set_digital_output (unsigned int digital_out, bool enable)=0 |
Set digital output state. More... | |
virtual bool | get_data (SensorData &sensor_data) |
Get all current sensor data. More... | |
void | set_drive_layout (float rb, float rw, float gear) |
Set omni drive layout parameters. More... | |
void | set_drive_limits (float trans_accel, float trans_decel, float rot_accel, float rot_decel) |
Set the omni drive limits. More... | |
virtual void | set_desired_vel (float vx, float vy, float omega) |
Set desired velocities. More... | |
void | project (float *m1, float *m2, float *m3, float vx, float vy, float omega) const |
Project the velocity of the robot in cartesian coordinates to single motor speeds. More... | |
void | unproject (float *vx, float *vy, float *omega, float m1, float m2, float m3) const |
Project single motor speeds to velocity in cartesian coordinates. More... | |
![]() | |
virtual | ~Thread () |
Virtual destructor. More... | |
virtual void | init () |
Initialize the thread. More... | |
bool | prepare_finalize () |
Prepare finalization. More... | |
virtual bool | prepare_finalize_user () |
Prepare finalization user implementation. More... | |
virtual void | finalize () |
Finalize the thread. More... | |
void | cancel_finalize () |
Cancel finalization. More... | |
void | start (bool wait=true) |
Call this method to start the thread. More... | |
void | cancel () |
Cancel a thread. More... | |
void | join () |
Join the thread. More... | |
void | detach () |
Detach the thread. More... | |
void | kill (int sig) |
Send signal to a thread. More... | |
bool | operator== (const Thread &thread) |
Check if two threads are the same. More... | |
void | wakeup () |
Wake up thread. More... | |
void | wakeup (Barrier *barrier) |
Wake up thread and wait for barrier afterwards. More... | |
void | wait_loop_done () |
Wait for the current loop iteration to finish. More... | |
OpMode | opmode () const |
Get operation mode. More... | |
pthread_t | thread_id () const |
Get ID of thread. More... | |
bool | started () const |
Check if thread has been started. More... | |
bool | cancelled () const |
Check if thread has been cancelled. More... | |
bool | detached () const |
Check if thread has been detached. More... | |
bool | running () const |
Check if the thread is running. More... | |
bool | waiting () const |
Check if thread is currently waiting for wakeup. More... | |
const char * | name () const |
Get name of thread. More... | |
void | set_flags (uint32_t flags) |
Set all flags in one go. More... | |
void | set_flag (uint32_t flag) |
Set flag for the thread. More... | |
void | unset_flag (uint32_t flag) |
Unset flag. More... | |
bool | flagged_bad () const |
Check if FLAG_BAD was set. More... | |
void | set_delete_on_exit (bool del) |
Set whether the thread should be deleted on exit. More... | |
void | set_prepfin_hold (bool hold) |
Hold prepare_finalize(). More... | |
void | add_notification_listener (ThreadNotificationListener *notification_listener) |
Add notification listener. More... | |
void | remove_notification_listener (ThreadNotificationListener *notification_listener) |
Remove notification listener. More... | |
void | notify_of_failed_init () |
Notify of failed init. More... | |
![]() | |
ClockAspect () | |
Constructor. More... | |
virtual | ~ClockAspect () |
Virtual empty destructor. More... | |
void | init_ClockAspect (Clock *clock) |
Set the clock. More... | |
![]() | |
const std::list< const char * > & | get_aspects () const |
Get list of aspect names attached to a aspected thread. More... | |
![]() | |
LoggingAspect () | |
Constructor. More... | |
virtual | ~LoggingAspect () |
Virtual empty Destructor. More... | |
void | init_LoggingAspect (Logger *logger) |
Set the logger. More... | |
Protected Member Functions | |
bool | update_velocities () |
Update velocity values. More... | |
![]() | |
Thread (const char *name) | |
Constructor. More... | |
Thread (const char *name, OpMode op_mode) | |
Constructor. More... | |
void | exit () |
Exit the thread. More... | |
void | test_cancel () |
Set cancellation point. More... | |
void | yield () |
Yield the processor to another thread or process. More... | |
virtual void | run () |
Code to execute in the thread. More... | |
void | set_opmode (OpMode op_mode) |
Set operation mode. More... | |
void | set_prepfin_conc_loop (bool concurrent=true) |
Set concurrent execution of prepare_finalize() and loop(). More... | |
void | set_coalesce_wakeups (bool coalesce=true) |
Set wakeup coalescing. More... | |
void | set_name (const char *format,...) |
Set name of thread. More... | |
virtual void | once () |
Execute an action exactly once. More... | |
virtual void | loop () |
Code to execute in the thread. More... | |
bool | wakeup_pending () |
Check if wakeups are pending. More... | |
![]() | |
void | add_aspect (const char *name) |
Add an aspect to a thread. More... | |
Protected Attributes | |
fawkes::Mutex * | data_mutex_ |
Mutex to protect data_. More... | |
SensorData | data_ |
Data struct that must be updated whenever new data is available. More... | |
bool | new_data_ |
Flag to indicate new data, set to true if data_ is modified. More... | |
![]() | |
bool | finalize_prepared |
True if prepare_finalize() has been called and was not stopped with a cancel_finalize(), false otherwise. More... | |
Mutex * | loop_mutex |
Mutex that is used to protect a call to loop(). More... | |
Mutex * | loopinterrupt_antistarve_mutex |
Mutex to avoid starvation when trying to lock loop_mutex. More... | |
![]() | |
Clock * | clock |
By means of this member access to the clock is given. More... | |
![]() | |
Logger * | logger |
This is the Logger member used to access the logger. More... | |
Additional Inherited Members | |
![]() | |
enum | OpMode { OPMODE_CONTINUOUS, OPMODE_WAITFORWAKEUP } |
Thread operation mode. More... | |
enum | CancelState { CANCEL_ENABLED, CANCEL_DISABLED } |
Cancel state. More... | |
![]() | |
static Thread * | current_thread () |
Get the Thread instance of the currently running thread. More... | |
static Thread * | current_thread_noexc () throw () |
Similar to current_thread, but does never throw an exception. More... | |
static pthread_t | current_thread_id () |
Get the ID of the currently running thread. More... | |
static void | init_main () |
Initialize Thread wrapper instance for main thread. More... | |
static void | destroy_main () |
Destroy main thread wrapper instance. More... | |
static void | set_cancel_state (CancelState new_state, CancelState *old_state=0) |
Set the cancel state of the current thread. More... | |
![]() | |
static const unsigned int | FLAG_BAD = 0x00000001 |
Standard thread flag: "thread is bad". More... | |
Virtual base class for thread that communicates with a Robotino.
A communication thread is always continuous and must communicate at the required pace. It provides hook for sensor and act threads.
Definition at line 39 of file com_thread.h.
RobotinoComThread::RobotinoComThread | ( | const char * | thread_name | ) |
Constructor.
thread_name | name of thread |
Definition at line 105 of file com_thread.cpp.
References data_mutex_, and new_data_.
|
virtual |
|
pure virtual |
Get actual velocity.
a1 | upon return contains velocity in RPM for first wheel |
a2 | upon return contains velocity in RPM for second wheel |
a3 | upon return contains velocity in RPM for third wheel |
seq | upon return contains sequence number of latest data |
t | upon return contains time of latest data |
Implemented in OpenRobotinoComThread, and DirectRobotinoComThread.
Referenced by RobotinoActThread::loop().
|
virtual |
Get all current sensor data.
sensor_data | upon return (true) contains the latest available sensor data |
sensor_data
, false otherwise Definition at line 155 of file com_thread.cpp.
References data_, data_mutex_, and new_data_.
Referenced by RobotinoSensorThread::loop().
|
pure virtual |
Get latest odometry value.
x | upon return contains x coordinate of odometry |
y | upon return contains y coordinate of odometry |
phi | upon return contains rptation of odometry |
Implemented in OpenRobotinoComThread, and DirectRobotinoComThread.
Referenced by RobotinoActThread::loop().
|
pure virtual |
Check if we are connected to OpenRobotino.
Implemented in OpenRobotinoComThread, and DirectRobotinoComThread.
Referenced by RobotinoActThread::loop().
|
pure virtual |
Check if gripper is open.
Implemented in OpenRobotinoComThread, and DirectRobotinoComThread.
Referenced by RobotinoActThread::loop().
void RobotinoComThread::project | ( | float * | m1, |
float * | m2, | ||
float * | m3, | ||
float | vx, | ||
float | vy, | ||
float | omega | ||
) | const |
Project the velocity of the robot in cartesian coordinates to single motor speeds.
From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany. The code has been released under a 2-clause BSD license.
m1 | The resulting speed of motor 1 in rpm |
m2 | The resulting speed of motor 2 in rpm |
m3 | The resulting speed of motor 3 in rpm |
vx | Velocity in x-direction in m/s |
vy | Velocity in y-direction in m/s |
omega | Angular velocity in rad/s |
Definition at line 346 of file com_thread.cpp.
Referenced by update_velocities().
|
pure virtual |
Reset odometry to zero.
Implemented in OpenRobotinoComThread, and DirectRobotinoComThread.
Referenced by RobotinoActThread::loop().
|
pure virtual |
Enable or disable emergency stop on bumper contact.
enabled | true to enable, false to disable |
Implemented in OpenRobotinoComThread, and DirectRobotinoComThread.
Referenced by RobotinoSensorThread::loop(), and RobotinoActThread::once().
|
virtual |
Set desired velocities.
vx | desired velocity in base_link frame X direction ("forward") |
vy | desired velocity in base_link frame Y direction ("sideward") |
omega | desired rotational velocity |
Reimplemented in DirectRobotinoComThread.
Definition at line 204 of file com_thread.cpp.
Referenced by RobotinoActThread::loop(), and DirectRobotinoComThread::set_desired_vel().
|
pure virtual |
Set digital output state.
digital_out | digital output as written on the robot, i.e., 1 to 8 |
enable | true to enable output, false to disable |
Implemented in OpenRobotinoComThread, and DirectRobotinoComThread.
Referenced by RobotinoSensorThread::loop().
void RobotinoComThread::set_drive_layout | ( | float | rb, |
float | rw, | ||
float | gear | ||
) |
Set omni drive layout parameters.
rb | Distance from Robotino center to wheel center in meters |
rw | Wheel radius in meters |
gear | Gear ratio between motors and wheels |
Definition at line 174 of file com_thread.cpp.
Referenced by RobotinoActThread::init().
void RobotinoComThread::set_drive_limits | ( | float | trans_accel, |
float | trans_decel, | ||
float | rot_accel, | ||
float | rot_decel | ||
) |
Set the omni drive limits.
trans_accel | maximum acceleration in translation |
trans_decel | maximum deceleration in translation |
rot_accel | maximum acceleration in rotation |
rot_decel | maximum deceleration in rotation |
Definition at line 189 of file com_thread.cpp.
Referenced by RobotinoActThread::init().
|
pure virtual |
Open or close gripper.
opened | true to open gripper, false to close |
Implemented in OpenRobotinoComThread, and DirectRobotinoComThread.
Referenced by RobotinoActThread::loop().
|
pure virtual |
Set acceleration limits of motors.
min_accel | minimum acceleration |
max_accel | maximum acceleration |
Implemented in OpenRobotinoComThread, and DirectRobotinoComThread.
|
pure virtual |
Set speed points for wheels.
s1 | speed point for first wheel in RPM |
s2 | speed point for second wheel in RPM |
s3 | speed point for third wheel in RPM |
Implemented in OpenRobotinoComThread, and DirectRobotinoComThread.
Referenced by RobotinoActThread::finalize(), and update_velocities().
void RobotinoComThread::unproject | ( | float * | vx, |
float * | vy, | ||
float * | omega, | ||
float | m1, | ||
float | m2, | ||
float | m3 | ||
) | const |
Project single motor speeds to velocity in cartesian coordinates.
From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany. The code has been released under a 2-clause BSD license.
vx | The resulting speed in x-direction in m/s |
vy | The resulting speed in y-direction in m/s |
omega | The resulting angular velocity in rad/s |
m1 | Speed of motor 1 in rpm |
m2 | Speed of motor 2 in rpm |
m3 | Speed of motor 3 in rpm |
RobotinoException | if no valid drive layout parameters are available. |
Definition at line 379 of file com_thread.cpp.
Referenced by RobotinoActThread::loop().
|
protected |
Update velocity values.
This method must be called periodically while driving to update the controller.
Definition at line 217 of file com_thread.cpp.
References fawkes::ClockAspect::clock, project(), and set_speed_points().
Referenced by DirectRobotinoComThread::set_desired_vel().
|
protected |
Data struct that must be updated whenever new data is available.
Definition at line 116 of file com_thread.h.
Referenced by DirectRobotinoComThread::get_act_velocity(), get_data(), DirectRobotinoComThread::get_odometry(), OpenRobotinoComThread::init(), DirectRobotinoComThread::loop(), OpenRobotinoComThread::loop(), DirectRobotinoComThread::set_bumper_estop_enabled(), and DirectRobotinoComThread::set_digital_output().
|
protected |
Mutex to protect data_.
Lock whenever accessing it.
Definition at line 114 of file com_thread.h.
Referenced by DirectRobotinoComThread::get_act_velocity(), OpenRobotinoComThread::get_act_velocity(), get_data(), DirectRobotinoComThread::get_odometry(), OpenRobotinoComThread::get_odometry(), DirectRobotinoComThread::is_gripper_open(), OpenRobotinoComThread::is_gripper_open(), DirectRobotinoComThread::loop(), OpenRobotinoComThread::loop(), RobotinoComThread(), DirectRobotinoComThread::set_bumper_estop_enabled(), DirectRobotinoComThread::set_digital_output(), and ~RobotinoComThread().
|
protected |
Flag to indicate new data, set to true if data_ is modified.
Definition at line 118 of file com_thread.h.
Referenced by get_data(), DirectRobotinoComThread::loop(), OpenRobotinoComThread::loop(), RobotinoComThread(), and DirectRobotinoComThread::set_digital_output().