Fawkes API
Fawkes Development Version
|
22 #include "sensor_thread.h"
24 #include "com_thread.h"
26 #include <interfaces/BatteryInterface.h>
27 #include <interfaces/IMUInterface.h>
28 #include <interfaces/RobotinoSensorInterface.h>
40 const std::vector<std::pair<double, double>> VOLTAGE_TO_DIST_DPS = {{0.3, 0.41},
59 :
Thread(
"RobotinoSensorThread",
Thread::OPMODE_WAITFORWAKEUP),
68 cfg_enable_gyro_ =
config->
get_bool(
"/hardware/robotino/gyro/enable");
69 cfg_imu_iface_id_ =
config->
get_string(
"/hardware/robotino/gyro/interface_id");
78 if (cfg_enable_gyro_) {
94 process_sensor_msgs();
106 update_distances(data.ir_voltages);
114 if (cfg_enable_gyro_) {
115 if (data.imu_enabled) {
121 if (fabs(data.imu_angular_velocity[0] + 1.) > 0.00001) {
134 RobotinoSensorThread::process_sensor_msgs()
154 RobotinoSensorThread::update_distances(
float *voltages)
156 float dist_m[NUM_IR_SENSORS];
157 const size_t num_dps = VOLTAGE_TO_DIST_DPS.size();
159 for (
int i = 0; i < NUM_IR_SENSORS; ++i) {
163 for (
size_t j = 0; j < num_dps - 1; ++j) {
174 const double lv = VOLTAGE_TO_DIST_DPS[j].first;
175 const double rv = VOLTAGE_TO_DIST_DPS[j + 1].first;
177 if ((voltages[i] >= lv) && (voltages[i] < rv)) {
178 const double ld = VOLTAGE_TO_DIST_DPS[j].second;
179 const double rd = VOLTAGE_TO_DIST_DPS[j + 1].second;
185 dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;
void set_digital_in(unsigned int index, const bool new_digital_in)
Set digital_in value at given index.
void msgq_pop()
Erase first message from queue.
bool msgq_empty()
Check if queue is empty.
virtual void finalize()
Finalize the thread.
void set_mot_position(unsigned int index, const int32_t new_mot_position)
Set mot_position value at given index.
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
void set_bumper_estop_enabled(const bool new_bumper_estop_enabled)
Set bumper_estop_enabled value.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
void set_mot_velocity(unsigned int index, const float new_mot_velocity)
Set mot_velocity value at given index.
virtual bool get_bool(const char *path)=0
void set_mot_current(unsigned int index, const float new_mot_current)
Set mot_current value at given index.
void set_analog_in(unsigned int index, const float new_analog_in)
Set analog_in value at given index.
const char * name() const
virtual void init()
Initialize the thread.
void set_absolute_soc(const float new_absolute_soc)
Set absolute_soc value.
virtual bool get_data(SensorData &sensor_data)
Get all current sensor data.
void set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
Set linear_acceleration value at given index.
virtual void loop()
Code to execute in the thread.
virtual void close(Interface *interface)=0
virtual void set_digital_output(unsigned int digital_out, bool enable)=0
void set_distance(unsigned int index, const float new_distance)
Set distance value at given index.
virtual void log_warn(const char *component, const char *format,...)=0
void set_voltage(const uint32_t new_voltage)
Set voltage value.
void set_current(const uint32_t new_current)
Set current value.
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
virtual void set_bumper_estop_enabled(bool enabled)=0
virtual std::string get_string(const char *path)=0
void write()
Write from local copy into BlackBoard memory.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
void set_bumper(const bool new_bumper)
Set bumper value.
void set_digital_out(unsigned int index, const bool new_digital_out)
Set digital_out value at given index.
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
RobotinoSensorThread(RobotinoComThread *com_thread)
Constructor.