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 #include "com_thread.h"
24 
25 #include <interfaces/BatteryInterface.h>
26 #include <interfaces/RobotinoSensorInterface.h>
27 #include <interfaces/IMUInterface.h>
28 
29 using namespace fawkes;
30 
31 /** @class RobotinoSensorThread "sensor_thread.h"
32  * Robotino sensor hook integration thread.
33  * This thread integrates into the Fawkes main loop at the SENSOR hook and
34  * writes new sensor data.
35  * @author Tim Niemueller
36  */
37 
38 /// taken from Robotino API2 DistanceSensorImpl.hpp
39 const std::vector<std::pair<double, double> > VOLTAGE_TO_DIST_DPS =
40  {
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}
44  };
45 
46 
47 /** Constructor.
48  * @param com_thread communication thread to trigger for writing data
49  */
51  : Thread("RobotinoSensorThread", Thread::OPMODE_WAITFORWAKEUP),
52  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
53 {
54  com_ = com_thread;
55 }
56 
57 
58 void
60 {
61  cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
62  cfg_imu_iface_id_ = config->get_string("/hardware/robotino/gyro/interface_id");
63 
64  batt_if_ = NULL;
65  sens_if_ = NULL;
66  imu_if_ = NULL;
67 
68  batt_if_ = blackboard->open_for_writing<BatteryInterface>("Robotino");
69  sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
70 
71  if (cfg_enable_gyro_) {
72  imu_if_ = blackboard->open_for_writing<IMUInterface>(cfg_imu_iface_id_.c_str());
73  }
74 }
75 
76 
77 void
79 {
80  blackboard->close(sens_if_);
81  blackboard->close(batt_if_);
82  blackboard->close(imu_if_);
83 }
84 
85 void
87 {
88  process_sensor_msgs();
89 
91  if (com_->get_data(data)) {
92  sens_if_->set_mot_velocity(data.mot_velocity);
93  sens_if_->set_mot_position(data.mot_position);
94  sens_if_->set_mot_current(data.mot_current);
95  sens_if_->set_bumper(data.bumper);
96  sens_if_->set_bumper_estop_enabled(data.bumper_estop_enabled);
97  sens_if_->set_digital_in(data.digital_in);
98  sens_if_->set_digital_out(data.digital_out);
99  sens_if_->set_analog_in(data.analog_in);
100  update_distances(data.ir_voltages);
101  sens_if_->write();
102 
103  batt_if_->set_voltage(data.bat_voltage);
104  batt_if_->set_current(data.bat_current);
105  batt_if_->set_absolute_soc(data.bat_absolute_soc);
106  batt_if_->write();
107 
108  if (cfg_enable_gyro_) {
109  if (data.imu_enabled) {
110  imu_if_->set_angular_velocity(data.imu_angular_velocity);
111  imu_if_->set_angular_velocity_covariance(data.imu_angular_velocity_covariance);
112  imu_if_->set_orientation(data.imu_orientation);
113  imu_if_->write();
114  } else {
115  if (fabs(data.imu_angular_velocity[0] + 1.) > 0.00001) {
116  imu_if_->set_linear_acceleration(0, -1.);
117  imu_if_->set_angular_velocity(0, -1.);
118  imu_if_->set_angular_velocity(2, 0.);
119  imu_if_->set_orientation(0, -1.);
120  imu_if_->write();
121  }
122  }
123  }
124  }
125 }
126 
127 
128 void
129 RobotinoSensorThread::process_sensor_msgs()
130 {
131  // process command messages
132  while (! sens_if_->msgq_empty()) {
134  sens_if_->msgq_first_safe(msg))
135  {
136  com_->set_bumper_estop_enabled(msg->is_enabled());
138  sens_if_->msgq_first_safe(msg))
139  {
140  try {
141  com_->set_digital_output(msg->digital_out(), msg->is_enabled());
142  } catch (Exception &e) {
143  logger->log_warn(name(), e);
144  }
145  }
146  sens_if_->msgq_pop();
147  } // while sensor msgq
148 }
149 
150 void
151 RobotinoSensorThread::update_distances(float *voltages)
152 {
153  float dist_m[NUM_IR_SENSORS];
154  const size_t num_dps = VOLTAGE_TO_DIST_DPS.size();
155 
156  for (int i = 0; i < NUM_IR_SENSORS; ++i) {
157  dist_m[i] = 0.;
158  // find the two enclosing data points
159 
160  for (size_t j = 0; j < num_dps - 1; ++j) {
161  // This determines two points, l(eft) and r(ight) that are
162  // defined by voltage (x coord) and distance (y coord). We
163  // assume a linear progression between two adjacent points,
164  // i.e. between l and r. We then do the following:
165  // 1. Find two adjacent voltage values lv and rv where
166  // the voltage lies inbetween
167  // 2. Interpolate by calculating the line parameters
168  // m = dd/dv, x = voltage - lv and b = ld.
169  // cf. http://www.acroname.com/robotics/info/articles/irlinear/irlinear.html
170 
171  const double lv = VOLTAGE_TO_DIST_DPS[j ].first;
172  const double rv = VOLTAGE_TO_DIST_DPS[j+1].first;
173 
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;
177 
178  double dv = rv - lv;
179  double dd = rd - ld;
180 
181  // Linear interpolation between
182  dist_m[i] = (dd / dv) * (voltages[i] - lv) + ld;
183  break;
184  }
185  }
186  }
187 
188  sens_if_->set_distance(dist_m);
189 }
Struct to exchange data between com and sensor thread.
Definition: com_thread.h:45
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
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.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
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.
Definition: logging.h:44
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.
Definition: interface.cpp:1193
virtual void init()
Initialize the thread.
Base class for exceptions in Fawkes.
Definition: exception.h:36
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.
Definition: com_thread.h:39
const char * name() const
Get name of thread.
Definition: thread.h:95
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.
Definition: com_thread.cpp:155
void set_digital_in(unsigned int index, const bool new_digital_in)
Set digital_in value at given index.
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:33
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.
Definition: interface.h:302
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.
Definition: configurable.h:44
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.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.