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 
30 using 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
40 const 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 
65 void
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");
76  sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
77 
78  if (cfg_enable_gyro_) {
79  imu_if_ = blackboard->open_for_writing<IMUInterface>(cfg_imu_iface_id_.c_str());
80  }
81 }
82 
83 void
85 {
86  blackboard->close(sens_if_);
87  blackboard->close(batt_if_);
88  blackboard->close(imu_if_);
89 }
90 
91 void
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 
133 void
134 RobotinoSensorThread::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 
153 void
154 RobotinoSensorThread::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 }
fawkes::RobotinoSensorInterface::set_digital_in
void set_digital_in(unsigned int index, const bool new_digital_in)
Set digital_in value at given index.
Definition: RobotinoSensorInterface.cpp:401
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1184
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1031
RobotinoSensorThread::finalize
virtual void finalize()
Finalize the thread.
Definition: sensor_thread.cpp:84
fawkes::RobotinoSensorInterface::set_mot_position
void set_mot_position(unsigned int index, const int32_t new_mot_position)
Set mot_position value at given index.
Definition: RobotinoSensorInterface.cpp:190
fawkes::IMUInterface::set_angular_velocity_covariance
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
Definition: IMUInterface.cpp:395
fawkes::IMUInterface
Definition: IMUInterface.h:39
fawkes::RobotinoSensorInterface::set_bumper_estop_enabled
void set_bumper_estop_enabled(const bool new_bumper_estop_enabled)
Set bumper_estop_enabled value.
Definition: RobotinoSensorInterface.cpp:558
fawkes::Interface::msgq_first_safe
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:303
fawkes::RobotinoSensorInterface::SetDigitalOutputMessage
Definition: RobotinoSensorInterface.h:103
fawkes::RobotinoSensorInterface::set_mot_velocity
void set_mot_velocity(unsigned int index, const float new_mot_velocity)
Set mot_velocity value at given index.
Definition: RobotinoSensorInterface.cpp:130
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
fawkes::RobotinoSensorInterface::set_mot_current
void set_mot_current(unsigned int index, const float new_mot_current)
Set mot_current value at given index.
Definition: RobotinoSensorInterface.cpp:250
fawkes::RobotinoSensorInterface::set_analog_in
void set_analog_in(unsigned int index, const float new_analog_in)
Set analog_in value at given index.
Definition: RobotinoSensorInterface.cpp:521
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:56
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
RobotinoSensorThread::init
virtual void init()
Initialize the thread.
Definition: sensor_thread.cpp:66
fawkes::BatteryInterface::set_absolute_soc
void set_absolute_soc(const float new_absolute_soc)
Set absolute_soc value.
Definition: BatteryInterface.cpp:194
RobotinoComThread::get_data
virtual bool get_data(SensorData &sensor_data)
Get all current sensor data.
Definition: com_thread.cpp:165
fawkes::IMUInterface::set_linear_acceleration
void set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
Set linear_acceleration value at given index.
Definition: IMUInterface.cpp:463
RobotinoSensorThread::loop
virtual void loop()
Code to execute in the thread.
Definition: sensor_thread.cpp:92
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes
fawkes::RobotinoSensorInterface
Definition: RobotinoSensorInterface.h:39
RobotinoComThread::set_digital_output
virtual void set_digital_output(unsigned int digital_out, bool enable)=0
RobotinoComThread
Definition: com_thread.h:38
fawkes::RobotinoSensorInterface::set_distance
void set_distance(unsigned int index, const float new_distance)
Set distance value at given index.
Definition: RobotinoSensorInterface.cpp:341
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::BatteryInterface::set_voltage
void set_voltage(const uint32_t new_voltage)
Set voltage value.
Definition: BatteryInterface.cpp:132
fawkes::BatteryInterface
Definition: BatteryInterface.h:39
fawkes::BatteryInterface::set_current
void set_current(const uint32_t new_current)
Set current value.
Definition: BatteryInterface.cpp:101
fawkes::IMUInterface::set_orientation
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
Definition: IMUInterface.cpp:191
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
RobotinoComThread::SensorData
Definition: com_thread.h:43
RobotinoComThread::set_bumper_estop_enabled
virtual void set_bumper_estop_enabled(bool enabled)=0
fawkes::Thread
Definition: thread.h:45
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:49
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::RobotinoSensorInterface::SetBumperEStopEnabledMessage
Definition: RobotinoSensorInterface.h:75
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:499
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::RobotinoSensorInterface::set_bumper
void set_bumper(const bool new_bumper)
Set bumper value.
Definition: RobotinoSensorInterface.cpp:283
fawkes::RobotinoSensorInterface::set_digital_out
void set_digital_out(unsigned int index, const bool new_digital_out)
Set digital_out value at given index.
Definition: RobotinoSensorInterface.cpp:461
fawkes::IMUInterface::set_angular_velocity
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
Definition: IMUInterface.cpp:327
RobotinoSensorThread::RobotinoSensorThread
RobotinoSensorThread(RobotinoComThread *com_thread)
Constructor.
Definition: sensor_thread.cpp:58
fawkes::Exception
Definition: exception.h:41