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> 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) {
81 __line_if->set_object_type(ObjectPositionInterface::TYPE_LINE);
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.
Fawkes library namespace.
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.
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.