Fawkes API Fawkes Development Version
sensor_thread.cpp
1
2/***************************************************************************
3 * sensor_thread.cpp - Robotino sensor thread
4 *
5 * Created: Sun Nov 13 15:35:24 2011
6 * Copyright 2011-2014 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "sensor_thread.h"
23
24#include "com_thread.h"
25
26#include <interfaces/BatteryInterface.h>
27#include <interfaces/IMUInterface.h>
28#include <interfaces/RobotinoSensorInterface.h>
29
30using namespace fawkes;
31
32/** @class RobotinoSensorThread "sensor_thread.h"
33 * Robotino sensor hook integration thread.
34 * This thread integrates into the Fawkes main loop at the SENSOR hook and
35 * writes new sensor data.
36 * @author Tim Niemueller
37 */
38
39/// taken from Robotino API2 DistanceSensorImpl.hpp
40const std::vector<std::pair<double, double>> VOLTAGE_TO_DIST_DPS = {{0.3, 0.41},
41 {0.39, 0.35},
42 {0.41, 0.30},
43 {0.5, 0.25},
44 {0.75, 0.18},
45 {0.8, 0.16},
46 {0.95, 0.14},
47 {1.05, 0.12},
48 {1.3, 0.10},
49 {1.4, 0.09},
50 {1.55, 0.08},
51 {1.8, 0.07},
52 {2.35, 0.05},
53 {2.55, 0.04}};
54
55/** Constructor.
56 * @param com_thread communication thread to trigger for writing data
57 */
59: Thread("RobotinoSensorThread", Thread::OPMODE_WAITFORWAKEUP),
60 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
61{
62 com_ = com_thread;
63}
64
65void
67{
68 cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
69 cfg_imu_iface_id_ = config->get_string("/hardware/robotino/gyro/interface_id");
70
71 batt_if_ = NULL;
72 sens_if_ = NULL;
73 imu_if_ = NULL;
74
75 batt_if_ = blackboard->open_for_writing<BatteryInterface>("Robotino");
77
78 if (cfg_enable_gyro_) {
79 imu_if_ = blackboard->open_for_writing<IMUInterface>(cfg_imu_iface_id_.c_str());
80 }
81}
82
83void
85{
86 blackboard->close(sens_if_);
87 blackboard->close(batt_if_);
88 blackboard->close(imu_if_);
89}
90
91void
93{
94 process_sensor_msgs();
95
97 if (com_->get_data(data)) {
98 sens_if_->set_mot_velocity(data.mot_velocity);
99 sens_if_->set_mot_position(data.mot_position);
100 sens_if_->set_mot_current(data.mot_current);
101 sens_if_->set_bumper(data.bumper);
102 sens_if_->set_bumper_estop_enabled(data.bumper_estop_enabled);
103 sens_if_->set_digital_in(data.digital_in);
104 sens_if_->set_digital_out(data.digital_out);
105 sens_if_->set_analog_in(data.analog_in);
106 update_distances(data.ir_voltages);
107 sens_if_->write();
108
109 batt_if_->set_voltage(data.bat_voltage);
110 batt_if_->set_current(data.bat_current);
111 batt_if_->set_absolute_soc(data.bat_absolute_soc);
112 batt_if_->write();
113
114 if (cfg_enable_gyro_) {
115 if (data.imu_enabled) {
116 imu_if_->set_angular_velocity(data.imu_angular_velocity);
117 imu_if_->set_angular_velocity_covariance(data.imu_angular_velocity_covariance);
118 imu_if_->set_orientation(data.imu_orientation);
119 imu_if_->write();
120 } else {
121 if (fabs(data.imu_angular_velocity[0] + 1.) > 0.00001) {
122 imu_if_->set_linear_acceleration(0, -1.);
123 imu_if_->set_angular_velocity(0, -1.);
124 imu_if_->set_angular_velocity(2, 0.);
125 imu_if_->set_orientation(0, -1.);
126 imu_if_->write();
127 }
128 }
129 }
130 }
131}
132
133void
134RobotinoSensorThread::process_sensor_msgs()
135{
136 // process command messages
137 while (!sens_if_->msgq_empty()) {
139 sens_if_->msgq_first_safe(msg)) {
140 com_->set_bumper_estop_enabled(msg->is_enabled());
142 sens_if_->msgq_first_safe(msg)) {
143 try {
144 com_->set_digital_output(msg->digital_out(), msg->is_enabled());
145 } catch (Exception &e) {
146 logger->log_warn(name(), e);
147 }
148 }
149 sens_if_->msgq_pop();
150 } // while sensor msgq
151}
152
153void
154RobotinoSensorThread::update_distances(float *voltages)
155{
156 float dist_m[NUM_IR_SENSORS];
157 const size_t num_dps = VOLTAGE_TO_DIST_DPS.size();
158
159 for (int i = 0; i < NUM_IR_SENSORS; ++i) {
160 dist_m[i] = 0.;
161 // find the two enclosing data points
162
163 for (size_t j = 0; j < num_dps - 1; ++j) {
164 // This determines two points, l(eft) and r(ight) that are
165 // defined by voltage (x coord) and distance (y coord). We
166 // assume a linear progression between two adjacent points,
167 // i.e. between l and r. We then do the following:
168 // 1. Find two adjacent voltage values lv and rv where
169 // the voltage lies inbetween
170 // 2. Interpolate by calculating the line parameters
171 // m = dd/dv, x = voltage - lv and b = ld.
172 // cf. http://www.acroname.com/robotics/info/articles/irlinear/irlinear.html
173
174 const double lv = VOLTAGE_TO_DIST_DPS[j].first;
175 const double rv = VOLTAGE_TO_DIST_DPS[j + 1].first;
176
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;
180
181 double dv = rv - lv;
182 double dd = rd - ld;
183
184 // Linear interpolation between
185 dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;
186 break;
187 }
188 }
189 }
190
191 sens_if_->set_distance(dist_m);
192}
Virtual base class for thread that communicates with a Robotino.
Definition: com_thread.h:41
virtual void set_bumper_estop_enabled(bool enabled)=0
Enable or disable emergency stop on bumper contact.
virtual bool get_data(SensorData &sensor_data)
Get all current sensor data.
Definition: com_thread.cpp:165
virtual void set_digital_output(unsigned int digital_out, bool enable)=0
Set digital output state.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
RobotinoSensorThread(RobotinoComThread *com_thread)
Constructor.
BatteryInterface Fawkes BlackBoard Interface.
void set_voltage(const uint32_t new_voltage)
Set voltage value.
void set_current(const uint32_t new_current)
Set current value.
void set_absolute_soc(const float new_absolute_soc)
Set absolute_soc value.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:34
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
void set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
Set linear_acceleration value at given index.
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
Definition: interface.h:340
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
SetBumperEStopEnabledMessage Fawkes BlackBoard Interface Message.
SetDigitalOutputMessage Fawkes BlackBoard Interface Message.
RobotinoSensorInterface Fawkes BlackBoard Interface.
void set_digital_in(unsigned int index, const bool new_digital_in)
Set digital_in value at given index.
void set_mot_position(unsigned int index, const int32_t new_mot_position)
Set mot_position value at given index.
void set_bumper(const bool new_bumper)
Set bumper value.
void set_mot_current(unsigned int index, const float new_mot_current)
Set mot_current value at given index.
void set_bumper_estop_enabled(const bool new_bumper_estop_enabled)
Set bumper_estop_enabled value.
void set_analog_in(unsigned int index, const float new_analog_in)
Set analog_in value at given index.
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.
void set_distance(unsigned int index, const float new_distance)
Set distance value at given index.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.
Struct to exchange data between com and sensor thread.
Definition: com_thread.h:44