22 #include "sensor_thread.h" 23 #include "com_thread.h" 25 #include <interfaces/BatteryInterface.h> 26 #include <interfaces/RobotinoSensorInterface.h> 27 #include <interfaces/IMUInterface.h> 39 const std::vector<std::pair<double, double> > VOLTAGE_TO_DIST_DPS =
41 {0.3 , 0.41}, {0.39, 0.35}, {0.41, 0.30}, {0.5 , 0.25}, {0.75, 0.18},
42 {0.8 , 0.16}, {0.95, 0.14}, {1.05, 0.12}, {1.3 , 0.10}, {1.4 , 0.09},
43 {1.55, 0.08}, {1.8 , 0.07}, {2.35, 0.05}, {2.55, 0.04}
51 :
Thread(
"RobotinoSensorThread",
Thread::OPMODE_WAITFORWAKEUP),
61 cfg_enable_gyro_ =
config->
get_bool(
"/hardware/robotino/gyro/enable");
62 cfg_imu_iface_id_ =
config->
get_string(
"/hardware/robotino/gyro/interface_id");
71 if (cfg_enable_gyro_) {
88 process_sensor_msgs();
100 update_distances(data.ir_voltages);
108 if (cfg_enable_gyro_) {
109 if (data.imu_enabled) {
115 if (fabs(data.imu_angular_velocity[0] + 1.) > 0.00001) {
129 RobotinoSensorThread::process_sensor_msgs()
151 RobotinoSensorThread::update_distances(
float *voltages)
153 float dist_m[NUM_IR_SENSORS];
154 const size_t num_dps = VOLTAGE_TO_DIST_DPS.size();
156 for (
int i = 0; i < NUM_IR_SENSORS; ++i) {
160 for (
size_t j = 0; j < num_dps - 1; ++j) {
171 const double lv = VOLTAGE_TO_DIST_DPS[j ].first;
172 const double rv = VOLTAGE_TO_DIST_DPS[j+1].first;
174 if ((voltages[i] >= lv) && (voltages[i] < rv)) {
175 const double ld = VOLTAGE_TO_DIST_DPS[j ].second;
176 const double rd = VOLTAGE_TO_DIST_DPS[j+1].second;
182 dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;
Struct to exchange data between com and sensor thread.
bool msgq_empty()
Check if queue is empty.
void set_absolute_soc(const float new_absolute_soc)
Set absolute_soc value.
RobotinoSensorThread(RobotinoComThread *com_thread)
Constructor.
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual void finalize()
Finalize the thread.
BatteryInterface Fawkes BlackBoard Interface.
void set_voltage(const uint32_t new_voltage)
Set voltage value.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
virtual void set_digital_output(unsigned int digital_out, bool enable)=0
Set digital output state.
Logger * logger
This is the Logger member used to access the logger.
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
void set_analog_in(unsigned int index, const float new_analog_in)
Set analog_in value at given index.
RobotinoSensorInterface Fawkes BlackBoard Interface.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
virtual void init()
Initialize the thread.
Base class for exceptions in Fawkes.
virtual void set_bumper_estop_enabled(bool enabled)=0
Enable or disable emergency stop on bumper contact.
void set_bumper_estop_enabled(const bool new_bumper_estop_enabled)
Set bumper_estop_enabled value.
SetBumperEStopEnabledMessage Fawkes BlackBoard Interface Message.
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
Virtual base class for thread that communicates with a Robotino.
const char * name() const
Get name of thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void set_digital_out(unsigned int index, const bool new_digital_out)
Set digital_out value at given index.
void set_mot_velocity(unsigned int index, const float new_mot_velocity)
Set mot_velocity value at given index.
virtual bool get_data(SensorData &sensor_data)
Get all current sensor data.
void set_digital_in(unsigned int index, const bool new_digital_in)
Set digital_in value at given index.
IMUInterface Fawkes BlackBoard Interface.
void set_current(const uint32_t new_current)
Set current value.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
void set_distance(unsigned int index, const float new_distance)
Set distance value at given index.
SetDigitalOutputMessage Fawkes BlackBoard Interface Message.
void set_mot_current(unsigned int index, const float new_mot_current)
Set mot_current value at given index.
void set_mot_position(unsigned int index, const int32_t new_mot_position)
Set mot_position value at given index.
virtual void loop()
Code to execute in the thread.
void set_bumper(const bool new_bumper)
Set bumper value.
Configuration * config
This is the Configuration member used to access the configuration.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
void set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
Set linear_acceleration value at given index.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.