21 #ifndef __PLUGINS_ROBOTINO_COM_THREAD_H_ 22 #define __PLUGINS_ROBOTINO_COM_THREAD_H_ 24 #include <core/threading/thread.h> 25 #include <aspect/logging.h> 26 #include <aspect/clock.h> 28 #include <utils/time/time.h> 37 #define NUM_IR_SENSORS 9 51 float mot_velocity[3];
52 int32_t mot_position[3];
55 bool bumper_estop_enabled;
62 float bat_absolute_soc;
65 float imu_orientation[4];
66 float imu_angular_velocity[3];
67 double imu_angular_velocity_covariance[9];
73 float ir_voltages[NUM_IR_SENSORS];
82 virtual bool is_connected() = 0;
84 virtual void set_gripper(
bool opened) = 0;
85 virtual bool is_gripper_open() = 0;
86 virtual void set_speed_points(
float s1,
float s2,
float s3) = 0;
87 virtual void get_act_velocity(
float &a1,
float &a2,
float &a3,
unsigned int &seq,
fawkes::Time &t) = 0;
88 virtual void get_odometry(
double &x,
double &y,
double &phi) = 0;
90 virtual void reset_odometry() = 0;
91 virtual void set_bumper_estop_enabled(
bool enabled) = 0;
92 virtual void set_motor_accel_limits(
float min_accel,
float max_accel) = 0;
93 virtual void set_digital_output(
unsigned int digital_out,
bool enable) = 0;
95 virtual bool get_data(
SensorData &sensor_data);
97 void set_drive_layout(
float rb,
float rw,
float gear);
98 void set_drive_limits(
float trans_accel,
float trans_decel,
float rot_accel,
float rot_decel);
99 virtual void set_desired_vel(
float vx,
float vy,
float omega);
102 void project(
float *m1,
float *m2,
float *m3,
float vx,
float vy,
float omega)
const;
103 void unproject(
float *vx,
float *vy,
float *omega,
float m1,
float m2,
float m3)
const;
106 bool update_velocities();
110 float update_speed(
float des,
float set,
float accel,
float decel,
float diff_sec);
124 float cfg_trans_accel_;
125 float cfg_trans_decel_;
126 float cfg_rot_accel_;
127 float cfg_rot_decel_;
140 #ifdef USE_VELOCITY_RECORDING SensorData data_
Data struct that must be updated whenever new data is available.
Struct to exchange data between com and sensor thread.
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
Thread aspect that allows to obtain the current time from the clock.
Fawkes library namespace.
A class for handling time.
Thread class encapsulation of pthreads.
fawkes::Mutex * data_mutex_
Mutex to protect data_.
Virtual base class for thread that communicates with a Robotino.
Thread aspect to log output.
Mutex mutual exclusion lock.