21#ifndef _PLUGINS_ROBOTINO_COM_THREAD_H_
22#define _PLUGINS_ROBOTINO_COM_THREAD_H_
24#include <aspect/clock.h>
25#include <aspect/logging.h>
26#include <core/threading/thread.h>
27#include <utils/time/time.h>
36#define NUM_IR_SENSORS 9
50 float mot_velocity[3];
51 int32_t mot_position[3];
54 bool bumper_estop_enabled;
61 float bat_absolute_soc;
64 float imu_orientation[4];
65 float imu_angular_velocity[3];
66 double imu_angular_velocity_covariance[9];
72 float ir_voltages[NUM_IR_SENSORS];
98 void set_drive_limits(
float trans_accel,
float trans_decel,
float rot_accel,
float rot_decel);
101 void project(
float *m1,
float *m2,
float *m3,
float vx,
float vy,
float omega)
const;
102 void unproject(
float *vx,
float *vy,
float *omega,
float m1,
float m2,
float m3)
const;
108 float update_speed(
float des,
float set,
float accel,
float decel,
float diff_sec);
122 float cfg_trans_accel_;
123 float cfg_trans_decel_;
124 float cfg_rot_accel_;
125 float cfg_rot_decel_;
138#ifdef USE_VELOCITY_RECORDING
Virtual base class for thread that communicates with a Robotino.
virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)=0
Get actual velocity.
virtual void reset_odometry()=0
Reset odometry to zero.
virtual void set_gripper(bool opened)=0
Open or close gripper.
void set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel)
Set the omni drive limits.
virtual void get_odometry(double &x, double &y, double &phi)=0
Get latest odometry value.
virtual bool is_connected()=0
Check if we are connected to OpenRobotino.
virtual void set_bumper_estop_enabled(bool enabled)=0
Enable or disable emergency stop on bumper contact.
virtual void set_speed_points(float s1, float s2, float s3)=0
Set speed points for wheels.
virtual void set_motor_accel_limits(float min_accel, float max_accel)=0
Set acceleration limits of motors.
fawkes::Mutex * data_mutex_
Mutex to protect data_.
void set_drive_layout(float rb, float rw, float gear)
Set omni drive layout parameters.
virtual bool is_gripper_open()=0
Check if gripper is open.
RobotinoComThread(const char *thread_name)
Constructor.
virtual void set_desired_vel(float vx, float vy, float omega)
Set desired velocities.
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.
virtual bool get_data(SensorData &sensor_data)
Get all current sensor data.
void unproject(float *vx, float *vy, float *omega, float m1, float m2, float m3) const
Project single motor speeds to velocity in cartesian coordinates.
virtual ~RobotinoComThread()
Destructor.
bool update_velocities()
Update velocity values.
SensorData data_
Data struct that must be updated whenever new data is available.
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
virtual void set_digital_output(unsigned int digital_out, bool enable)=0
Set digital output state.
Thread aspect that allows to obtain the current time from the clock.
Thread aspect to log output.
Mutex mutual exclusion lock.
Thread class encapsulation of pthreads.
A class for handling time.
Fawkes library namespace.
Struct to exchange data between com and sensor thread.