25 #include "sensproc_thread.h"
26 #include "hough_transform.h"
28 #include <interfaces/Laser360Interface.h>
29 #include <interfaces/ObjectPositionInterface.h>
30 #include <interfaces/VisualDisplay2DInterface.h>
32 #include <utils/math/angle.h>
33 #include <utils/math/coord.h>
34 #ifdef LASERHT_TIMETRACKER
35 # include <utils/time/tracker.h>
40 using namespace fawkes;
52 :
Thread(
"LaserHtSensorProcThread",
Thread::OPMODE_WAITFORWAKEUP),
65 __cfg_num_samples =
config->
get_uint(
"/plugins/laserht/line/num_samples");
67 __cfg_laser_ifid =
config->
get_string(
"/plugins/laserht/laser_interface_id");
68 __cfg_enable_disp =
config->
get_bool(
"/plugins/laserht/line/enable_display");
69 __cfg_vote_threshold =
config->
get_uint(
"/plugins/laserht/line/vote_threshold");
70 __cfg_dist_threshold =
config->
get_float(
"/plugins/laserht/line/dist_threshold");
71 __cfg_fitting_error_threshold =
config->
get_float(
"/plugins/laserht/line/fitting_error_threshold");
77 if (__cfg_enable_disp) {
90 __num_vals = __cfg_num_samples;
91 __angle_step = 180.f / __num_vals;
92 __r_scale = __cfg_r_scale;
93 __values =
new int*[__num_vals];
94 for (
unsigned int i = 0; i < __num_vals; ++i) {
95 __values[i] =
new int[2];
98 #ifdef LASERHT_TIMETRACKER
101 __ttc_reset = __tt->add_class(
"Reset");
102 __ttc_process = __tt->add_class(
"Processing");
103 __ttc_fitting = __tt->add_class(
"Fitting");
104 __ttc_total = __tt->add_class(
"Total");
120 for (
unsigned int i = 0; i < __num_vals; ++i) {
121 delete[] __values[i];
128 LaserHtSensorProcThread::line_points_from_params(
float r,
float phi,
129 float &x1,
float &y1,
130 float &x2,
float &y2)
133 float phi_mod = phi - (floorf(phi / 90.) * 90);
134 float r_scaled = r * __r_scale;
140 float alpha, y_factor = 1;
141 if ( ((phi >= 0) && (phi < 90)) ||
148 float dx = 1 * cos(alpha);
149 float dy = 1 * y_factor * sin(alpha);
158 __laser360_if->
read();
159 float *distances = __laser360_if->
distances();
162 #ifdef LASERHT_TIMETRACKER
163 __tt->ping_start(__ttc_total);
164 __tt->ping_start(__ttc_reset);
167 #ifdef LASERHT_TIMETRACKER
168 __tt->ping_end(__ttc_reset);
169 __tt->ping_start(__ttc_process);
172 for (
size_t i = 0; i < num_dist; ++i) {
174 if (distances[i] > 0) {
175 for (
unsigned int j = 0; j < __num_vals; ++j) {
177 float theta =
deg2rad(j * __angle_step);
180 float r = x * cos(theta) + y * sin(theta);
182 __values[j][0] = (int)roundf(r);
183 __values[j][1] = (int)roundf(j * __angle_step);
185 __ht->
process(__values, __num_vals);
188 #ifdef LASERHT_TIMETRACKER
189 __tt->ping_end(__ttc_process);
193 unsigned int max_count = __ht->
max(max_values);
195 if (max_count >= __cfg_vote_threshold) {
196 float x1, y1, x2, y2;
197 line_points_from_params(max_values[0], max_values[1], x1, y1, x2, y2);
200 if (__cfg_enable_disp && __visdisp_if->
has_writer()) {
202 float x[2] = {x1, x2};
203 float y[2] = {y1, y2};
204 unsigned char color[4] = {0, 255, 0, 255};
207 VisualDisplay2DInterface::LS_SOLID, color);
230 float theta =
deg2rad(max_values[1]);
231 float alpha = 0.5 * M_PI - theta;
232 float r_scaled = max_values[0] * __r_scale;
233 float cos_alpha = cos(alpha);
234 float sin_alpha = sin(alpha);
235 float threshold = __cfg_dist_threshold;
236 float r_min = r_scaled - threshold;
237 float r_max = r_scaled + threshold;
239 bool first_x_minmax =
true;
240 float x_min = 0, x_max = 0;
242 std::vector<laser_reading_t> readings;
244 for (
size_t i = 0; i < num_dist; ++i) {
246 if (distances[i] > 0) {
250 float r = x * cos(theta) + y * sin(theta);
252 if ( (r >= r_min) && (r <= r_max) ) {
273 float x_rot = x * cos_alpha - y * sin_alpha;
274 float y_rot = x * sin_alpha + y * cos_alpha;
276 laser_reading_t l = {phi, distances[i], x_rot, y_rot};
277 readings.push_back(l);
278 if (first_x_minmax) {
279 first_x_minmax =
false;
283 if (x_rot < x_min) x_min = x_rot;
284 if (x_rot > x_max) x_max = x_rot;
290 #ifdef LASERHT_TIMETRACKER
291 __tt->ping_start(__ttc_fitting);
294 float a = 0, b = 0, e = 0;
295 fit_line(readings, 0, a, b, e);
296 #ifdef LASERHT_TIMETRACKER
297 __tt->ping_end(__ttc_fitting);
300 if ( e <= __cfg_fitting_error_threshold ) {
302 float y_min = a * x_min + b;
303 float y_max = a * x_max + b;
308 float x_min_rot = x_min * cos_alpha + y_min * sin_alpha;
309 float y_min_rot = y_min * cos_alpha - x_min * sin_alpha;
310 float x_max_rot = x_max * cos_alpha + y_max * sin_alpha;
311 float y_max_rot = y_max * cos_alpha - x_max * sin_alpha;
314 float alpha_fit = atan2f(y_max_rot - y_min_rot,
315 x_max_rot - x_min_rot);
316 if ( (theta <= 0.5 * M_PI) ||
317 ((theta >= M_PI) && (theta <= 1.5 * M_PI)) ) {
318 alpha_fit = 0.5 * M_PI + alpha_fit;
320 float theta_fit = floorf(theta / (0.5*M_PI)) * 0.5*M_PI + alpha_fit;
321 float r_fit = x_min_rot * cos(theta_fit) + y_min_rot * sin(theta_fit);
323 if (__cfg_enable_disp && __visdisp_if->
has_writer()) {
324 float x1, y1, x2, y2;
325 line_points_from_params(r_fit / __r_scale,
rad2deg(theta_fit),
330 float x[2] = {x1, x2};
331 float y[2] = {y1, y2};
332 unsigned char color[4] = {0, 0, 255, 255};
335 VisualDisplay2DInterface::LS_SOLID, color);
355 e, __cfg_fitting_error_threshold);
361 max_count, __cfg_vote_threshold);
367 #ifdef LASERHT_TIMETRACKER
368 __tt->ping_end(__ttc_total);
369 if (++__tt_loop >= 100) {
370 __tt->print_to_stdout();
376 #define sqr(x) ((x) * (x))
379 LaserHtSensorProcThread::fit_line(
const std::vector<laser_reading_t> &points,
380 const unsigned int first_index,
381 float &a,
float &b,
float &least_square_error)
383 const size_t n = points.size();
384 float sum_x = 0.0, sum_xy = 0.0, sum_y = 0.0, sum_xx = 0.0;
387 register float e = 0.0;
388 for (
size_t i = first_index; i < n; ++i ) {
399 b = ( sum_y * sum_xx - sum_x * sum_xy ) / ( n * sum_xx - sum_x * sum_x );
400 a = ( n * sum_xy - sum_x * sum_y ) / ( n * sum_xx - sum_x * sum_x );
403 for (
size_t i = first_index; i < n; ++i ) {
405 e += sqr( points[i].y - (points[i].x*a + b) );
408 least_square_error = e;
Laser360Interface Fawkes BlackBoard Interface.
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
virtual void init()
Initialize the thread.
void set_valid(const bool new_valid)
Set valid value.
ObjectPositionInterface Fawkes BlackBoard Interface.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void set_distance(const float new_distance)
Set distance value.
virtual void loop()
Code to execute in the thread.
void set_roll(const float new_roll)
Set roll value.
void set_relative_y(const float new_relative_y)
Set relative_y value.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
Logger * logger
This is the Logger member used to access the logger.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
bool has_writer() const
Check if there is a writer for the interface.
Thread aspect to use blocked timing.
virtual void finalize()
Finalize the thread.
LaserHtSensorProcThread()
Constructor.
void set_object_type(const uint32_t new_object_type)
Set object_type value.
Base class for exceptions in Fawkes.
void read()
Read from BlackBoard into local copy.
AddCartLineMessage Fawkes BlackBoard Interface Message.
void set_visible(const bool new_visible)
Set visible value.
void set_bearing(const float new_bearing)
Set bearing value.
const char * name() const
Get name of thread.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
float * distances() const
Get distances value.
void set_world_x(const float new_world_x)
Set world_x value.
void set_relative_x(const float new_relative_x)
Set relative_x value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
DeleteAllMessage Fawkes BlackBoard Interface Message.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
size_t maxlenof_distances() const
Get maximum length of distances value.
Configuration * config
This is the Configuration member used to access the configuration.
VisualDisplay2DInterface Fawkes BlackBoard Interface.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
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.
void set_world_y(const float new_world_y)
Set world_y value.
virtual void close(Interface *interface)=0
Close interface.