bank.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2018, Andreas Gustavsson.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * Author: Andreas Gustavsson
35 *********************************************************************/
36 
37 /* ROS */
38 #include <ros/ros.h>
39 #include <ros/console.h>
40 #include <ros/assert.h>
41 #include <ros/header.h>
42 #include <sensor_msgs/LaserScan.h>
43 #include <sensor_msgs/PointCloud2.h>
44 #include <sensor_msgs/PointField.h>
45 #include <visualization_msgs/Marker.h>
46 #include <visualization_msgs/MarkerArray.h>
47 
48 #include <tf2_ros/transform_listener.h>
49 #include <tf2_ros/message_filter.h>
50 #include <message_filters/subscriber.h>
51 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
52 
53 // #include <geometry_msgs/Point.h>
54 
55 /* C/C++ */
56 #include <iostream>
57 #include <sstream>
58 #include <string>
59 #include <cstring>
60 #include <cmath>
61 // #include <pthread.h>
62 
63 /* Local includes */
64 #include <find_moving_objects/MovingObject.h>
65 #include <find_moving_objects/MovingObjectArray.h>
66 #include <find_moving_objects/bank.h>
67 
68 
69 // namespace geometry_msgs
70 // {
71 // Point operator*(float f, Point v1)
72 // {
73 // Point v;
74 // v.x = f * v1.x;
75 // v.y = f * v1.y;
76 // v.z = f * v1.z;
77 // return v;
78 // }
79 // Point operator*(Point v1, float f)
80 // {
81 // return f*v1;
82 // }
83 // Point operator/(Point v1, float f)
84 // {
85 // Point v;
86 // v.x = v1.x / f;
87 // v.y = v1.y / f;
88 // v.z = v1.z / f;
89 // return v;
90 // }
91 // float operator^(Point v1, Point v2)
92 // {
93 // return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
94 // }
95 // Point operator+(Point v1, Point v2)
96 // {
97 // Point v;
98 // v.x = v1.x + v2.x;
99 // v.y = v1.y + v2.y;
100 // v.z = v1.z + v2.z;
101 // return v;
102 // }
103 // } // namespace geometry_msgs
104 
105 
106 namespace find_moving_objects
107 {
108 
109 const float TWO_PI = 2*M_PI;
110 
111 /*
112  * Constructors
113  */
114 BankArgument::BankArgument()
115 {
116  ema_alpha = 1.0;
117  nr_scans_in_bank = 11;
118  points_per_scan = 360;
119  angle_min = -M_PI;
120  angle_max = M_PI;
121  sensor_frame_has_z_axis_forward = false;
122  object_threshold_edge_max_delta_range = 0.15;
123  object_threshold_min_nr_points = 5;
124  object_threshold_max_distance = 6.5;
125  object_threshold_min_speed = 0.03;
126  object_threshold_max_delta_width_in_points = 5;
127  object_threshold_min_confidence = 0.67;
128  object_threshold_bank_tracking_max_delta_distance = 0.2;
129  base_confidence = 0.3;
130  publish_objects = true;
131  publish_ema = true;
132  publish_objects_closest_point_markers = false;
133  publish_objects_velocity_arrows = false;
134  publish_objects_delta_position_lines = false;
135  publish_objects_width_lines = false;
136  velocity_arrows_use_full_gray_scale = false;
137  velocity_arrows_use_sensor_frame = false;
138  velocity_arrows_use_base_frame = false;
139  velocity_arrows_use_fixed_frame = false;
140  velocity_arrow_ns = "velocity_arrow_ns";
141  delta_position_line_ns = "delta_position_line_ns";
142  width_line_ns = "width_line_ns";
143  topic_objects = "moving_objects_arrays";
144  topic_ema = "ema";
145  topic_objects_closest_point_markers = "objects_closest_point_markers";
146  topic_objects_velocity_arrows = "objects_velocity_arrows";
147  topic_objects_delta_position_lines = "objects_delta_position_lines";
148  topic_objects_width_lines = "objects_width_lines";
149  publish_buffer_size = 10;
150  map_frame = "map";
151  fixed_frame = "odom";
152  base_frame = "base_link";
153 // merge_threshold_max_angle_gap = 0.0 / 180.0 * M_PI;
154 // merge_threshold_max_end_points_distance_delta = 0.2;
155 // merge_threshold_max_velocity_direction_delta = 25.0 / 180.0 * M_PI;
156 // merge_threshold_max_speed_delta = 0.2;
157  PC2_message_x_coordinate_field_name = "x";
158  PC2_message_y_coordinate_field_name = "y";
159  PC2_message_z_coordinate_field_name = "z";
160  PC2_voxel_leaf_size = 0.02;
161  PC2_threshold_z_min = 0.1;
162  PC2_threshold_z_max = 1.0;
163 
164  node_name_suffix = "";
165 }
166 
167 std::ostream& operator<<(std::ostream & os, const BankArgument & ba)
168 {
169  os << "Bank Arguments:" << std::endl <<
170  " ema_alpha = " << ba.ema_alpha << std::endl <<
171  " nr_scans_in_bank = " << ba.nr_scans_in_bank << std::endl <<
172  " points_per_scan = " << ba.points_per_scan << std::endl <<
173  " angle_min = " << ba.angle_min << std::endl <<
174  " angle_max = " << ba.angle_max << std::endl <<
175  " sensor_frame_has_z_axis_forward = " << ba.sensor_frame_has_z_axis_forward << std::endl <<
176  " object_threshold_edge_max_delta_range = " << ba.object_threshold_edge_max_delta_range << std::endl <<
177  " object_threshold_min_nr_points = " << ba.object_threshold_min_nr_points << std::endl <<
178  " object_threshold_max_distance = " << ba.object_threshold_max_distance << std::endl <<
179  " object_threshold_min_speed = " << ba.object_threshold_min_speed << std::endl <<
180  " object_threshold_max_delta_width_in_points = " << ba.object_threshold_max_delta_width_in_points << std::endl <<
181  " object_threshold_min_confidence = " << ba.object_threshold_min_confidence << std::endl <<
182  " object_threshold_bank_tracking_max_delta_distance = " <<
183  ba.object_threshold_bank_tracking_max_delta_distance << std::endl <<
184  " base_confidence = " << ba.base_confidence << std::endl <<
185  " publish_objects = " << ba.publish_objects << std::endl <<
186  " publish_ema = " << ba.publish_ema << std::endl <<
187  " publish_objects_closest_point_markers = " << ba.publish_objects_closest_point_markers << std::endl <<
188  " publish_objects_velocity_arrows = " << ba.publish_objects_velocity_arrows << std::endl <<
189  " publish_objects_delta_position_lines = " << ba.publish_objects_delta_position_lines << std::endl <<
190  " publish_objects_width_lines = " << ba.publish_objects_width_lines << std::endl <<
191  " velocity_arrows_use_full_gray_scale = " << ba.velocity_arrows_use_full_gray_scale << std::endl <<
192  " velocity_arrows_use_sensor_frame = " << ba.velocity_arrows_use_sensor_frame << std::endl <<
193  " velocity_arrows_use_base_frame = " << ba.velocity_arrows_use_base_frame << std::endl <<
194  " velocity_arrows_use_fixed_frame = " << ba.velocity_arrows_use_fixed_frame << std::endl <<
195  " velocity_arrow_ns = " << ba.velocity_arrow_ns << std::endl <<
196  " delta_position_line_ns = " << ba.delta_position_line_ns << std::endl <<
197  " width_line_ns = " << ba.width_line_ns << std::endl <<
198  " topic_objects = " << ba.topic_objects << std::endl <<
199  " topic_ema = " << ba.topic_ema << std::endl <<
200  " topic_objects_closest_point_markers = " << ba.topic_objects_closest_point_markers << std::endl <<
201  " topic_objects_velocity_arrows = " << ba.topic_objects_velocity_arrows << std::endl <<
202  " topic_objects_delta_position_lines = " << ba.topic_objects_delta_position_lines << std::endl <<
203  " topic_objects_width_lines = " << ba.topic_objects_width_lines << std::endl <<
204  " publish_buffer_size = " << ba.publish_buffer_size << std::endl <<
205  " map_frame = " << ba.map_frame << std::endl <<
206  " fixed_frame = " << ba.fixed_frame << std::endl <<
207  " base_frame = " << ba.base_frame << std::endl <<
208 // " merge_threshold_max_angle_gap = " << ba.merge_threshold_max_angle_gap << std::endl <<
209 // " merge_threshold_max_end_points_distance_delta = " <<
210 // ba.merge_threshold_max_end_points_distance_delta << std::endl <<
211 // " merge_threshold_max_velocity_direction_delta = " <<
212 // ba.merge_threshold_max_velocity_direction_delta << std::endl <<
213 // " merge_threshold_max_speed_delta = " << ba.merge_threshold_max_speed_delta << std::endl <<
214  " PC2_message_x_coordinate_field_name = " << ba.PC2_message_x_coordinate_field_name << std::endl <<
215  " PC2_message_y_coordinate_field_name = " << ba.PC2_message_y_coordinate_field_name << std::endl <<
216  " PC2_message_z_coordinate_field_name = " << ba.PC2_message_z_coordinate_field_name << std::endl <<
217  " PC2_voxel_leaf_size = " << ba.PC2_voxel_leaf_size << std::endl <<
218  " PC2_threshold_z_min = " << ba.PC2_threshold_z_min << std::endl <<
219  " PC2_threshold_z_max = " << ba.PC2_threshold_z_max << std::endl;
220  os << "Private Bank Arguments:" << std::endl <<
221  " sensor_frame = " << ba.sensor_frame << std::endl <<
222  " angle_increment = " << ba.angle_increment << std::endl <<
223  " time_increment = " << ba.time_increment << std::endl <<
224  " scan_time = " << ba.scan_time << std::endl <<
225  " range_min = " << ba.range_min << std::endl <<
226  " range_max = " << ba.range_max << std::endl <<
227  " node_name_suffix = " << ba.node_name_suffix << std::endl;
228 
229  return os;
230 }
231 
232 // Bank::Bank()
233 // {
234 // bank_is_initialized = false;
235 // bank_is_filled = false;
236 //
237 // /* Check endianness in case of PointCloud2 */
238 // volatile uint32_t dummy = 0x01234567; // If little endian, then 0x67 is the value at the lowest memory address
239 // machine_is_little_endian = (*((uint8_t*)(&dummy))) == 0x67;
240 //
241 // /* Create handle to this node */
242 // node = new ros::NodeHandle;
243 //
244 // /* Create TF listener */
245 // tf_listener = new tf::TransformListener;
246 // }
247 
248 Bank::Bank(tf2_ros::Buffer * buffer)
249 {
250  bank_is_initialized = false;
251  bank_is_filled = false;
252 
253  /* Check endianness in case of PointCloud2 */
254  volatile uint32_t dummy = 0x01234567; // If little endian, then 0x67 is the value at the lowest memory address
255  machine_is_little_endian = (*((uint8_t*)(&dummy))) == 0x67;
256 
257  /* Create handle to this node */
258  node = new ros::NodeHandle;
259 
260  /* Assign TF listener and buffer */
261 // tf_listener = listener;
262  tf_buffer = buffer;
263 }
264 
265 /*
266  * Destructor
267  */
268 Bank::~Bank()
269 {
270  for (int i=0; i<bank_argument.nr_scans_in_bank; ++i)
271  {
272  free(bank_ranges_ema[i]);
273  }
274  free(bank_ranges_ema);
275  free(bank_stamp);
276 }
277 
278 
279 /*
280  * Check values of bank arguments.
281  */
282 void BankArgument::check()
283 {
284  ROS_ASSERT_MSG(0.0 <= ema_alpha && ema_alpha <= 1.0,
285  "The EMA weighting decrease coefficient must be a value in [0,1].");
286 
287  ROS_ASSERT_MSG(2 <= nr_scans_in_bank,
288  "There must be at least 2 messages in the bank. Otherwise, velocities cannot be calculated.");
289 
290  ROS_ASSERT_MSG(0 < points_per_scan,
291  "There must be at least 1 point per scan.");
292 
293  ROS_ASSERT_MSG(angle_max - angle_min <= TWO_PI,
294  "Angle interval cannot be larger than 2*PI (360 degrees).");
295 
296  ROS_ASSERT_MSG(0.0 <= object_threshold_edge_max_delta_range,
297  "Cannot be negative.");
298 
299  ROS_ASSERT_MSG(1 <= object_threshold_min_nr_points,
300  "An object must consist of at least 1 point.");
301 
302  ROS_ASSERT_MSG(0.0 <= object_threshold_max_distance,
303  "Cannot be negative.");
304 
305  ROS_ASSERT_MSG(0.0 <= object_threshold_min_speed,
306  "Cannot be negative.");
307 
308  ROS_ASSERT_MSG(0.0 <= object_threshold_max_delta_width_in_points,
309  "Cannot be negative.");
310 
311  ROS_ASSERT_MSG(0.0 <= object_threshold_min_confidence && object_threshold_min_confidence <= 1.0,
312  "Cannot be negative or larger than 1.0.");
313 
314 // ROS_ASSERT_MSG(sensor_frame_has_z_axis_forward);
315 // ROS_ASSERT_MSG(base_confidence);
316 // ROS_ASSERT_MSG(publish_objects);
317 // ROS_ASSERT_MSG(publish_ema);
318 // ROS_ASSERT_MSG(publish_objects_closest_point_markers);
319 // ROS_ASSERT_MSG(publish_objects_velocity_arrows);
320 // ROS_ASSERT_MSG(velocity_arrows_use_full_gray_scale);
321 // ROS_ASSERT_MSG(velocity_arrows_use_sensor_frame);
322 // ROS_ASSERT_MSG(velocity_arrows_use_base_frame);
323 // ROS_ASSERT_MSG(velocity_arrows_use_fixed_frame);
324 
325  ROS_ASSERT_MSG(!publish_objects_velocity_arrows || velocity_arrow_ns != "",
326  "If publishing velocity arrows, then a name space for them must be given.");
327 
328  ROS_ASSERT_MSG(!publish_objects_delta_position_lines || delta_position_line_ns != "",
329  "If publishing delta position lines, then a name space for them must be given.");
330 
331  ROS_ASSERT_MSG(!publish_objects_width_lines || width_line_ns != "",
332  "If publishing width lines, then a name space for them must be given.");
333 
334  ROS_ASSERT_MSG(!publish_objects || topic_objects != "",
335  "If publishing MovingObjectArray messages, then a topic for that must be given.");
336 
337  ROS_ASSERT_MSG(!publish_ema || topic_ema != "",
338  "If publishing object points via LaserScan visualization messages, "
339  "then a topic for that must be given.");
340 
341  ROS_ASSERT_MSG(!publish_objects_closest_point_markers || topic_objects_closest_point_markers != "",
342  "If publishing the closest point of each object via LaserScan visualization messages, "
343  "then a topic for that must be given.");
344 
345  ROS_ASSERT_MSG(!publish_objects_velocity_arrows || topic_objects_velocity_arrows != "",
346  "If publishing the velocity of each object via MarkerArray visualization messages, "
347  "then a topic for that must be given.");
348 
349  ROS_ASSERT_MSG(!publish_objects_delta_position_lines || topic_objects_delta_position_lines != "",
350  "If publishing the delta position of each object via MarkerArray visualization messages, "
351  "then a topic for that must be given.");
352 
353  ROS_ASSERT_MSG(!publish_objects_width_lines || topic_objects_width_lines != "",
354  "If publishing the width of each object via MarkerArray visualization messages, "
355  "then a topic for that must be given.");
356 
357  ROS_ASSERT_MSG(1 <= publish_buffer_size,
358  "Publish buffer size must be at least 1.");
359 
360  ROS_ASSERT_MSG(map_frame != "",
361  "Please specify map frame.");
362 
363  ROS_ASSERT_MSG(fixed_frame != "",
364  "Please specify fixed frame.");
365 
366  ROS_ASSERT_MSG(base_frame != "",
367  "Please specify base frame.");
368 
369 // ROS_ASSERT_MSG(0.0 <= merge_threshold_max_angle_gap && merge_threshold_max_angle_gap <= angle_max - angle_min,
370 // "Invalid gap angle.");
371 //
372 // ROS_ASSERT_MSG(0.0 <= merge_threshold_max_end_points_distance_delta,
373 // "Distance delta cannot be negative.");
374 //
375 // ROS_ASSERT_MSG(0.0 <= merge_threshold_max_velocity_direction_delta && merge_threshold_max_velocity_direction_delta <= M_PI,
376 // "The maximum angle between two vectors is always greater than 0 and smaller than PI.");
377 //
378 // ROS_ASSERT_MSG(0.0 <= merge_threshold_max_speed_delta,
379 // "Speed delta cannot be negative.");
380 }
381 
382 
383 /*
384  * Check values of bank arguments, PC2-specific.
385  */
386 void BankArgument::check_PC2()
387 {
388  ROS_ASSERT_MSG(PC2_message_x_coordinate_field_name != "",
389  "Please specify a field name for x coordinates, or do not alter the default value.");
390 
391  ROS_ASSERT_MSG(PC2_message_y_coordinate_field_name != "",
392  "Please specify a field name for y coordinates, or do not alter the default value.");
393 
394  ROS_ASSERT_MSG(PC2_message_z_coordinate_field_name != "",
395  "Please specify a field name for z coordinates, or do not alter the default value.");
396 
397  ROS_ASSERT_MSG(0.0 <= PC2_voxel_leaf_size,
398  "Cannot be negative.");
399 
400  ROS_ASSERT_MSG(PC2_threshold_z_min <= PC2_threshold_z_max,
401  "Invalid thresholds.");
402 }
403 
404 
405 /*
406  * Initialize bank based on information received from the user and sensor
407  */
408 void Bank::initBank(BankArgument bank_argument)
409 {
410  if (bank_is_initialized)
411  {
412  return;
413  }
414 
415  bank_argument.check();
416 
417  bank_index_put = -1;
418  bank_index_newest = -1;
419 
420  /* Create publishers */
421  pub_ema =
422  node->advertise<sensor_msgs::LaserScan>(bank_argument.topic_ema,
423  bank_argument.publish_buffer_size);
424  pub_objects_closest_point_markers =
425  node->advertise<sensor_msgs::LaserScan>(bank_argument.topic_objects_closest_point_markers,
426  bank_argument.publish_buffer_size);
427  pub_objects_velocity_arrows =
428  node->advertise<visualization_msgs::MarkerArray>(bank_argument.topic_objects_velocity_arrows,
429  bank_argument.publish_buffer_size);
430  pub_objects_delta_position_lines =
431  node->advertise<visualization_msgs::MarkerArray>(bank_argument.topic_objects_delta_position_lines,
432  bank_argument.publish_buffer_size);
433  pub_objects_width_lines =
434  node->advertise<visualization_msgs::MarkerArray>(bank_argument.topic_objects_width_lines,
435  bank_argument.publish_buffer_size);
436  pub_objects =
437  node->advertise<MovingObjectArray>(bank_argument.topic_objects,
438  bank_argument.publish_buffer_size);
439 
440  /* Init bank */
441  this->bank_argument = bank_argument;
442  this->bank_argument.sensor_is_360_degrees = fabsf(bank_argument.angle_max - bank_argument.angle_min - TWO_PI) <=
443  2.0 * bank_argument.angle_increment; // Safety margin
444 
445  bank_stamp = (double *) malloc(bank_argument.nr_scans_in_bank * sizeof(double));
446  bank_ranges_ema = (float **) malloc(bank_argument.nr_scans_in_bank * sizeof(float*));
447  ROS_ASSERT_MSG(bank_ranges_ema != NULL, "Could not allocate buffer space for messages.");
448  for (unsigned int i=0; i<bank_argument.nr_scans_in_bank; ++i)
449  {
450  bank_ranges_ema[i] = (float *) malloc(bank_argument.points_per_scan * sizeof(float));
451  ROS_ASSERT_MSG(bank_ranges_ema[i] != NULL, "Could not allocate buffer space message %d.", i);
452  }
453 
454  /* Init messages to publish - init constant fields */
455  // EMA (with detected moving objects/objects)
456  if (bank_argument.publish_ema)
457  {
458  msg_ema.header.frame_id = bank_argument.sensor_frame;
459  msg_ema.angle_min = bank_argument.angle_min;
460  msg_ema.angle_max = bank_argument.angle_max;
461  msg_ema.angle_increment = bank_argument.angle_increment;
462  msg_ema.time_increment = bank_argument.time_increment;
463  msg_ema.scan_time = bank_argument.scan_time;
464  msg_ema.range_min = bank_argument.range_min;
465  msg_ema.range_max = bank_argument.range_max;
466  msg_ema.ranges.resize(bank_argument.points_per_scan);
467  msg_ema.intensities.resize(bank_argument.points_per_scan);
468  bzero(&msg_ema.intensities[0], bank_argument.points_per_scan * sizeof(float));
469  }
470  // Arrows for position and velocity
471  if (bank_argument.publish_objects_velocity_arrows)
472  {
473  if (bank_argument.velocity_arrows_use_sensor_frame)
474  {
475  msg_objects_velocity_arrow.header.frame_id = bank_argument.sensor_frame;
476  }
477  else if (bank_argument.velocity_arrows_use_base_frame)
478  {
479  msg_objects_velocity_arrow.header.frame_id = bank_argument.base_frame;
480  }
481  else if (bank_argument.velocity_arrows_use_fixed_frame)
482  {
483  msg_objects_velocity_arrow.header.frame_id = bank_argument.fixed_frame;
484  }
485  else // map frame
486  {
487  msg_objects_velocity_arrow.header.frame_id = bank_argument.map_frame;
488  }
489  msg_objects_velocity_arrow.ns = bank_argument.velocity_arrow_ns;
490  msg_objects_velocity_arrow.type = visualization_msgs::Marker::ARROW;
491  msg_objects_velocity_arrow.action = visualization_msgs::Marker::ADD;
492  // msg_objects_velocity_arrow.pose.position.x = 0.0;
493  // msg_objects_velocity_arrow.pose.position.y = 0.0;
494  // msg_objects_velocity_arrow.pose.position.z = 0.0;
495  // msg_objects_velocity_arrow.pose.orientation.x = 0.0;
496  // msg_objects_velocity_arrow.pose.orientation.y = 0.0;
497  // msg_objects_velocity_arrow.pose.orientation.z = 0.0;
498  msg_objects_velocity_arrow.pose.orientation.w = 1.0;
499  msg_objects_velocity_arrow.scale.x = 0.05; // shaft diameter
500  msg_objects_velocity_arrow.scale.y = 0.1; // arrow head diameter
501  // msg_objects_velocity_arrow.scale.z = 0.0;
502  // msg_objects_velocity_arrow.color.r = 0.0;
503  // msg_objects_velocity_arrow.color.g = 0.0;
504  // msg_objects_velocity_arrow.color.b = 0.0;
505  msg_objects_velocity_arrow.color.a = 1.0;
506  msg_objects_velocity_arrow.lifetime = ros::Duration(0.4);
507  msg_objects_velocity_arrow.frame_locked = true;
508  msg_objects_velocity_arrow.points.resize(2);
509  // msg_objects_velocity_arrow.points[0].z = 0.0;
510  // msg_objects_velocity_arrow.points[1].z = 0.0;
511  }
512  // Lines for delta position
513  if (bank_argument.publish_objects_delta_position_lines)
514  {
515  msg_objects_delta_position_line.header.frame_id = bank_argument.sensor_frame;
516  msg_objects_delta_position_line.ns = bank_argument.delta_position_line_ns;
517  msg_objects_delta_position_line.type = visualization_msgs::Marker::LINE_STRIP;
518  msg_objects_delta_position_line.action = visualization_msgs::Marker::ADD;
519  // msg_objects_delta_position_line.pose.position.x = 0.0;
520  // msg_objects_delta_position_line.pose.position.y = 0.0;
521  // msg_objects_delta_position_line.pose.position.z = 0.0;
522  // msg_objects_delta_position_line.pose.orientation.x = 0.0;
523  // msg_objects_delta_position_line.pose.orientation.y = 0.0;
524  // msg_objects_delta_position_line.pose.orientation.z = 0.0;
525  msg_objects_delta_position_line.pose.orientation.w = 1.0; // No translation or rotation
526  msg_objects_delta_position_line.scale.x = 0.04; // diameter
527  // msg_objects_delta_position_line.scale.y = 0.0;
528  // msg_objects_delta_position_line.scale.z = 0.0;
529  // msg_objects_delta_position_line.color.r = 0.0;
530  // msg_objects_delta_position_line.color.g = 0.0;
531  msg_objects_delta_position_line.color.b = 1.0; // Blue lines
532  msg_objects_delta_position_line.color.a = 1.0;
533  msg_objects_delta_position_line.lifetime = ros::Duration(0.4);
534  msg_objects_delta_position_line.frame_locked = true;
535  msg_objects_delta_position_line.points.resize(2);
536 // msg_objects_delta_position_line.points[0].z = 0.0;
537 // msg_objects_delta_position_line.points[1].z = 0.0;
538  }
539  // Lines for width
540  if (bank_argument.publish_objects_width_lines)
541  {
542  msg_objects_width_line.header.frame_id = bank_argument.sensor_frame;
543  msg_objects_width_line.ns = bank_argument.width_line_ns;
544  msg_objects_width_line.type = visualization_msgs::Marker::LINE_STRIP;
545  msg_objects_width_line.action = visualization_msgs::Marker::ADD;
546  // msg_objects_width_line.pose.position.x = 0.0;
547  // msg_objects_width_line.pose.position.y = 0.0;
548  // msg_objects_width_line.pose.position.z = 0.0;
549  // msg_objects_width_line.pose.orientation.x = 0.0;
550  // msg_objects_width_line.pose.orientation.y = 0.0;
551  // msg_objects_width_line.pose.orientation.z = 0.0;
552  msg_objects_width_line.pose.orientation.w = 1.0; // No translation or rotation
553  msg_objects_width_line.scale.x = 0.02; // diameter
554  // msg_objects_width_line.scale.y = 0.0;
555  // msg_objects_width_line.scale.z = 0.0;
556  // msg_objects_width_line.color.r = 0.0;
557  msg_objects_width_line.color.g = 1.0; // Green lines
558 // msg_objects_width_line.color.b = 1.0;
559  msg_objects_width_line.color.a = 1.0;
560  msg_objects_width_line.lifetime = ros::Duration(0.4);
561  msg_objects_width_line.frame_locked = true;
562  msg_objects_width_line.points.resize(2);
563 // msg_objects_width_line.points[0].z = 0.0;
564 // msg_objects_width_line.points[1].z = 0.0;
565  }
566  // Laserscan points for closest points
567  if (bank_argument.publish_objects_closest_point_markers)
568  {
569  msg_objects_closest_point_markers.header.frame_id = bank_argument.sensor_frame;
570  msg_objects_closest_point_markers.angle_min = bank_argument.angle_min;
571  msg_objects_closest_point_markers.angle_max = bank_argument.angle_max;
572  msg_objects_closest_point_markers.angle_increment = bank_argument.angle_increment;
573  msg_objects_closest_point_markers.time_increment = bank_argument.time_increment;
574  msg_objects_closest_point_markers.scan_time = bank_argument.scan_time;
575  msg_objects_closest_point_markers.range_min = bank_argument.range_min;
576  msg_objects_closest_point_markers.range_max = bank_argument.range_max;
577  msg_objects_closest_point_markers.intensities.resize(bank_argument.points_per_scan);
578  msg_objects_closest_point_markers.ranges.resize(bank_argument.points_per_scan);
579  for (unsigned int i=0; i<bank_argument.points_per_scan; ++i)
580  {
581  msg_objects_closest_point_markers.ranges[i] = msg_objects_closest_point_markers.range_max + 10.0;
582  msg_objects_closest_point_markers.intensities[i] = 0.0;
583  }
584  }
585 
586  // Nr scan points
587  bank_ranges_bytes = sizeof(float) * bank_argument.points_per_scan;
588 
589  // Init sequence nr
590  moa_seq = 0;
591 
592  bank_is_initialized = true;
593 }
594 
595 
596 /*
597  * Recursive tracking of an object through history to get the indices of its middle,
598  * left and right points in the oldest scans, along with the sum of all ranges etc.
599  */
600 void Bank::getOldIndices(const float range_min,
601  const float range_max,
602  const unsigned int object_width_in_points,
603  const int current_level,
604  const unsigned int levels_searched,
605  const unsigned int index_mean,
606  const unsigned int consecutive_failures_to_find_object,
607  const unsigned int threshold_consecutive_failures_to_find_object,
608  int * index_min_old,
609  int * index_mean_old,
610  int * index_max_old,
611  float * range_sum_old,
612  float * range_at_min_index_old,
613  float * range_at_max_index_old)
614 {
615  // Base case reached?
616  if (levels_searched == bank_argument.nr_scans_in_bank)
617  {
618  return;
619  }
620 
621  // To find the end indices of the object,
622  int left = index_mean;
623  float prev_range = bank_ranges_ema[current_level][index_mean];
624  float range_sum = prev_range;
625  int right_upper_limit_out_of_bounds = bank_argument.points_per_scan;
626  unsigned int width_in_points = 1; // prev_rang = range at index_mean
627 
628  // Check range
629  if (prev_range < range_min ||
630  range_max < prev_range)
631  {
632  *index_min_old = -1;
633  *index_mean_old = -1;
634  *index_max_old = -1;
635  *range_sum_old = 0;
636  *range_at_min_index_old = 0;
637  *range_at_max_index_old = 0;
638  return;
639  }
640 
641  // Search lower index side, with possible wrap around
642  bool stopped = false;
643  for (int i=index_mean-1; 0<=i; --i)
644  {
645  // Same type of range check as in the main code
646  const float range = bank_ranges_ema[current_level][i];
647  if (range_min <= range &&
648  range <= range_max &&
649  fabsf(range - prev_range) <= bank_argument.object_threshold_edge_max_delta_range)
650  {
651  left = i;
652  prev_range = range;
653  range_sum += range;
654  width_in_points++;
655  }
656  else
657  {
658  stopped = true;
659  break;
660  }
661  }
662  if (!stopped)
663  {
664  // Continue from highest index
665  for (int i=bank_argument.points_per_scan-1; index_mean<i; --i)
666  {
667  // Same type of range check as in the main code
668  const float range = bank_ranges_ema[current_level][i];
669  if (range_min <= range &&
670  range <= range_max &&
671  fabsf(range - prev_range) <= bank_argument.object_threshold_edge_max_delta_range)
672  {
673  left = i;
674  prev_range = range;
675  range_sum += range;
676  right_upper_limit_out_of_bounds--;
677  width_in_points++;
678  }
679  else
680  {
681  break;
682  }
683  }
684  }
685  // prev_range holds the range at left
686  *range_at_min_index_old = prev_range;
687 
688  // Search higher index side
689  stopped = false;
690  int right = index_mean;
691  prev_range = bank_ranges_ema[current_level][index_mean];
692  for (int i=index_mean+1; i<right_upper_limit_out_of_bounds; ++i)
693  {
694  // Same type of range check as in the main code
695  const float range = bank_ranges_ema[current_level][i];
696  if (range_min <= range &&
697  range <= range_max &&
698  fabsf(range - prev_range) <= bank_argument.object_threshold_edge_max_delta_range)
699  {
700  right = i;
701  prev_range = range;
702  range_sum += range;
703  width_in_points++;
704  }
705  else
706  {
707  stopped = true;
708  break;
709  }
710  }
711  // Here we must make sure that we have not already wrapped around while going left
712  if (!stopped && right_upper_limit_out_of_bounds == bank_argument.points_per_scan)
713  {
714  // Continue from lowest index (0) - we did not wrap around while going left
715  for (int i=0; i<left; ++i)
716  {
717  // Same type of range check as in the main code
718  const float range = bank_ranges_ema[current_level][i];
719  if (range_min <= range &&
720  range <= range_max &&
721  fabsf(range - prev_range) <= bank_argument.object_threshold_edge_max_delta_range)
722  {
723  right = i;
724  prev_range = range;
725  range_sum += range;
726  width_in_points++;
727  }
728  else
729  {
730  break;
731  }
732  }
733  }
734  // prev_range holds the range at right
735  *range_at_max_index_old = prev_range;
736 
737  // Did we find a valid object?
738  unsigned int misses = consecutive_failures_to_find_object;
739  if (width_in_points < bank_argument.object_threshold_min_nr_points ||
740  bank_argument.object_threshold_max_delta_width_in_points < abs(width_in_points - object_width_in_points) ||
741  bank_argument.object_threshold_bank_tracking_max_delta_distance <
742  fabs(range_sum / width_in_points - *range_sum_old / object_width_in_points))
743  // range_sum_old holds the range sum of the previous (newer) scanned object
744  {
745  // No
746  misses++;
747  if (threshold_consecutive_failures_to_find_object < misses)
748  {
749  // Return -1 to signal that no index_mean was found
750  *index_min_old = -1;
751  *index_mean_old = -1;
752  *index_max_old = -1;
753  *range_sum_old = 0;
754  *range_at_min_index_old = 0;
755  *range_at_max_index_old = 0;
756  return;
757  }
758  }
759  else
760  {
761  // Yes
762  misses = 0;
763  }
764 
765  // If reaching this point, a valid object was found
766  // Update end points
767  *index_min_old = left;
768  *index_mean_old = (left + (width_in_points-1) / 2) % bank_argument.points_per_scan;
769  *index_max_old = right;
770  *range_sum_old = range_sum;
771 
772  // Continue searching based on the new index_mean
773  getOldIndices(range_min,
774  range_max,
775  width_in_points,
776  (current_level - 1) < 0 ? bank_argument.nr_scans_in_bank - 1 : current_level - 1, // wrap around
777  levels_searched + 1,
778  *index_mean_old,
779  misses,
780  threshold_consecutive_failures_to_find_object,
781  index_min_old,
782  index_mean_old,
783  index_max_old,
784  range_sum_old, // *range_sum_old was set to range_sum above
785  range_at_min_index_old,
786  range_at_max_index_old);
787 }
788 
789 
790 // /*
791 // * Compares consecutive found objects to see if they are to be considered the same object. If so, then a merge of them
792 // * is performed
793 // */
794 // void Bank::mergeFoundObjects(MovingObjectArray * moa)
795 // {
796 // // unsigned int nr_objects_found = moa->objects.size();
797 //
798 // if (1 < moa->objects.size())
799 // {
800 //
801 // MovingObject * object_1 = &moa->objects[0];
802 // MovingObject * object_2;
803 // std::vector<MovingObject> moa_merged;
804 //
805 // // Loop through objects
806 // for (unsigned int i=1; i<moa->objects.size(); ++i)
807 // {
808 // object_2 = &moa->objects[i];
809 //
810 // // Angle betweeen velocity vectors
811 // // float velocity_direction_delta = acosf((object_1->velocity.x * object_2->velocity.x +
812 // // object_1->velocity.y * object_2->velocity.y +
813 // // object_1->velocity.z * object_2->velocity.z) /
814 // // (object_1->speed * object_2->speed));
815 //
816 // float velocity_direction_delta = acosf((object_1->velocity ^ object_2->velocity) /
817 // (object_1->speed * object_2->speed));
818 //
819 // float gap_angle = object_2->angle_begin - object_1->angle_end;
820 //
821 // // std::cout << "Comparing objects " << i-1 << " and " << i << std::endl <<
822 // // " gap_angle = " << gap_angle * 180 / M_PI << std::endl <<
823 // // " delta_distance_at_angle_gap_ends = " << fabsf(object_2->distance_at_angle_begin - object_1->distance_at_angle_end) << std::endl <<
824 // // " delta_speed = " << fabsf(object_1->speed * object_2->speed) << std::endl <<
825 // // " velocity_direction_delta = " << std::left << std::setw(11) << velocity_direction_delta *180 / M_PI<<
826 // // " " << std::left << std::setw(9) << object_1->velocity.x << " " << std::left << std::setw(9) << object_2->velocity.x << std::endl <<
827 // // " " << std::left << std::setw(9) << object_1->velocity.y << " " << std::left << std::setw(9) << object_2->velocity.y << std::endl <<
828 // // " " << std::left << std::setw(9) << object_1->velocity.z << " " << std::left << std::setw(9) << object_2->velocity.z << std::endl;
829 //
830 // // Compare objects - end of object 1 <-> start of object 2, velocities
831 // if (gap_angle <= bank_argument.merge_threshold_max_angle_gap &&
832 // fabsf(object_2->distance_at_angle_begin - object_1->distance_at_angle_end) <=
833 // bank_argument.merge_threshold_max_end_points_distance_delta &&
834 // velocity_direction_delta <= bank_argument.merge_threshold_max_velocity_direction_delta &&
835 // fabsf(object_1->speed - object_2->speed) <= bank_argument.merge_threshold_max_speed_delta)
836 // {
837 // std::cout << "MERGING OBJECTS:" << std::endl; // <<
838 // // *object_1 << std::endl <<
839 // // "---------------------------" << std::endl <<
840 // // *object_2 << std::endl <<
841 // // "---------------------------" << std::endl <<
842 // // "---------------------------" << std::endl;
843 // // "Join" objects and add the new object to moa_merged
844 // MovingObject mo = *object_1;
845 //
846 // // Update properties
847 // mo.seen_width = sqrt( object_1->distance_at_angle_begin *
848 // object_1->distance_at_angle_begin +
849 // object_2->distance_at_angle_end *
850 // object_2->distance_at_angle_end -
851 // 2 * object_1->distance_at_angle_begin *
852 // object_2->distance_at_angle_end *
853 // cosf (object_2->angle_end - object_1->angle_begin)
854 // );
855 // mo.angle_end = object_2->angle_end;
856 // mo.distance_at_angle_end = object_2->distance_at_angle_end;
857 // mo.distance = (object_1->distance *
858 // (object_1->angle_end - object_1->angle_begin) / bank_argument.angle_increment +
859 // object_2->distance *
860 // (object_2->angle_end - object_2->angle_begin) / bank_argument.angle_increment) * // Range sum
861 // bank_argument.angle_increment / (object_2->angle_end - object_1->angle_begin);
862 //
863 // // The length of the line between objects 1 and 2
864 // float gap_width = sqrt( object_1->distance_at_angle_end *
865 // object_1->distance_at_angle_end +
866 // object_2->distance_at_angle_begin *
867 // object_2->distance_at_angle_begin -
868 // 2 * object_1->distance_at_angle_end *
869 // object_2->distance_at_angle_begin *
870 // cosf (gap_angle)
871 // );
872 // // The center point of the new object lies this far from object_2 on that line
873 // float shift_factor = (object_1->seen_width + gap_width) /
874 // (object_1->seen_width + object_2->seen_width + 2 * gap_width);
875 //
876 // mo.position_in_map_frame = shift_factor * object_1->position_in_map_frame + (1 - shift_factor) * object_2->position_in_map_frame;
877 // mo.position_in_fixed_frame = shift_factor * object_1->position_in_fixed_frame + (1 - shift_factor) * object_2->position_in_fixed_frame;
878 // mo.position_in_base_frame = shift_factor * object_1->position_in_base_frame + (1 - shift_factor) * object_2->position_in_base_frame;
879 // mo.position = shift_factor * object_1->position + (1 - shift_factor) * object_2->position;
880 // mo.velocity_in_map_frame = 0.5 * (object_1->velocity_in_map_frame + object_2->velocity_in_map_frame);
881 // mo.velocity_in_fixed_frame = 0.5 * (object_1->velocity_in_fixed_frame + object_2->velocity_in_fixed_frame);
882 // mo.velocity_in_base_frame = 0.5 * (object_1->velocity_in_base_frame + object_2->velocity_in_base_frame);
883 // mo.velocity = 0.5 * (object_1->velocity + object_2->velocity);
884 // mo.speed_in_map_frame = sqrt (mo.velocity_in_map_frame ^ mo.velocity_in_map_frame);
885 // mo.speed_in_fixed_frame = sqrt (mo.velocity_in_fixed_frame ^ mo.velocity_in_fixed_frame);
886 // mo.speed_in_base_frame = sqrt (mo.velocity_in_base_frame ^ mo.velocity_in_base_frame);
887 // mo.speed = sqrt (mo.velocity ^ mo.velocity);
888 // mo.velocity_normalized_in_map_frame = mo.velocity_in_map_frame / mo.speed_in_map_frame;
889 // mo.velocity_normalized_in_fixed_frame = mo.velocity_in_fixed_frame / mo.speed_in_fixed_frame;
890 // mo.velocity_normalized_in_base_frame = mo.velocity_in_base_frame / mo.speed_in_base_frame;
891 // mo.velocity_normalized = mo.velocity / mo.speed;
892 // bool object_1_is_closest = object_1->closest_distance < object_2->closest_distance;
893 // mo.closest_point_in_map_frame = object_1_is_closest ? object_1->closest_point_in_map_frame : object_2->closest_point_in_map_frame;
894 // mo.closest_point_in_fixed_frame = object_1_is_closest ? object_1->closest_point_in_fixed_frame : object_2->closest_point_in_fixed_frame;
895 // mo.closest_point_in_base_frame = object_1_is_closest ? object_1->closest_point_in_base_frame : object_2->closest_point_in_base_frame;
896 // mo.closest_point = object_1_is_closest ? object_1->closest_point : object_2->closest_point;
897 // mo.closest_distance = object_1_is_closest ? object_1->closest_distance : object_2->closest_distance;
898 // mo.angle_for_closest_distance = object_1_is_closest ? object_1->angle_for_closest_distance : object_2->angle_for_closest_distance;
899 // mo.confidence = object_1->confidence < object_2->confidence ? object_2->confidence : object_1->confidence;
900 //
901 // // Manipulate moa, object_1 and i
902 // moa->objects.at(i-1) = mo;
903 // moa->objects.erase(moa->objects.begin()+i);
904 // object_1 = &moa->objects[i-1];
905 // i--; // Also updated in loop, we want to redo current i
906 // }
907 // else
908 // {
909 // // Go to next two objects
910 // object_1 = object_2;
911 // }
912 // }
913 //
914 // // TODO: Wrap around if 360 degree sensor
915 // }
916 // }
917 
918 /*
919  * Find and report moving objects based on the current content of the bank
920  */
921 void Bank::findAndReportMovingObjects()
922 {
923  // Is the bank filled with scans?
924  if (!bank_is_filled)
925  {
926  ROS_WARN("Bank is not filled yet-cannot report objects!");
927  return;
928  }
929 
930  // Moving object array message
931  MovingObjectArray moa;
932 
933  // Old positions of the objects in moa
934  MovingObjectArray moa_old_positions;
935 
936  /* Find objects in the new scans */
937  unsigned int nr_objects_found = 0;
938  unsigned int nr_object_points = 0;
939  const float range_max = (bank_argument.range_max < bank_argument.object_threshold_max_distance ?
940  bank_argument.range_max : bank_argument.object_threshold_max_distance);
941  const float range_min = bank_argument.range_min;
942  unsigned int i=0;
943 
944  // Stamps
945  ros::Time old_time = ros::Time(bank_stamp[bank_index_put]);
946  ros::Time new_time = ros::Time(bank_stamp[bank_index_newest]);
947 
948  // Handle 360 degrees sensors!
949  unsigned int upper_limit_out_of_bounds_scan_point = bank_argument.points_per_scan;
950  while(i<upper_limit_out_of_bounds_scan_point)
951  {
952  /* Find first valid scan from where we currently are */
953  const float range_i = bank_ranges_ema[bank_index_newest][i];
954  float object_range_sum = range_i;
955 
956  // Is i out-of-range?
957  if (range_i < bank_argument.range_min ||
958  bank_argument.range_max < range_i)
959  {
960  i++;
961  continue;
962  }
963 
964  // i is a valid scan
965  nr_object_points = 1;
966  float object_range_min = range_i;
967  float object_range_max = range_i;
968  unsigned int object_range_min_index = i;
969  unsigned int object_range_max_index = i;
970 
971  // Count valid scans that are within the object threshold
972 
973  float range_at_angle_begin = range_i; // Might be updated later
974  float range_at_angle_end; // Updated later
975  unsigned int index_at_angle_begin = i; // Might be updated later
976  unsigned int index_at_angle_end; // Updated later
977  float prev_range = range_i;
978  unsigned int j=i+1;
979  for (; j<bank_argument.points_per_scan; ++j)
980  {
981  const float range_j = bank_ranges_ema[bank_index_newest][j];
982 
983  // Range check
984  if (bank_argument.range_min <= range_j &&
985  range_j <= bank_argument.range_max &&
986  fabsf(prev_range - range_j) <= bank_argument.object_threshold_edge_max_delta_range)
987  {
988  // j is part of the current object
989  nr_object_points++;
990  object_range_sum += range_j;
991 
992  // Update min and max ranges
993  if (range_j < object_range_min)
994  {
995  object_range_min = range_j;
996  object_range_min_index = j;
997  }
998  else if (object_range_max < range_j)
999  {
1000  object_range_max = range_j;
1001  object_range_max_index = j;
1002  }
1003  prev_range = range_j;
1004  }
1005  else
1006  {
1007  // j is not part of this object
1008  break;
1009  }
1010  }
1011 
1012  // Update range at end
1013  range_at_angle_end = prev_range;
1014  index_at_angle_end = j-1; // j is not part of the object
1015 
1016  // If i is 0 and sensor is 360 deg, then we must also search higher end of scan points and account for these
1017  if (i == 0 && bank_argument.sensor_is_360_degrees)
1018  {
1019  // Start from i again
1020  prev_range = range_i;
1021  //Save prev_range so we can restore it?
1022 
1023  // Do not step all the way to j again - it has already been considered
1024  for (unsigned int k=bank_argument.points_per_scan-1; j<k; k--)
1025  {
1026  const float range_k = bank_ranges_ema[bank_index_newest][k];
1027 
1028  // Range check
1029  if (bank_argument.range_min <= range_k &&
1030  range_k <= bank_argument.range_max &&
1031  fabsf(prev_range - range_k) <= bank_argument.object_threshold_edge_max_delta_range)
1032  {
1033  // k is part of the current object
1034  nr_object_points++;
1035  object_range_sum += range_k;
1036 
1037  // Adapt the loop upper limit
1038  upper_limit_out_of_bounds_scan_point--;
1039 
1040  // Update min and max ranges
1041  if (range_k < object_range_min)
1042  {
1043  object_range_min = range_k;
1044  object_range_min_index = k;
1045  }
1046  else if (object_range_max < range_k)
1047  {
1048  object_range_max = range_k;
1049  object_range_max_index = k;
1050  }
1051  prev_range = range_k;
1052  }
1053  else
1054  {
1055  // k is not part of this object
1056  break;
1057  }
1058  }
1059 
1060  // Update range at begin; it might not be range_i anymore
1061  range_at_angle_begin = prev_range;
1062  index_at_angle_begin = upper_limit_out_of_bounds_scan_point;
1063  }
1064 
1065  /* Evaluate the found object (it consists of at least the ith scan) */
1066  const float distance = object_range_sum / nr_object_points; // Average distance
1067  const float object_seen_width = sqrt( range_at_angle_begin *
1068  range_at_angle_begin +
1069  range_at_angle_end *
1070  range_at_angle_end -
1071  2 * range_at_angle_begin *
1072  range_at_angle_end *
1073  cosf (bank_argument.angle_increment * nr_object_points)
1074  ); // This is the seen object width using the law of cosine
1075 
1076  // Threshold check
1077  if (bank_argument.object_threshold_min_nr_points <= nr_object_points)
1078  {
1079  // Valid object
1080  nr_objects_found++;
1081 
1082  // Recursively derive the min, mean and max indices and the sum of all ranges of the object (if found)
1083  // in the oldest scans in the bank
1084  const unsigned int index_min = index_at_angle_begin;
1085  const unsigned int index_max = index_at_angle_end;
1086  const unsigned int index_mean = (index_min + (nr_object_points-1) / 2) % bank_argument.points_per_scan;
1087  // Accounts for 360 deg sensor => i==0 could mean that index_max < index_min
1088  int index_min_old = -1;
1089  int index_mean_old = -1;
1090  int index_max_old = -1;
1091  float range_sum_old = object_range_sum;
1092  float range_at_min_index_old = 0;
1093  float range_at_max_index_old = 0;
1094  getOldIndices(range_min,
1095  range_max,
1096  nr_object_points,
1097  (bank_index_newest - 1) < 0 ? bank_argument.nr_scans_in_bank - 1 : bank_index_newest - 1,
1098  1, // levels searched
1099  index_mean,
1100  0, // consecutive misses
1101  0, // threshold for consecutive misses; 0 -> allow no misses
1102  &index_min_old,
1103  &index_mean_old,
1104  &index_max_old,
1105  &range_sum_old,
1106  &range_at_min_index_old,
1107  &range_at_max_index_old);
1108 
1109  // Could we track object?
1110  if (0 <= index_mean_old)
1111  {
1112  // YES!
1113  // Create a Moving Object
1114  MovingObject mo;
1115  MovingObject mo_old_positions;
1116 
1117  // Set the expected information
1118  mo.map_frame = bank_argument.map_frame;
1119  mo.fixed_frame = bank_argument.fixed_frame;
1120  mo.base_frame = bank_argument.base_frame;
1121  mo.header.frame_id = bank_argument.sensor_frame;
1122  mo.header.seq = nr_objects_found;
1123  mo.header.stamp = new_time; // ros::Time(bank_stamp[bank_index_newest]);
1124  mo.seen_width = object_seen_width;
1125  mo.angle_begin = index_min * bank_argument.angle_increment + bank_argument.angle_min;
1126  mo.angle_end = index_max * bank_argument.angle_increment + bank_argument.angle_min;
1127  const double angle_mean = index_mean * bank_argument.angle_increment + bank_argument.angle_min;
1128  mo.distance_at_angle_begin = range_at_angle_begin;
1129  mo.distance_at_angle_end = range_at_angle_end;
1130  // Position is dependent on the distance and angle_mean
1131  // Reference coordinate system (relation to the Lidar):
1132  // x: forward
1133  // y: left
1134  // z: up
1135  mo.distance = distance;
1136 
1137  // Optical frame?
1138  if (bank_argument.sensor_frame_has_z_axis_forward)
1139  {
1140  // Yes, Z-axis forward, X-axis right, Y-axis down
1141  mo.position.x = (double) - distance * sinf(angle_mean);
1142  mo.position.y = 0.0;
1143  mo.position.z = (double) distance * cosf(angle_mean);
1144  }
1145  else
1146  {
1147  // No, X-axis forward, Y-axis left, Z-axis up
1148  mo.position.x = (double) distance * cosf(angle_mean);
1149  mo.position.y = (double) distance * sinf(angle_mean);
1150  mo.position.z = 0.0;
1151  }
1152 
1153  // This will be negated rotation around the Y-axis in the case of an optical frame!
1154  mo.angle_for_closest_distance = object_range_min_index * bank_argument.angle_increment +
1155  bank_argument.angle_min;
1156  mo.closest_distance = object_range_min;
1157 
1158  // Optical frame?
1159  if (bank_argument.sensor_frame_has_z_axis_forward)
1160  {
1161  // Yes, Z-axis forward, X-axis right, Y-axis down
1162  mo.closest_point.x = - object_range_min * sinf(mo.angle_for_closest_distance);
1163  mo.closest_point.y = 0.0;
1164  mo.closest_point.z = object_range_min * cosf(mo.angle_for_closest_distance);
1165  }
1166  else
1167  {
1168  // No, X-axis forward, Y-axis left, Z-axis up
1169  mo.closest_point.x = object_range_min * cosf(mo.angle_for_closest_distance);
1170  mo.closest_point.y = object_range_min * sinf(mo.angle_for_closest_distance);
1171  mo.closest_point.z = 0.0;
1172  }
1173 
1174  // Distance from sensor to object at old time
1175  const unsigned int nr_object_points_old = (index_min_old <= index_max_old) ?
1176  (index_max_old - index_min_old + 1) :
1177  bank_argument.points_per_scan - (index_min_old - index_max_old) + 1;
1178  const float distance_old = range_sum_old / nr_object_points_old;
1179  // distance is found at index_mean_old, this is the angle at which distance is found
1180  const double distance_angle_old = index_mean_old * bank_argument.angle_increment + bank_argument.angle_min;
1181  // Covered angle
1182  const double covered_angle_old = nr_object_points_old * bank_argument.angle_increment;
1183  // Width of old object
1184  const double object_seen_width_old = sqrt( range_at_min_index_old *
1185  range_at_min_index_old +
1186  range_at_max_index_old *
1187  range_at_max_index_old -
1188  2 * range_at_min_index_old *
1189  range_at_max_index_old *
1190  cosf (covered_angle_old)
1191  ); // This is the seen object width using the law of cosine
1192  // Coordinates at old time
1193  double x_old;
1194  double y_old;
1195  double z_old;
1196 
1197  if (bank_argument.sensor_frame_has_z_axis_forward)
1198  {
1199  // Yes, Z-axis forward, X-axis right, Y-axis down
1200  x_old = - distance_old * sinf(distance_angle_old);
1201  y_old = 0.0;
1202  z_old = distance_old * cosf(distance_angle_old);
1203  }
1204  else
1205  {
1206  x_old = distance_old * cosf(distance_angle_old);
1207  y_old = distance_old * sinf(distance_angle_old);
1208  z_old = 0.0;
1209  }
1210 
1211  mo_old_positions.position.x = x_old;
1212  mo_old_positions.position.y = y_old;
1213  mo_old_positions.position.z = z_old;
1214 
1215  // Lookup transformation from old position of sensor_frame to new location of sensor_frame
1216 // bool transform_old_time_map_frame_success = false;
1217 // bool transform_new_time_map_frame_success = false;
1218 // // tf::StampedTransform transform_map_frame_old_time;
1219 // // tf::StampedTransform transform_map_frame_new_time;
1220 // geometry_msgs::TransformStamped transform_map_frame_old_time;
1221 // geometry_msgs::TransformStamped transform_map_frame_new_time;
1222 // bool transform_old_time_fixed_frame_success = false;
1223 // bool transform_new_time_fixed_frame_success = false;
1224 // // tf::StampedTransform transform_fixed_frame_old_time;
1225 // // tf::StampedTransform transform_fixed_frame_new_time;
1226 // geometry_msgs::TransformStamped transform_fixed_frame_old_time;
1227 // geometry_msgs::TransformStamped transform_fixed_frame_new_time;
1228 // bool transform_old_time_base_frame_success = false;
1229 // bool transform_new_time_base_frame_success = false;
1230 // // tf::StampedTransform transform_base_frame_old_time;
1231 // // tf::StampedTransform transform_base_frame_new_time;
1232 // geometry_msgs::TransformStamped transform_base_frame_old_time;
1233 // geometry_msgs::TransformStamped transform_base_frame_new_time;
1234 
1235 // // Map frame
1236 // // OLD time
1237 // try
1238 // {
1239 // // const bool transform_available =
1240 // // tf_listener->waitForTransform(bank_argument.map_frame, // Target
1241 // // bank_argument.sensor_frame, // Source
1242 // // ros::Time(bank_stamp[bank_index_put]),
1243 // // ros::Duration(1.0)); // Timeout
1244 // // if (transform_available)
1245 // // {
1246 // // tf_listener->lookupTransform(bank_argument.map_frame,
1247 // // bank_argument.sensor_frame,
1248 // // ros::Time(bank_stamp[bank_index_put]),
1249 // // transform_map_frame_old_time); // Resulting transform
1250 // transform_map_frame_old_time = tf_buffer->lookupTransform(bank_argument.map_frame,
1251 // bank_argument.sensor_frame,
1252 // ros::Time(bank_stamp[bank_index_put]));
1253 // transform_old_time_map_frame_success = true;
1254 // // }
1255 //
1256 // // else
1257 // // {
1258 // // ROS_ERROR("Cannot determine transform from %s to %s at old time %f.", bank_argument.sensor_frame.c_str(), \
1259 // // bank_argument.map_frame.c_str(), bank_stamp[bank_index_put]);
1260 // // }
1261 //
1262 // }
1263 // catch (tf2::TransformException ex)
1264 // {
1265 // ROS_ERROR("Cannot determine transform from %s to %s at old time %f.\n%s",
1266 // bank_argument.sensor_frame.c_str(),
1267 // bank_argument.map_frame.c_str(),
1268 // bank_stamp[bank_index_put],
1269 // ex.what());
1270 // // ROS_ERROR("%s", ex.what());
1271 // // transform_old_time_map_frame_success = false;
1272 // }
1273 //
1274 // // NEW time
1275 // try
1276 // {
1277 // const bool transform_available =
1278 // tf_listener->waitForTransform(bank_argument.map_frame, // Target
1279 // bank_argument.sensor_frame, // Source
1280 // ros::Time(bank_stamp[bank_index_newest]),
1281 // ros::Duration(1.0)); // Timeout
1282 // if (transform_available)
1283 // {
1284 // tf_listener->lookupTransform(bank_argument.map_frame,
1285 // bank_argument.sensor_frame,
1286 // ros::Time(bank_stamp[bank_index_newest]),
1287 // transform_map_frame_new_time); // Resulting transform
1288 // }
1289 // else
1290 // {
1291 // ROS_ERROR("Cannot determine transform from %s to %s at new time %f.", bank_argument.sensor_frame.c_str(), \
1292 // bank_argument.map_frame.c_str(), bank_stamp[bank_index_newest]);
1293 // }
1294 // }
1295 // catch (tf::TransformException ex)
1296 // {
1297 // ROS_ERROR("%s", ex.what());
1298 // transform_new_time_map_frame_success = false;
1299 // }
1300 //
1301 // // Fixed frame
1302 // // OLD time
1303 // try
1304 // {
1305 // const bool transform_available =
1306 // tf_listener->waitForTransform(bank_argument.fixed_frame, // Target
1307 // bank_argument.sensor_frame, // Source
1308 // ros::Time(bank_stamp[bank_index_put]),
1309 // ros::Duration(1.0)); // Timeout
1310 // if (transform_available)
1311 // {
1312 // tf_listener->lookupTransform(bank_argument.fixed_frame,
1313 // bank_argument.sensor_frame,
1314 // ros::Time(bank_stamp[bank_index_put]),
1315 // transform_fixed_frame_old_time); // Resulting transform
1316 // }
1317 // else
1318 // {
1319 // ROS_ERROR("Cannot determine transform from %s to %s at old time %f.", bank_argument.sensor_frame.c_str(), \
1320 // bank_argument.fixed_frame.c_str(), bank_stamp[bank_index_put]);
1321 // }
1322 // }
1323 // catch (tf::TransformException ex)
1324 // {
1325 // ROS_ERROR("%s", ex.what());
1326 // transform_old_time_fixed_frame_success = false;
1327 // }
1328 //
1329 // // NEW time
1330 // try
1331 // {
1332 // const bool transform_available =
1333 // tf_listener->waitForTransform(bank_argument.fixed_frame, // Target
1334 // bank_argument.sensor_frame, // Source
1335 // ros::Time(bank_stamp[bank_index_newest]),
1336 // ros::Duration(1.0)); // Timeout
1337 // if (transform_available)
1338 // {
1339 // tf_listener->lookupTransform(bank_argument.fixed_frame,
1340 // bank_argument.sensor_frame,
1341 // ros::Time(bank_stamp[bank_index_newest]),
1342 // transform_fixed_frame_new_time); // Resulting transform
1343 // }
1344 // else
1345 // {
1346 // ROS_ERROR("Cannot determine transform from %s to %s at new time %f.", bank_argument.sensor_frame.c_str(), \
1347 // bank_argument.fixed_frame.c_str(), bank_stamp[bank_index_newest]);
1348 // }
1349 // }
1350 // catch (tf::TransformException ex)
1351 // {
1352 // ROS_ERROR("%s", ex.what());
1353 // transform_new_time_fixed_frame_success = false;
1354 // }
1355 //
1356 // // Base frame
1357 // // OLD time
1358 // try
1359 // {
1360 // const bool transform_available =
1361 // tf_listener->waitForTransform(bank_argument.base_frame, // Target
1362 // bank_argument.sensor_frame, // Source
1363 // ros::Time(bank_stamp[bank_index_put]),
1364 // ros::Duration(1.0)); // Timeout
1365 // if (transform_available)
1366 // {
1367 // tf_listener->lookupTransform(bank_argument.base_frame,
1368 // bank_argument.sensor_frame,
1369 // ros::Time(bank_stamp[bank_index_put]),
1370 // transform_base_frame_old_time); // Resulting transform
1371 // }
1372 // else
1373 // {
1374 // ROS_ERROR("Cannot determine transform from %s to %s at old time %f.", bank_argument.sensor_frame.c_str(), \
1375 // bank_argument.base_frame.c_str(), bank_stamp[bank_index_put]);
1376 // }
1377 // }
1378 // catch (tf::TransformException ex)
1379 // {
1380 // ROS_ERROR("%s", ex.what());
1381 // transform_old_time_base_frame_success = false;
1382 // }
1383 //
1384 // // NEW time
1385 // try
1386 // {
1387 // const bool transform_available =
1388 // tf_listener->waitForTransform(bank_argument.base_frame, // Target
1389 // bank_argument.sensor_frame, // Source
1390 // ros::Time(bank_stamp[bank_index_newest]),
1391 // ros::Duration(1.0)); // Timeout
1392 // if (transform_available)
1393 // {
1394 // tf_listener->lookupTransform(bank_argument.base_frame,
1395 // bank_argument.sensor_frame,
1396 // ros::Time(bank_stamp[bank_index_newest]),
1397 // transform_base_frame_new_time); // Resulting transform
1398 // }
1399 // else
1400 // {
1401 // ROS_ERROR("Cannot determine transform from %s to %s at new time %f.", bank_argument.sensor_frame.c_str(),
1402 // bank_argument.base_frame.c_str(), bank_stamp[bank_index_newest]);
1403 // }
1404 // }
1405 // catch (tf::TransformException ex)
1406 // {
1407 // ROS_ERROR("%s", ex.what());
1408 // transform_new_time_base_frame_success = false;
1409 // }
1410 //
1411 // // Coordinates translated
1412 // tf::Stamped<tf::Point> old_point(tf::Point(x_old, y_old, z_old),
1413 // ros::Time(bank_stamp[bank_index_put]),
1414 // bank_argument.sensor_frame);
1415 // tf::Stamped<tf::Point> new_point(tf::Point(mo.position.x, mo.position.y, mo.position.z),
1416 // ros::Time(bank_stamp[bank_index_newest]),
1417 // bank_argument.sensor_frame);
1418 // tf::Stamped<tf::Point> closest_point(tf::Point(mo.closest_point.x, mo.closest_point.y, mo.closest_point.z),
1419 // ros::Time(bank_stamp[bank_index_newest]),
1420 // bank_argument.sensor_frame);
1421 //
1422 // tf::Point old_point_in_map_frame;
1423 // tf::Point new_point_in_map_frame;
1424 // tf::Point closest_point_in_map_frame;
1425 //
1426 // tf::Point old_point_in_fixed_frame;
1427 // tf::Point new_point_in_fixed_frame;
1428 // tf::Point closest_point_in_fixed_frame;
1429 //
1430 // tf::Point old_point_in_base_frame;
1431 // tf::Point new_point_in_base_frame;
1432 // tf::Point closest_point_in_base_frame;
1433 //
1434 // if (transform_old_time_map_frame_success && transform_new_time_map_frame_success)
1435 // {
1436 // old_point_in_map_frame = transform_map_frame_old_time * old_point;
1437 // new_point_in_map_frame = transform_map_frame_new_time * new_point;
1438 // closest_point_in_map_frame = transform_map_frame_new_time * closest_point;
1439 // }
1440 // else
1441 // {
1442 // old_point_in_map_frame = old_point;
1443 // new_point_in_map_frame = new_point;
1444 // closest_point_in_map_frame = closest_point;
1445 // }
1446 //
1447 // if (transform_old_time_fixed_frame_success && transform_new_time_fixed_frame_success)
1448 // {
1449 // old_point_in_fixed_frame = transform_fixed_frame_old_time * old_point;
1450 // new_point_in_fixed_frame = transform_fixed_frame_new_time * new_point;
1451 // closest_point_in_fixed_frame = transform_fixed_frame_new_time * closest_point;
1452 // }
1453 // else
1454 // {
1455 // old_point_in_fixed_frame = old_point;
1456 // new_point_in_fixed_frame = new_point;
1457 // closest_point_in_fixed_frame = closest_point;
1458 // }
1459 //
1460 // if (transform_old_time_base_frame_success && transform_new_time_base_frame_success)
1461 // {
1462 // old_point_in_base_frame = transform_base_frame_old_time * old_point;
1463 // new_point_in_base_frame = transform_base_frame_new_time * new_point;
1464 // closest_point_in_base_frame = transform_base_frame_new_time * closest_point;
1465 // }
1466 // else
1467 // {
1468 // old_point_in_base_frame = old_point;
1469 // new_point_in_base_frame = new_point;
1470 // closest_point_in_base_frame = closest_point;
1471 // }
1472 
1473  geometry_msgs::PointStamped in;
1474  geometry_msgs::PointStamped out;
1475 
1476  // Transform old point into map, fixed and base frames at old_time
1477  in.header.frame_id = bank_argument.sensor_frame;
1478  in.header.stamp = old_time;
1479  in.point = mo_old_positions.position;
1480 
1481  try {
1482  tf_buffer->transform(in, // mo_old_positions.position,
1483  out, // mo_old_positions.position_in_map_frame,
1484  bank_argument.map_frame,
1485  old_time,
1486  bank_argument.fixed_frame);
1487  mo_old_positions.position_in_map_frame = out.point;
1488 
1489 
1490  tf_buffer->transform(in, // mo_old_positions.position,
1491  out, // mo_old_positions.position_in_fixed_frame,
1492  bank_argument.fixed_frame,
1493  old_time,
1494  bank_argument.fixed_frame);
1495  mo_old_positions.position_in_fixed_frame = out.point;
1496 
1497  tf_buffer->transform(in, // mo_old_positions.position,
1498  out, // mo_old_positions.position_in_base_frame,
1499  bank_argument.base_frame,
1500  old_time,
1501  bank_argument.fixed_frame);
1502  mo_old_positions.position_in_base_frame = out.point;
1503 
1504  // Transform new point into map, fixed and base frames at new_time
1505  in.header.stamp = new_time;
1506  in.point = mo.position;
1507 
1508  tf_buffer->transform(in, // mo.position,
1509  out, // mo.position_in_map_frame,
1510  bank_argument.map_frame,
1511  new_time,
1512  bank_argument.fixed_frame);
1513  mo.position_in_map_frame = out.point;
1514 
1515  tf_buffer->transform(in, // mo.position,
1516  out, // mo.position_in_fixed_frame,
1517  bank_argument.fixed_frame,
1518  new_time,
1519  bank_argument.fixed_frame);
1520  mo.position_in_fixed_frame = out.point;
1521 
1522  tf_buffer->transform(in, // mo.position,
1523  out, // mo.position_in_base_frame,
1524  bank_argument.base_frame,
1525  new_time,
1526  bank_argument.fixed_frame);
1527  mo.position_in_base_frame = out.point;
1528 
1529  // Transform closest point into map, fixed and base frames at new_time
1530  in.point = mo.closest_point;
1531 
1532  tf_buffer->transform(in, // mo.closest_point,
1533  out, // mo.closest_point_in_map_frame,
1534  bank_argument.map_frame,
1535  new_time,
1536  bank_argument.fixed_frame);
1537  mo.closest_point_in_map_frame = out.point;
1538 
1539  tf_buffer->transform(in, // mo.closest_point,
1540  out, // mo.closest_point_in_fixed_frame,
1541  bank_argument.fixed_frame,
1542  new_time,
1543  bank_argument.fixed_frame);
1544  mo.closest_point_in_fixed_frame = out.point;
1545 
1546  tf_buffer->transform(in, // mo.closest_point,
1547  out, // mo.closest_point_in_base_frame,
1548  bank_argument.base_frame,
1549  new_time,
1550  bank_argument.fixed_frame);
1551  mo.closest_point_in_base_frame = out.point;
1552  }
1553  catch (tf2::TransformException e)
1554  {
1555  ROS_ERROR_STREAM("Caught some exception: " << e.what());
1556  }
1557 
1558  // Check how object has moved
1559  const double dx_map = mo.position_in_map_frame.x - mo_old_positions.position_in_map_frame.x;
1560  const double dy_map = mo.position_in_map_frame.y - mo_old_positions.position_in_map_frame.y;
1561  const double dz_map = mo.position_in_map_frame.z - mo_old_positions.position_in_map_frame.z;
1562  const double dx_fixed = mo.position_in_fixed_frame.x - mo_old_positions.position_in_fixed_frame.x;
1563  const double dy_fixed = mo.position_in_fixed_frame.y - mo_old_positions.position_in_fixed_frame.y;
1564  const double dz_fixed = mo.position_in_fixed_frame.z - mo_old_positions.position_in_fixed_frame.z;
1565  const double dx_base = mo.position_in_base_frame.x - mo_old_positions.position_in_base_frame.x;
1566  const double dy_base = mo.position_in_base_frame.y - mo_old_positions.position_in_base_frame.y;
1567  const double dz_base = mo.position_in_base_frame.z - mo_old_positions.position_in_base_frame.z;
1568  const double dx_sensor = mo.position.x - x_old;
1569  const double dy_sensor = mo.position.y - y_old;
1570  const double dz_sensor = mo.position.z - z_old;
1571 
1572 
1573 // // Set old position in map_frame
1574 // mo_old_positions.position_in_map_frame.x = old_point_in_map_frame.x();
1575 // mo_old_positions.position_in_map_frame.y = old_point_in_map_frame.y();
1576 // mo_old_positions.position_in_map_frame.z = old_point_in_map_frame.z();
1577 //
1578 // // Set old position in fixed_frame
1579 // mo_old_positions.position_in_fixed_frame.x = old_point_in_fixed_frame.x();
1580 // mo_old_positions.position_in_fixed_frame.y = old_point_in_fixed_frame.y();
1581 // mo_old_positions.position_in_fixed_frame.z = old_point_in_fixed_frame.z();
1582 //
1583 // // Set old position in base_frame
1584 // mo_old_positions.position_in_base_frame.x = old_point_in_base_frame.x();
1585 // mo_old_positions.position_in_base_frame.y = old_point_in_base_frame.y();
1586 // mo_old_positions.position_in_base_frame.z = old_point_in_base_frame.z();
1587 //
1588 // // Set position in map_frame
1589 // mo.position_in_map_frame.x = new_point_in_map_frame.x();
1590 // mo.position_in_map_frame.y = new_point_in_map_frame.y();
1591 // mo.position_in_map_frame.z = new_point_in_map_frame.z();
1592 //
1593 // // Set position in fixed_frame
1594 // mo.position_in_fixed_frame.x = new_point_in_fixed_frame.x();
1595 // mo.position_in_fixed_frame.y = new_point_in_fixed_frame.y();
1596 // mo.position_in_fixed_frame.z = new_point_in_fixed_frame.z();
1597 //
1598 // // Set position in base_frame
1599 // mo.position_in_base_frame.x = new_point_in_base_frame.x();
1600 // mo.position_in_base_frame.y = new_point_in_base_frame.y();
1601 // mo.position_in_base_frame.z = new_point_in_base_frame.z();
1602 //
1603 // // Set closest point in map_frame
1604 // mo.closest_point_in_map_frame.x = closest_point_in_map_frame.x();
1605 // mo.closest_point_in_map_frame.y = closest_point_in_map_frame.y();
1606 // mo.closest_point_in_map_frame.z = closest_point_in_map_frame.z();
1607 //
1608 // // Set closest point in fixed_frame
1609 // mo.closest_point_in_fixed_frame.x = closest_point_in_fixed_frame.x();
1610 // mo.closest_point_in_fixed_frame.y = closest_point_in_fixed_frame.y();
1611 // mo.closest_point_in_fixed_frame.z = closest_point_in_fixed_frame.z();
1612 //
1613 // // Set closest point in base_frame
1614 // mo.closest_point_in_base_frame.x = closest_point_in_base_frame.x();
1615 // mo.closest_point_in_base_frame.y = closest_point_in_base_frame.y();
1616 // mo.closest_point_in_base_frame.z = closest_point_in_base_frame.z();
1617 //
1618 // // Check how object has moved
1619 // const float dx_map = new_point_in_map_frame.x() - old_point_in_map_frame.x();
1620 // const float dy_map = new_point_in_map_frame.y() - old_point_in_map_frame.y();
1621 // const float dz_map = new_point_in_map_frame.z() - old_point_in_map_frame.z();
1622 // const float dx_fixed = new_point_in_fixed_frame.x() - old_point_in_fixed_frame.x();
1623 // const float dy_fixed = new_point_in_fixed_frame.y() - old_point_in_fixed_frame.y();
1624 // const float dz_fixed = new_point_in_fixed_frame.z() - old_point_in_fixed_frame.z();
1625 // const float dx_base = new_point_in_base_frame.x() - old_point_in_base_frame.x();
1626 // const float dy_base = new_point_in_base_frame.y() - old_point_in_base_frame.y();
1627 // const float dz_base = new_point_in_base_frame.z() - old_point_in_base_frame.z();
1628 // const float dx_sensor = mo.position.x - x_old;
1629 // const float dy_sensor = mo.position.y - y_old;
1630 // const float dz_sensor = mo.position.z - z_old;
1631 
1632  // And with what velocity
1633 // const double dt = bank_stamp[bank_index_newest] - bank_stamp[bank_index_put];
1634  const double dt = mo.header.stamp.toSec() - bank_stamp[bank_index_put];
1635 // ROS_ERROR_STREAM("newest stamp = " << mo.header.stamp.toSec() << std::endl << "oldest stamp = " << bank_stamp[bank_index_put] << std::endl << "dt = " << dt << std::endl);
1636  mo.velocity.x = dx_sensor / dt;
1637  mo.velocity.y = dy_sensor / dt;
1638  mo.velocity.z = dz_sensor / dt;
1639  mo.velocity_in_map_frame.x = dx_map / dt;
1640  mo.velocity_in_map_frame.y = dy_map / dt;
1641  mo.velocity_in_map_frame.z = dz_map / dt;
1642  mo.velocity_in_fixed_frame.x = dx_fixed / dt;
1643  mo.velocity_in_fixed_frame.y = dy_fixed / dt;
1644  mo.velocity_in_fixed_frame.z = dz_fixed / dt;
1645  mo.velocity_in_base_frame.x = dx_base / dt;
1646  mo.velocity_in_base_frame.y = dy_base / dt;
1647  mo.velocity_in_base_frame.z = dz_base / dt;
1648 
1649  // Calculate speed and normalized velocity
1650  mo.speed = sqrt(mo.velocity.x * mo.velocity.x +
1651  mo.velocity.y * mo.velocity.y +
1652  mo.velocity.z * mo.velocity.z);
1653  mo.speed_in_map_frame = sqrt(mo.velocity_in_map_frame.x * mo.velocity_in_map_frame.x +
1654  mo.velocity_in_map_frame.y * mo.velocity_in_map_frame.y +
1655  mo.velocity_in_map_frame.z * mo.velocity_in_map_frame.z);
1656  mo.speed_in_fixed_frame = sqrt(mo.velocity_in_fixed_frame.x * mo.velocity_in_fixed_frame.x +
1657  mo.velocity_in_fixed_frame.y * mo.velocity_in_fixed_frame.y +
1658  mo.velocity_in_fixed_frame.z * mo.velocity_in_fixed_frame.z);
1659  mo.speed_in_base_frame = sqrt(mo.velocity_in_base_frame.x * mo.velocity_in_base_frame.x +
1660  mo.velocity_in_base_frame.y * mo.velocity_in_base_frame.y +
1661  mo.velocity_in_base_frame.z * mo.velocity_in_base_frame.z);
1662 
1663  // Avoid division by 0
1664  if (0 < mo.speed)
1665  {
1666  mo.velocity_normalized.x = mo.velocity.x / mo.speed;
1667  mo.velocity_normalized.y = mo.velocity.y / mo.speed;
1668  mo.velocity_normalized.z = mo.velocity.z / mo.speed;
1669  }
1670  else
1671  {
1672  mo.velocity_normalized.x = 0.0;
1673  mo.velocity_normalized.y = 0.0;
1674  mo.velocity_normalized.z = 0.0;
1675  }
1676  if (0 < mo.speed_in_map_frame)
1677  {
1678  mo.velocity_normalized_in_map_frame.x = mo.velocity_in_map_frame.x / mo.speed_in_map_frame;
1679  mo.velocity_normalized_in_map_frame.y = mo.velocity_in_map_frame.y / mo.speed_in_map_frame;
1680  mo.velocity_normalized_in_map_frame.z = mo.velocity_in_map_frame.z / mo.speed_in_map_frame;
1681  }
1682  else
1683  {
1684  mo.velocity_normalized_in_map_frame.x = 0.0;
1685  mo.velocity_normalized_in_map_frame.y = 0.0;
1686  mo.velocity_normalized_in_map_frame.z = 0.0;
1687  }
1688  if (0 < mo.speed_in_fixed_frame)
1689  {
1690  mo.velocity_normalized_in_fixed_frame.x = mo.velocity_in_fixed_frame.x / mo.speed_in_fixed_frame;
1691  mo.velocity_normalized_in_fixed_frame.y = mo.velocity_in_fixed_frame.y / mo.speed_in_fixed_frame;
1692  mo.velocity_normalized_in_fixed_frame.z = mo.velocity_in_fixed_frame.z / mo.speed_in_fixed_frame;
1693  }
1694  else
1695  {
1696  mo.velocity_normalized_in_fixed_frame.x = 0.0;
1697  mo.velocity_normalized_in_fixed_frame.y = 0.0;
1698  mo.velocity_normalized_in_fixed_frame.z = 0.0;
1699  }
1700  if (0 < mo.speed_in_base_frame)
1701  {
1702  mo.velocity_normalized_in_base_frame.x = mo.velocity_in_base_frame.x / mo.speed_in_base_frame;
1703  mo.velocity_normalized_in_base_frame.y = mo.velocity_in_base_frame.y / mo.speed_in_base_frame;
1704  mo.velocity_normalized_in_base_frame.z = mo.velocity_in_base_frame.z / mo.speed_in_base_frame;
1705  }
1706  else
1707  {
1708  mo.velocity_normalized_in_base_frame.x = 0.0;
1709  mo.velocity_normalized_in_base_frame.y = 0.0;
1710  mo.velocity_normalized_in_base_frame.z = 0.0;
1711  }
1712 
1713  // Threshold check
1714  if (bank_argument.object_threshold_min_speed <= mo.speed ||
1715  bank_argument.object_threshold_min_speed <= mo.speed_in_map_frame ||
1716  bank_argument.object_threshold_min_speed <= mo.speed_in_fixed_frame ||
1717  bank_argument.object_threshold_min_speed <= mo.speed_in_base_frame)
1718  {
1719  // We believe that the object is moving in relation to at least one of the frames
1720  ROS_DEBUG_STREAM("Moving object:" << std::endl \
1721  << " (sensor) x=" << std::setw(12) << std::left << mo.position.x \
1722  << " y=" << std::setw(12) << std::left << mo.position.y \
1723  << " z=" << std::setw(12) << std::left << mo.position.z \
1724  << std::endl \
1725  << " vx=" << std::setw(12) << std::left << mo.velocity.x \
1726  << " vy=" << std::setw(12) << std::left << mo.velocity.y \
1727  << " vz=" << std::setw(12) << std::left << mo.velocity.z \
1728  << " speed=" << mo.speed \
1729  << std::endl \
1730  << " (map) x=" << std::setw(12) << std::left << mo.position_in_map_frame.x \
1731  << " y=" << std::setw(12) << std::left << mo.position_in_map_frame.y \
1732  << " z=" << std::setw(12) << std::left << mo.position_in_map_frame.z \
1733  << std::endl \
1734  << " vx=" << std::setw(12) << std::left << mo.velocity_in_map_frame.x \
1735  << " vy=" << std::setw(12) << std::left << mo.velocity_in_map_frame.y \
1736  << " vz=" << std::setw(12) << std::left << mo.velocity_in_map_frame.z \
1737  << " speed=" << mo.speed_in_map_frame \
1738  << std::endl \
1739  << " (fixed) x=" << std::setw(12) << std::left << mo.position_in_fixed_frame.x \
1740  << " y=" << std::setw(12) << std::left << mo.position_in_fixed_frame.y \
1741  << " z=" << std::setw(12) << std::left << mo.position_in_fixed_frame.z \
1742  << std::endl \
1743  << " vx=" << std::setw(12) << std::left << mo.velocity_in_fixed_frame.x \
1744  << " vy=" << std::setw(12) << std::left << mo.velocity_in_fixed_frame.y \
1745  << " vz=" << std::setw(12) << std::left << mo.velocity_in_fixed_frame.z \
1746  << " speed=" << mo.speed_in_fixed_frame \
1747  << std::endl \
1748  << " (base) x=" << std::setw(12) << std::left << mo.position_in_base_frame.x \
1749  << " y=" << std::setw(12) << std::left << mo.position_in_base_frame.y \
1750  << " z=" << std::setw(12) << std::left << mo.position_in_base_frame.z \
1751  << std::endl \
1752  << " vx=" << std::setw(12) << std::left << mo.velocity_in_base_frame.x \
1753  << " vy=" << std::setw(12) << std::left << mo.velocity_in_base_frame.y \
1754  << " vz=" << std::setw(12) << std::left << mo.velocity_in_base_frame.z \
1755  << " speed=" << mo.speed_in_base_frame \
1756  << std::endl);
1757 
1758  // Calculate confidence value using the user-defined function
1759  mo.confidence = calculateConfidence(mo,
1760  bank_argument,
1761  dt,
1762  object_seen_width_old);
1763 
1764  // Bound the value to [0,1]
1765  mo.confidence = (mo.confidence < 0.0 ? 0.0 : mo.confidence);
1766  mo.confidence = (mo.confidence < 1.0 ? mo.confidence : 1.0);
1767 
1768  // Are we confident enough to report this object?
1769  if (bank_argument.object_threshold_min_confidence <= mo.confidence)
1770  {
1771  // Adapt EMA message intensities
1772  if (bank_argument.publish_ema)
1773  {
1774  // Are we avoiding wrapping around the bank edges?
1775  if (index_min <= index_max)
1776  {
1777  // YES
1778  for (unsigned int k=index_min; k<=index_max; ++k)
1779  {
1780  msg_ema.intensities[k] = 300.0f;
1781  }
1782  }
1783  else
1784  {
1785  // NO - we are wrapping around
1786  // index_max < index_min
1787  for (unsigned int k=index_min; k<bank_argument.points_per_scan; ++k)
1788  {
1789  msg_ema.intensities[k] = 300.0f;
1790  }
1791  for (unsigned int k=0; k<index_max; ++k)
1792  {
1793  msg_ema.intensities[k] = 300.0f;
1794  }
1795  }
1796  }
1797 
1798  // Push back the moving object info to the msg
1799  moa.objects.push_back(mo);
1800  moa_old_positions.objects.push_back(mo_old_positions);
1801  }
1802  }
1803  }
1804  }
1805 
1806  i = index_at_angle_end + 1;
1807  nr_object_points = 0;
1808  }
1809 
1810  // Filter found objects
1811 // mergeFoundObjects(&moa);
1812 
1813  // Moving object array message
1814  ++moa_seq;
1815  if (bank_argument.publish_objects && 0 < moa.objects.size())
1816  {
1817  moa.origin_node_name = ros::this_node::getName() + bank_argument.node_name_suffix;
1818 
1819  // Publish MOA message
1820  pub_objects.publish(moa);
1821  }
1822 
1823  // Save timestamp
1824  ros::Time now = ros::Time::now();
1825 
1826  // EMA message
1827  if (bank_argument.publish_ema)
1828  {
1829  // Copy ranges and set header
1830  memcpy(msg_ema.ranges.data(), &bank_ranges_ema[bank_index_newest][0], bank_ranges_bytes);
1831  msg_ema.header.seq = moa_seq;
1832  msg_ema.header.stamp = now;
1833 
1834  // Publish EMA message
1835  pub_ema.publish(msg_ema);
1836  }
1837 
1838  // Update headers of the marker, arrow, delta position and width messages
1839  if (bank_argument.publish_objects_closest_point_markers)
1840  {
1841  msg_objects_closest_point_markers.header.stamp = now;
1842  msg_objects_closest_point_markers.header.seq = moa_seq;
1843  }
1844  if (bank_argument.publish_objects_velocity_arrows)
1845  {
1846  msg_objects_velocity_arrow.header.stamp = now;
1847  msg_objects_velocity_arrow.header.seq = moa_seq;
1848  }
1849  if (bank_argument.publish_objects_delta_position_lines)
1850  {
1851  msg_objects_delta_position_line.header.stamp = now;
1852  msg_objects_delta_position_line.header.seq = moa_seq;
1853  }
1854  if (bank_argument.publish_objects_width_lines)
1855  {
1856  msg_objects_width_line.header.stamp = now;
1857  msg_objects_width_line.header.seq = moa_seq;
1858  }
1859 
1860  // Go through found objects
1861  MovingObject * mo;
1862  MovingObject * mo_old_positions;
1863  const unsigned int nr_moving_objects_found = moa.objects.size();
1864  for (unsigned int i=0; i<nr_moving_objects_found; ++i)
1865  {
1866  mo = &moa.objects[i];
1867  mo_old_positions = &moa_old_positions.objects[i];
1868 
1869  // Laserscan Marker (square)
1870  if (bank_argument.publish_objects_closest_point_markers)
1871  {
1872  // Find index for closest range for this object - reverse calculation
1873  const unsigned int distance_min_index =
1874  round((mo->angle_for_closest_distance - bank_argument.angle_min) / bank_argument.angle_increment);
1875  msg_objects_closest_point_markers.ranges[distance_min_index] = mo->closest_distance;
1876  msg_objects_closest_point_markers.intensities[distance_min_index] = 1000;
1877  }
1878 
1879  // Visualization Marker (velocity arrow)
1880  if (bank_argument.publish_objects_velocity_arrows)
1881  {
1882  msg_objects_velocity_arrow.id = i;
1883  if (bank_argument.velocity_arrows_use_sensor_frame)
1884  {
1885  // Origin: (the size of points is 2)
1886  msg_objects_velocity_arrow.points[0].x = mo->position.x;
1887  msg_objects_velocity_arrow.points[0].y = mo->position.y;
1888  msg_objects_velocity_arrow.points[0].z = mo->position.z;
1889  // End:
1890  msg_objects_velocity_arrow.points[1].x = mo->position.x + mo->velocity.x;
1891  msg_objects_velocity_arrow.points[1].y = mo->position.y + mo->velocity.y;
1892  msg_objects_velocity_arrow.points[1].z = mo->position.z + mo->velocity.z;
1893  }
1894  else if (bank_argument.velocity_arrows_use_base_frame)
1895  {
1896  // Origin (the size of points is 2)
1897  msg_objects_velocity_arrow.points[0].x = mo->position_in_base_frame.x;
1898  msg_objects_velocity_arrow.points[0].y = mo->position_in_base_frame.y;
1899  msg_objects_velocity_arrow.points[0].z = mo->position_in_base_frame.z;
1900  // End:
1901  msg_objects_velocity_arrow.points[1].x = mo->position_in_base_frame.x + mo->velocity_in_base_frame.x;
1902  msg_objects_velocity_arrow.points[1].y = mo->position_in_base_frame.y + mo->velocity_in_base_frame.y;
1903  msg_objects_velocity_arrow.points[1].z = mo->position_in_base_frame.z + mo->velocity_in_base_frame.z;
1904  }
1905  else if (bank_argument.velocity_arrows_use_fixed_frame)
1906  {
1907  // Origin (the size of points is 2)
1908  msg_objects_velocity_arrow.points[0].x = mo->position_in_fixed_frame.x;
1909  msg_objects_velocity_arrow.points[0].y = mo->position_in_fixed_frame.y;
1910  msg_objects_velocity_arrow.points[0].z = mo->position_in_fixed_frame.z;
1911  // End:
1912  msg_objects_velocity_arrow.points[1].x = mo->position_in_fixed_frame.x + mo->velocity_in_fixed_frame.x;
1913  msg_objects_velocity_arrow.points[1].y = mo->position_in_fixed_frame.y + mo->velocity_in_fixed_frame.y;
1914  msg_objects_velocity_arrow.points[1].z = mo->position_in_fixed_frame.z + mo->velocity_in_fixed_frame.z;
1915  }
1916  else
1917  {
1918  // Origin (the size of points is 2)
1919  msg_objects_velocity_arrow.points[0].x = mo->position_in_map_frame.x;
1920  msg_objects_velocity_arrow.points[0].y = mo->position_in_map_frame.y;
1921  msg_objects_velocity_arrow.points[0].z = mo->position_in_map_frame.z;
1922  // End:
1923  msg_objects_velocity_arrow.points[1].x = mo->position_in_map_frame.x + mo->velocity_in_map_frame.x;
1924  msg_objects_velocity_arrow.points[1].y = mo->position_in_map_frame.y + mo->velocity_in_map_frame.y;
1925  msg_objects_velocity_arrow.points[1].z = mo->position_in_map_frame.z + mo->velocity_in_map_frame.z;
1926  }
1927 
1928  // Color of the arrow represents the confidence black=low, white=high
1929  if (bank_argument.velocity_arrows_use_full_gray_scale && bank_argument.object_threshold_min_confidence < 1)
1930  {
1931  const double adapted_confidence = (mo->confidence - bank_argument.object_threshold_min_confidence) /
1932  (1 - bank_argument.object_threshold_min_confidence);
1933  msg_objects_velocity_arrow.color.r = adapted_confidence;
1934  msg_objects_velocity_arrow.color.g = adapted_confidence;
1935  msg_objects_velocity_arrow.color.b = adapted_confidence;
1936  }
1937  else
1938  {
1939  msg_objects_velocity_arrow.color.r = mo->confidence;
1940  msg_objects_velocity_arrow.color.g = mo->confidence;
1941  msg_objects_velocity_arrow.color.b = mo->confidence;
1942  }
1943 
1944  // Add to array of markers
1945  msg_objects_velocity_arrows.markers.push_back(msg_objects_velocity_arrow);
1946  }
1947 
1948  // Visualization Marker (delta position)
1949  if (bank_argument.publish_objects_delta_position_lines)
1950  {
1951  msg_objects_delta_position_line.id = i;
1952 
1953  // Copy line end points
1954  msg_objects_delta_position_line.points[0].x = mo_old_positions->position.x;
1955  msg_objects_delta_position_line.points[0].y = mo_old_positions->position.y;
1956  msg_objects_delta_position_line.points[0].z = mo_old_positions->position.z;
1957  msg_objects_delta_position_line.points[1].x = mo->position.x;
1958  msg_objects_delta_position_line.points[1].y = mo->position.y;
1959  msg_objects_delta_position_line.points[1].z = mo->position.z;
1960 
1961  // Add to array of markers
1962  msg_objects_delta_position_lines.markers.push_back(msg_objects_delta_position_line);
1963  }
1964 
1965  // Visualization Marker (width)
1966  if (bank_argument.publish_objects_width_lines)
1967  {
1968  msg_objects_width_line.id = i;
1969 
1970  // Calculate line end points
1971  if (!bank_argument.sensor_frame_has_z_axis_forward)
1972  {
1973  // angle_min
1974  msg_objects_width_line.points[0].x = mo->distance_at_angle_begin * cosf(mo->angle_begin);
1975  msg_objects_width_line.points[0].y = mo->distance_at_angle_begin * sinf(mo->angle_begin);
1976  msg_objects_width_line.points[0].z = 0.0;
1977  // angle_max
1978  msg_objects_width_line.points[1].x = mo->distance_at_angle_end * cosf(mo->angle_end);
1979  msg_objects_width_line.points[1].y = mo->distance_at_angle_end * sinf(mo->angle_end);
1980  msg_objects_width_line.points[1].z = 0.0;
1981  }
1982  else
1983  {
1984  // angle_min
1985  msg_objects_width_line.points[0].x = -mo->distance_at_angle_begin * sinf(mo->angle_begin);
1986  msg_objects_width_line.points[0].y = 0.0;
1987  msg_objects_width_line.points[0].z = mo->distance_at_angle_begin * cosf(mo->angle_begin);
1988  // angle_max
1989  msg_objects_width_line.points[1].x = -mo->distance_at_angle_end * sinf(mo->angle_end);
1990  msg_objects_width_line.points[1].y = 0.0;
1991  msg_objects_width_line.points[1].z = mo->distance_at_angle_end * cosf(mo->angle_end);
1992  }
1993 
1994  // Add to array of markers
1995  msg_objects_width_lines.markers.push_back(msg_objects_width_line);
1996  }
1997  }
1998 
1999  // Publish if we are supposed to
2000  if (bank_argument.publish_objects_closest_point_markers)
2001  {
2002  pub_objects_closest_point_markers.publish(msg_objects_closest_point_markers);
2003  }
2004 
2005  // Dito
2006  if (bank_argument.publish_objects_velocity_arrows)
2007  {
2008  pub_objects_velocity_arrows.publish(msg_objects_velocity_arrows);
2009  }
2010 
2011  // Dito
2012  if (bank_argument.publish_objects_delta_position_lines)
2013  {
2014  pub_objects_delta_position_lines.publish(msg_objects_delta_position_lines);
2015  }
2016 
2017  // Dito
2018  if (bank_argument.publish_objects_width_lines)
2019  {
2020  pub_objects_width_lines.publish(msg_objects_width_lines);
2021  }
2022 
2023  // Reset range and intensity of markers and delete found objects
2024  if (bank_argument.publish_objects_closest_point_markers)
2025  {
2026  for (unsigned int i=0; i<nr_moving_objects_found; ++i)
2027  {
2028  mo = &moa.objects[i];
2029  const unsigned int distance_min_index =
2030  round((mo->angle_for_closest_distance - bank_argument.angle_min) / bank_argument.angle_increment);
2031  msg_objects_closest_point_markers.ranges[distance_min_index] = msg_objects_closest_point_markers.range_max + 10.0;
2032  msg_objects_closest_point_markers.intensities[distance_min_index] = 0.0;
2033  }
2034  }
2035  if (bank_argument.publish_objects_velocity_arrows)
2036  {
2037  msg_objects_velocity_arrows.markers.clear();
2038  }
2039  if (bank_argument.publish_objects_delta_position_lines)
2040  {
2041  msg_objects_delta_position_lines.markers.clear();
2042  }
2043  if (bank_argument.publish_objects_width_lines)
2044  {
2045  msg_objects_width_lines.markers.clear();
2046  }
2047  if (bank_argument.publish_ema)
2048  {
2049  bzero(msg_ema.intensities.data(), bank_ranges_bytes);
2050  }
2051 }
2052 
2053 
2054 /* HANDLING ENDIANNESS */
2055 void Bank::reverseBytes(byte_t * bytes, unsigned int nr_bytes)
2056 {
2057  const unsigned int half_way = nr_bytes/2;
2058  for (unsigned int i=0; i<half_way; ++i)
2059  {
2060  const byte_t tmp_byte = bytes[i];
2061  bytes[i] = bytes[nr_bytes - i - 1];
2062  bytes[nr_bytes - i - 1] = tmp_byte;
2063  }
2064 }
2065 
2066 
2067 /*
2068  * Read offsets and nr of bytes from PointCloud2 message
2069  */
2070 int Bank::getOffsetsAndBytes(BankArgument bank_argument, sensor_msgs::PointCloud2::ConstPtr msg)
2071 {
2072  PC2_message_x_offset = -1;
2073  PC2_message_x_bytes = -1;
2074  PC2_message_y_offset = -1;
2075  PC2_message_y_bytes = -1;
2076  PC2_message_z_offset = -1;
2077  PC2_message_z_bytes = -1;
2078  const bool must_reverse_bytes = (msg->is_bigendian != !machine_is_little_endian);
2079  const unsigned int fields = msg->fields.size();
2080  byte_t tmp_byte4[4]; // Used for reading offset, count = 1 (ROS: uint32)
2081  byte_t tmp_byte; // Used for reading datatype
2082 
2083  for (unsigned int i=0; i<fields; ++i)
2084  {
2085  // X
2086  if (strcmp(bank_argument.PC2_message_x_coordinate_field_name.c_str(),
2087  msg->fields[i].name.c_str()) == 0)
2088  {
2089  // Read offset
2090  memcpy(&tmp_byte4[0], &msg->fields[i].offset, 4);
2091  if (must_reverse_bytes)
2092  {
2093  reverseBytes(tmp_byte4, 4);
2094  }
2095  memcpy(&PC2_message_x_offset, tmp_byte4, 4);
2096 
2097  // Read datatype
2098  tmp_byte = msg->fields[i].datatype;
2099  if (tmp_byte == sensor_msgs::PointField::INT8 ||
2100  tmp_byte == sensor_msgs::PointField::UINT8)
2101  {
2102  PC2_message_x_bytes = 1;
2103  }
2104  else if (tmp_byte == sensor_msgs::PointField::INT16 ||
2105  tmp_byte == sensor_msgs::PointField::UINT16)
2106  {
2107  PC2_message_x_bytes = 2;
2108  }
2109  else if (tmp_byte == sensor_msgs::PointField::INT32 ||
2110  tmp_byte == sensor_msgs::PointField::UINT32 ||
2111  tmp_byte == sensor_msgs::PointField::FLOAT32)
2112  {
2113  PC2_message_x_bytes = 4;
2114  }
2115  else if (tmp_byte == sensor_msgs::PointField::FLOAT64)
2116  {
2117  PC2_message_x_bytes = 8;
2118  }
2119  else
2120  {
2121  ROS_ERROR("Cannot determine number of bytes for X coordinate");
2122  return -1;
2123  }
2124  }
2125  // Y
2126  else if (strcmp(bank_argument.PC2_message_y_coordinate_field_name.c_str(),
2127  msg->fields[i].name.c_str()) == 0)
2128  {
2129  // Read offset
2130  memcpy(&tmp_byte4[0], &msg->fields[i].offset, 4);
2131  if (must_reverse_bytes)
2132  {
2133  reverseBytes(tmp_byte4, 4);
2134  }
2135  memcpy(&PC2_message_y_offset, tmp_byte4, 4);
2136 
2137  // Read datatype
2138  tmp_byte = msg->fields[i].datatype;
2139  if (tmp_byte == sensor_msgs::PointField::INT8 ||
2140  tmp_byte == sensor_msgs::PointField::UINT8)
2141  {
2142  PC2_message_y_bytes = 1;
2143  }
2144  else if (tmp_byte == sensor_msgs::PointField::INT16 ||
2145  tmp_byte == sensor_msgs::PointField::UINT16)
2146  {
2147  PC2_message_y_bytes = 2;
2148  }
2149  else if (tmp_byte == sensor_msgs::PointField::INT32 ||
2150  tmp_byte == sensor_msgs::PointField::UINT32 ||
2151  tmp_byte == sensor_msgs::PointField::FLOAT32)
2152  {
2153  PC2_message_y_bytes = 4;
2154  }
2155  else if (tmp_byte == sensor_msgs::PointField::FLOAT64)
2156  {
2157  PC2_message_y_bytes = 8;
2158  }
2159  else
2160  {
2161  ROS_ERROR("Cannot determine number of bytes for Y coordinate");
2162  return -1;
2163  }
2164  }
2165  // Z
2166  else if (strcmp(bank_argument.PC2_message_z_coordinate_field_name.c_str(),
2167  msg->fields[i].name.c_str()) == 0)
2168  {
2169  // Read offset
2170  memcpy(&tmp_byte4[0], &msg->fields[i].offset, 4);
2171  if (must_reverse_bytes)
2172  {
2173  reverseBytes(tmp_byte4, 4);
2174  }
2175  memcpy(&PC2_message_z_offset, tmp_byte4, 4);
2176 
2177  // Read datatype
2178  tmp_byte = msg->fields[i].datatype;
2179  if (tmp_byte == sensor_msgs::PointField::INT8 ||
2180  tmp_byte == sensor_msgs::PointField::UINT8)
2181  {
2182  PC2_message_z_bytes = 1;
2183  }
2184  else if (tmp_byte == sensor_msgs::PointField::INT16 ||
2185  tmp_byte == sensor_msgs::PointField::UINT16)
2186  {
2187  PC2_message_z_bytes = 2;
2188  }
2189  else if (tmp_byte == sensor_msgs::PointField::INT32 ||
2190  tmp_byte == sensor_msgs::PointField::UINT32 ||
2191  tmp_byte == sensor_msgs::PointField::FLOAT32)
2192  {
2193  PC2_message_z_bytes = 4;
2194  }
2195  else if (tmp_byte == sensor_msgs::PointField::FLOAT64)
2196  {
2197  PC2_message_z_bytes = 8;
2198  }
2199  else
2200  {
2201  ROS_ERROR("Cannot determine number of bytes for Z coordinate");
2202  return -1;
2203  }
2204  }
2205  }
2206 
2207  if (0 <= PC2_message_x_offset &&
2208  0 <= PC2_message_x_bytes &&
2209  0 <= PC2_message_y_offset &&
2210  0 <= PC2_message_y_bytes &&
2211  0 <= PC2_message_z_offset &&
2212  0 <= PC2_message_z_bytes)
2213  {
2214  return 0;
2215  }
2216  else
2217  {
2218  ROS_ERROR("Could not determine offsets and bytes");
2219  return -1;
2220  }
2221 }
2222 
2223 int Bank::getOffsetsAndBytes(BankArgument bank_argument, const sensor_msgs::PointCloud2 * msg)
2224 {
2225  PC2_message_x_offset = -1;
2226  PC2_message_x_bytes = -1;
2227  PC2_message_y_offset = -1;
2228  PC2_message_y_bytes = -1;
2229  PC2_message_z_offset = -1;
2230  PC2_message_z_bytes = -1;
2231  const bool must_reverse_bytes = (msg->is_bigendian != !machine_is_little_endian);
2232  const unsigned int fields = msg->fields.size();
2233  byte_t tmp_byte4[4]; // Used for reading offset, count = 1 (ROS: uint32)
2234  byte_t tmp_byte; // Used for reading datatype
2235 
2236  for (unsigned int i=0; i<fields; ++i)
2237  {
2238  // X
2239  if (strcmp(bank_argument.PC2_message_x_coordinate_field_name.c_str(),
2240  msg->fields[i].name.c_str()) == 0)
2241  {
2242  // Read offset
2243  memcpy(&tmp_byte4[0], &msg->fields[i].offset, 4);
2244  if (must_reverse_bytes)
2245  {
2246  reverseBytes(tmp_byte4, 4);
2247  }
2248  memcpy(&PC2_message_x_offset, tmp_byte4, 4);
2249 
2250  // Read datatype
2251  tmp_byte = msg->fields[i].datatype;
2252  if (tmp_byte == sensor_msgs::PointField::INT8 ||
2253  tmp_byte == sensor_msgs::PointField::UINT8)
2254  {
2255  PC2_message_x_bytes = 1;
2256  }
2257  else if (tmp_byte == sensor_msgs::PointField::INT16 ||
2258  tmp_byte == sensor_msgs::PointField::UINT16)
2259  {
2260  PC2_message_x_bytes = 2;
2261  }
2262  else if (tmp_byte == sensor_msgs::PointField::INT32 ||
2263  tmp_byte == sensor_msgs::PointField::UINT32 ||
2264  tmp_byte == sensor_msgs::PointField::FLOAT32)
2265  {
2266  PC2_message_x_bytes = 4;
2267  }
2268  else if (tmp_byte == sensor_msgs::PointField::FLOAT64)
2269  {
2270  PC2_message_x_bytes = 8;
2271  }
2272  else
2273  {
2274  ROS_ERROR("Cannot determine number of bytes for X coordinate");
2275  return -1;
2276  }
2277  }
2278  // Y
2279  else if (strcmp(bank_argument.PC2_message_y_coordinate_field_name.c_str(),
2280  msg->fields[i].name.c_str()) == 0)
2281  {
2282  // Read offset
2283  memcpy(&tmp_byte4[0], &msg->fields[i].offset, 4);
2284  if (must_reverse_bytes)
2285  {
2286  reverseBytes(tmp_byte4, 4);
2287  }
2288  memcpy(&PC2_message_y_offset, tmp_byte4, 4);
2289 
2290  // Read datatype
2291  tmp_byte = msg->fields[i].datatype;
2292  if (tmp_byte == sensor_msgs::PointField::INT8 ||
2293  tmp_byte == sensor_msgs::PointField::UINT8)
2294  {
2295  PC2_message_y_bytes = 1;
2296  }
2297  else if (tmp_byte == sensor_msgs::PointField::INT16 ||
2298  tmp_byte == sensor_msgs::PointField::UINT16)
2299  {
2300  PC2_message_y_bytes = 2;
2301  }
2302  else if (tmp_byte == sensor_msgs::PointField::INT32 ||
2303  tmp_byte == sensor_msgs::PointField::UINT32 ||
2304  tmp_byte == sensor_msgs::PointField::FLOAT32)
2305  {
2306  PC2_message_y_bytes = 4;
2307  }
2308  else if (tmp_byte == sensor_msgs::PointField::FLOAT64)
2309  {
2310  PC2_message_y_bytes = 8;
2311  }
2312  else
2313  {
2314  ROS_ERROR("Cannot determine number of bytes for Y coordinate");
2315  return -1;
2316  }
2317  }
2318  // Z
2319  else if (strcmp(bank_argument.PC2_message_z_coordinate_field_name.c_str(),
2320  msg->fields[i].name.c_str()) == 0)
2321  {
2322  // Read offset
2323  memcpy(&tmp_byte4[0], &msg->fields[i].offset, 4);
2324  if (must_reverse_bytes)
2325  {
2326  reverseBytes(tmp_byte4, 4);
2327  }
2328  memcpy(&PC2_message_z_offset, tmp_byte4, 4);
2329 
2330  // Read datatype
2331  tmp_byte = msg->fields[i].datatype;
2332  if (tmp_byte == sensor_msgs::PointField::INT8 ||
2333  tmp_byte == sensor_msgs::PointField::UINT8)
2334  {
2335  PC2_message_z_bytes = 1;
2336  }
2337  else if (tmp_byte == sensor_msgs::PointField::INT16 ||
2338  tmp_byte == sensor_msgs::PointField::UINT16)
2339  {
2340  PC2_message_z_bytes = 2;
2341  }
2342  else if (tmp_byte == sensor_msgs::PointField::INT32 ||
2343  tmp_byte == sensor_msgs::PointField::UINT32 ||
2344  tmp_byte == sensor_msgs::PointField::FLOAT32)
2345  {
2346  PC2_message_z_bytes = 4;
2347  }
2348  else if (tmp_byte == sensor_msgs::PointField::FLOAT64)
2349  {
2350  PC2_message_z_bytes = 8;
2351  }
2352  else
2353  {
2354  ROS_ERROR("Cannot determine number of bytes for Z coordinate");
2355  return -1;
2356  }
2357  }
2358  }
2359 
2360  if (0 <= PC2_message_x_offset &&
2361  0 <= PC2_message_x_bytes &&
2362  0 <= PC2_message_y_offset &&
2363  0 <= PC2_message_y_bytes &&
2364  0 <= PC2_message_z_offset &&
2365  0 <= PC2_message_z_bytes)
2366  {
2367  return 0;
2368  }
2369  else
2370  {
2371  ROS_ERROR("Could not determine offsets and bytes");
2372  return -1;
2373  }
2374 }
2375 
2376 
2377 /* DATA POINT HANDLING */
2378 void Bank::readPoint(const byte_t * start_of_point,
2379  const bool must_reverse_bytes,
2380  double * x,
2381  double * y,
2382  double * z)
2383 {
2384  uint8_t x_array[PC2_message_x_bytes];
2385  uint8_t y_array[PC2_message_y_bytes];
2386  uint8_t z_array[PC2_message_z_bytes];
2387  memcpy(x_array, start_of_point + PC2_message_x_offset, PC2_message_x_bytes);
2388  memcpy(y_array, start_of_point + PC2_message_y_offset, PC2_message_y_bytes);
2389  memcpy(z_array, start_of_point + PC2_message_z_offset, PC2_message_z_bytes);
2390 
2391  // Does the msg have different byte order compared to the CPU?
2392  if (must_reverse_bytes)
2393  {
2394  reverseBytes(x_array, PC2_message_x_bytes);
2395  reverseBytes(y_array, PC2_message_y_bytes);
2396  reverseBytes(z_array, PC2_message_z_bytes);
2397  }
2398 
2399  // Read the coordinates for this point
2400  // X
2401  if (PC2_message_x_bytes == sizeof(float))
2402  {
2403  float xf;
2404  memcpy(&xf, x_array, PC2_message_x_bytes);
2405  *x = xf;
2406  }
2407  else if (PC2_message_x_bytes == sizeof(double))
2408  {
2409  memcpy(x, x_array, PC2_message_x_bytes);
2410  }
2411  else
2412  {
2413  ROS_ERROR("Cannot determine how to read X coordinate for this point!");
2414  return;
2415  }
2416  // Y
2417  if (PC2_message_y_bytes == sizeof(float))
2418  {
2419  float yf;
2420  memcpy(&yf, y_array, PC2_message_y_bytes);
2421  *y = yf;
2422  }
2423  else if (PC2_message_y_bytes == sizeof(double))
2424  {
2425  memcpy(y, y_array, PC2_message_y_bytes);
2426  }
2427  else
2428  {
2429  ROS_ERROR("Cannot determine how to read Y coordinate for this point!");
2430  return;
2431  }
2432  // Z
2433  if (PC2_message_z_bytes == sizeof(float))
2434  {
2435  float zf;
2436  memcpy(&zf, z_array, PC2_message_z_bytes);
2437  *z = zf;
2438  }
2439  else if (PC2_message_z_bytes == sizeof(double))
2440  {
2441  memcpy(z, z_array, PC2_message_z_bytes);
2442  }
2443  else
2444  {
2445  ROS_ERROR("Cannot determine how to read Z coordinate for this point!");
2446  return;
2447  }
2448 }
2449 
2450 
2451 /* BANK HANDLING */
2452 // Resets the range bank[i] for each i to a value that is larger than the largest allowed (threshold_distance_max)
2453 void Bank::resetPutPoints()
2454 {
2455  const double range = bank_argument.object_threshold_max_distance + 10.0;
2456  float * bank_put = bank_ranges_ema[bank_index_put];
2457  for (unsigned int i=0; i<bank_argument.points_per_scan; ++i)
2458  {
2459  bank_put[i] = range;
2460  }
2461 }
2462 
2463 
2464 // Assumes that threshold_distance_max < bank[i] (i.e. that values have been reset)
2465 // and that bank_view_angle is centered at the x-axis.
2466 // Reads all points from msg and puts them at bank[i] such that i corresponds to the angle at which
2467 // the point is found in the x,y plane of the sensor.
2468 // Tries to fill several i for one and the same point if needed based on the voxel leaf size.
2469 unsigned int Bank::putPoints(const sensor_msgs::PointCloud2::ConstPtr msg)
2470 {
2471  const bool must_reverse_bytes = (msg->is_bigendian != !machine_is_little_endian);
2472  float * bank_put = bank_ranges_ema[bank_index_put];
2473  const double bank_view_angle = bank_argument.angle_max - bank_argument.angle_min;
2474  const double bank_view_angle_half = bank_view_angle / 2;
2475  const double voxel_leaf_size_half = bank_argument.PC2_voxel_leaf_size / 2;
2476  const double inverted_bank_resolution = bank_argument.points_per_scan / bank_view_angle;
2477  const int bank_index_max = bank_argument.points_per_scan - 1;
2478  const unsigned int rows = msg->height;
2479  const unsigned int bytes_per_row = msg->row_step;
2480  const unsigned int bytes_per_point = msg->point_step;
2481 
2482  // Loop through rows
2483  unsigned int added_points_out = 0;
2484  for (unsigned int i=0; i<rows; i++)
2485  {
2486  // Loop through points in each row
2487  const unsigned int row_offset = i * bytes_per_row;
2488  for (unsigned int j=0; j<bytes_per_row; j+=bytes_per_point)
2489  {
2490  double x, y, z;
2491  const uint8_t * start_of_point = &msg->data[row_offset + j];
2492 
2493  readPoint(start_of_point,
2494  must_reverse_bytes,
2495  &x,
2496  &y,
2497  &z);
2498 
2499  // Is this point outside the considered volume?
2500  if (!bank_argument.sensor_frame_has_z_axis_forward)
2501  {
2502  // Assume Z-axis is pointing up and X-axis forward
2503  if (z < bank_argument.PC2_threshold_z_min ||
2504  bank_argument.PC2_threshold_z_max < z ||
2505  x < 0.02)
2506  {
2507  continue;
2508  }
2509  }
2510  else
2511  {
2512  // Assume Y-axis is pointing down and Z-axis forward
2513  if (-y < bank_argument.PC2_threshold_z_min ||
2514  bank_argument.PC2_threshold_z_max < -y ||
2515  z < 0.02)
2516  {
2517  continue;
2518  }
2519  }
2520 
2521  // Sanity check
2522  if (std::isnan(x) || std::isnan(y) || std::isnan(z))
2523  {
2524  ROS_DEBUG_STREAM("Skipping point (" << x << "," << y << "," << z << ")");
2525  continue;
2526  }
2527 
2528  // Another valid point
2529  added_points_out = added_points_out + 1;
2530 
2531  // Calculate index (indices) of point in bank
2532  const double range = sqrt(x*x + y*y + z*z); // TODO?
2533  double point_angle_min;
2534  double point_angle_max;
2535  if (!bank_argument.sensor_frame_has_z_axis_forward)
2536  {
2537  // Assume Z-axis is pointing up, 0.02 <= x
2538  point_angle_min = atan((y - voxel_leaf_size_half) / x);
2539  point_angle_max = atan((y + voxel_leaf_size_half) / x);
2540  }
2541  else
2542  {
2543  // Assume Y-axis is pointing down, 0.02 <= z
2544  point_angle_min = atan((-x - voxel_leaf_size_half) / z);
2545  point_angle_max = atan((-x + voxel_leaf_size_half) / z);
2546  }
2547 
2548 
2549  const int bank_index_point_min =
2550  (0 > (point_angle_min + bank_view_angle_half) * inverted_bank_resolution ?
2551  0: // MAX of 0 and next row
2552  (point_angle_min + bank_view_angle_half) * inverted_bank_resolution);
2553  const int bank_index_point_max =
2554  (bank_index_max < (point_angle_max + bank_view_angle_half) * inverted_bank_resolution ?
2555  bank_index_max : // MIN of bank_index_max and next row
2556  (point_angle_max + bank_view_angle_half) * inverted_bank_resolution);
2557 
2558  ROS_DEBUG_STREAM("The point (" << x << "," << y << "," << z << ") is added in the bank between indices " << \
2559  std::setw(4) << std::left << bank_index_point_min << " and " << bank_index_point_max << std::endl);
2560 
2561  // Fill all indices covered by this point
2562  // Check if there is already a range at the given index, only add if this point is closer
2563  for (int p=bank_index_point_min; p<=bank_index_point_max; ++p)
2564  {
2565  if (range < bank_put[p])
2566  {
2567  bank_put[p] = range;
2568  }
2569  }
2570  }
2571  }
2572 
2573  return added_points_out;
2574 }
2575 
2576 unsigned int Bank::putPoints(const sensor_msgs::PointCloud2 * msg)
2577 {
2578  const bool must_reverse_bytes = (msg->is_bigendian != !machine_is_little_endian);
2579  float * bank_put = bank_ranges_ema[bank_index_put];
2580  const double bank_view_angle = bank_argument.angle_max - bank_argument.angle_min;
2581  const double bank_view_angle_half = bank_view_angle / 2;
2582  const double voxel_leaf_size_half = bank_argument.PC2_voxel_leaf_size / 2;
2583  const double inverted_bank_resolution = bank_argument.points_per_scan / bank_view_angle;
2584  const int bank_index_max = bank_argument.points_per_scan - 1;
2585  const unsigned int rows = msg->height;
2586  const unsigned int bytes_per_row = msg->row_step;
2587  const unsigned int bytes_per_point = msg->point_step;
2588 
2589  // Loop through rows
2590  unsigned int added_points_out = 0;
2591  for (unsigned int i=0; i<rows; i++)
2592  {
2593  // Loop through points in each row
2594  const unsigned int row_offset = i * bytes_per_row;
2595  for (unsigned int j=0; j<bytes_per_row; j+=bytes_per_point)
2596  {
2597  double x, y, z;
2598  const uint8_t * start_of_point = &msg->data[row_offset + j];
2599 
2600  readPoint(start_of_point,
2601  must_reverse_bytes,
2602  &x,
2603  &y,
2604  &z);
2605 
2606  // Is this point outside the considered volume?
2607  if (!bank_argument.sensor_frame_has_z_axis_forward)
2608  {
2609  // Assume Z-axis is pointing up and X-axis forward
2610  if (z < bank_argument.PC2_threshold_z_min ||
2611  bank_argument.PC2_threshold_z_max < z ||
2612  x < 0.02)
2613  {
2614  continue;
2615  }
2616  }
2617  else
2618  {
2619  // Assume Y-axis is pointing down and Z-axis forward
2620  if (-y < bank_argument.PC2_threshold_z_min ||
2621  bank_argument.PC2_threshold_z_max < -y ||
2622  z < 0.02)
2623  {
2624  continue;
2625  }
2626  }
2627 
2628  // Sanity check
2629  if (std::isnan(x) || std::isnan(y) || std::isnan(z))
2630  {
2631  ROS_DEBUG_STREAM("Skipping point (" << x << "," << y << "," << z << ")");
2632  continue;
2633  }
2634 
2635  // Another valid point
2636  added_points_out = added_points_out + 1;
2637 
2638  // Calculate index (indices) of point in bank
2639  const double range = sqrt(x*x + y*y + z*z); // TODO?
2640  double point_angle_min;
2641  double point_angle_max;
2642  if (!bank_argument.sensor_frame_has_z_axis_forward)
2643  {
2644  // Assume Z-axis is pointing up, 0.02 <= x
2645  point_angle_min = atan((y - voxel_leaf_size_half) / x);
2646  point_angle_max = atan((y + voxel_leaf_size_half) / x);
2647  }
2648  else
2649  {
2650  // Assume Y-axis is pointing down, 0.02 <= z
2651  point_angle_min = atan((-x - voxel_leaf_size_half) / z);
2652  point_angle_max = atan((-x + voxel_leaf_size_half) / z);
2653  }
2654 
2655 
2656  const int bank_index_point_min =
2657  (0 > (point_angle_min + bank_view_angle_half) * inverted_bank_resolution ?
2658  0: // MAX of 0 and next row
2659  (point_angle_min + bank_view_angle_half) * inverted_bank_resolution);
2660  const int bank_index_point_max =
2661  (bank_index_max < (point_angle_max + bank_view_angle_half) * inverted_bank_resolution ?
2662  bank_index_max : // MIN of bank_index_max and next row
2663  (point_angle_max + bank_view_angle_half) * inverted_bank_resolution);
2664 
2665  ROS_DEBUG_STREAM("The point (" << x << "," << y << "," << z << ") is added in the bank between indices " << \
2666  std::setw(4) << std::left << bank_index_point_min << " and " << bank_index_point_max << std::endl);
2667 
2668  // Fill all indices covered by this point
2669  // Check if there is already a range at the given index, only add if this point is closer
2670  for (int p=bank_index_point_min; p<=bank_index_point_max; ++p)
2671  {
2672  if (range < bank_put[p])
2673  {
2674  bank_put[p] = range;
2675  }
2676  }
2677  }
2678  }
2679 
2680  return added_points_out;
2681 }
2682 
2683 
2684 // Assumes that bank[bank_index_put] is filled with ranges from a new message
2685 // (i.e. that indices have not yet been updated).
2686 // These values are EMA-adapted based on the previous set of EMA-adapted values at bank[index_previous]
2687 void Bank::emaPutMessage()
2688 {
2689  const double alpha = bank_argument.ema_alpha;
2690 
2691  // No need to do this if alpha < 1.0!
2692  if (alpha < 1.0)
2693  {
2694  const double alpha_prev = 1.0 - alpha;
2695  float * bank_put = bank_ranges_ema[bank_index_put];
2696  float * bank_prev = bank_ranges_ema[bank_index_newest];
2697 
2698  for (unsigned int i=0; i<bank_argument.points_per_scan; ++i)
2699  {
2700  bank_put[i] = alpha * bank_put[i] +
2701  alpha_prev * bank_prev[i];
2702  }
2703  }
2704 }
2705 
2706 
2707 // Debug/print bank column/msg
2708 std::string Bank::getStringPutPoints()
2709 {
2710  float * bank_put = bank_ranges_ema[bank_index_put];
2711  std::ostringstream stream;
2712  stream << "Bank points (at put index):";
2713  for (unsigned int i=0; i<bank_argument.points_per_scan; ++i)
2714  {
2715  stream << " " << bank_put[i];
2716  }
2717  stream << std::endl;
2718  std::string string = stream.str();
2719  return string;
2720 }
2721 
2722 
2723 // Init indices
2724 inline void Bank::initIndex()
2725 {
2726  bank_index_put = 1;
2727  bank_index_newest = 0;
2728 }
2729 
2730 
2731 // Advance indices
2732 inline void Bank::advanceIndex()
2733 {
2734  bank_index_put = (bank_index_put + 1) % bank_argument.nr_scans_in_bank; // points to the oldest message
2735  bank_index_newest = (bank_index_newest + 1) % bank_argument.nr_scans_in_bank; // points to the newest/this message
2736 }
2737 
2738 
2739 // Init bank based on LaserScan msg
2740 long Bank::init(BankArgument bank_argument, const sensor_msgs::LaserScan * msg)
2741 {
2742  if (!bank_is_initialized)
2743  {
2744  if (!bank_argument.sensor_frame_has_z_axis_forward && strstr(msg->header.frame_id.c_str(), "_optical") != NULL)
2745  {
2746  ROS_WARN_STREAM("The sensor frame (" << msg->header.frame_id.c_str() << ") seems to be a camera/optical frame. "
2747  "Perhaps sensor_frame_has_z_axis_forward should be set?");
2748  }
2749  else if (bank_argument.sensor_frame_has_z_axis_forward)
2750  {
2751  ROS_WARN_STREAM("Please note that setting sensor_frame_has_z_axis_forward will cause the ema and "
2752  "objects_closest_points messages to be shown incorrectly.");
2753  }
2754  }
2755 
2756  bank_argument.sensor_frame = msg->header.frame_id;
2757  bank_argument.points_per_scan = msg->ranges.size();
2758  bank_argument.angle_min = msg->angle_min;
2759  bank_argument.angle_max = msg->angle_max;
2760  bank_argument.angle_increment = msg->angle_increment;
2761  bank_argument.time_increment = msg->time_increment;
2762  bank_argument.scan_time = msg->scan_time;
2763  bank_argument.range_min = msg->range_min;
2764  bank_argument.range_max = msg->range_max;
2765  resolution = bank_argument.angle_increment;
2766 
2767  initBank(bank_argument);
2768 
2769  ROS_DEBUG_STREAM("Bank arguments:" << std::endl << bank_argument);
2770 
2771  return addFirstMessage(msg);
2772 }
2773 
2774 
2775 // Add FIRST LaserScan message to bank - no ema
2776 long Bank::addFirstMessage(const sensor_msgs::LaserScan * msg)
2777 {
2778  bank_stamp[0] = msg->header.stamp.toSec();
2779 
2780 // if (bank_argument.ema_alpha != 1.0)
2781 // {
2782  float * bank_put = bank_ranges_ema[0];
2783  for (unsigned int i=0; i<bank_argument.points_per_scan; ++i)
2784  {
2785  if (msg->ranges[i] == std::numeric_limits<float>::infinity())
2786  {
2787  bank_put[i] = bank_argument.range_max + 0.01;
2788  }
2789  else if (msg->ranges[i] == -std::numeric_limits<float>::infinity())
2790  {
2791  bank_put[i] = bank_argument.range_min - 0.01;
2792  }
2793 // else if (msg->ranges[i] != msg->ranges[i])
2794  else if (std::isnan(msg->ranges[i]))
2795  {
2796  // The range is NaN
2797  bank_put[i] = bank_argument.range_max + 0.01;
2798  }
2799  else
2800  {
2801  // Turn infinity into large value
2802  bank_put[i] = msg->ranges[i];
2803  }
2804  }
2805 // }
2806 // else
2807 // {
2808 // memcpy(&bank_ranges_ema[0][0], msg->ranges.data(), bank_ranges_bytes);
2809 // }
2810 
2811  initIndex(); // set put to 1 and newest to 0
2812  bank_is_filled = false;
2813 
2814  ROS_DEBUG_STREAM("First message (LaserScan):" << std::endl << *msg);
2815 
2816  return 0;
2817 }
2818 
2819 // Add LaserScan message and perform EMA
2820 long Bank::addMessage(const sensor_msgs::LaserScan * msg)
2821 {
2822  // Save timestamp
2823  bank_stamp[bank_index_put] = msg->header.stamp.toSec();
2824 
2825  // Save EMA of ranges, if applicable, otherwise, perform memcpy
2826  const double alpha = bank_argument.ema_alpha;
2827 // if (alpha != 1.0)
2828 // {
2829  const double alpha_prev = 1.0 - bank_argument.ema_alpha;
2830  float * bank_put = bank_ranges_ema[bank_index_put];
2831  float * bank_newest = bank_ranges_ema[bank_index_newest];
2832  for (unsigned int i=0; i<bank_argument.points_per_scan; ++i)
2833  {
2834  if (msg->ranges[i] == std::numeric_limits<float>::infinity())
2835  {
2836  bank_put[i] = bank_argument.range_max + 0.01;
2837  }
2838  else if (msg->ranges[i] == -std::numeric_limits<float>::infinity())
2839  {
2840  bank_put[i] = bank_argument.range_min - 0.01;
2841  }
2842  else if (msg->ranges[i] != msg->ranges[i])
2843  {
2844  // The range is NaN
2845  bank_put[i] = bank_argument.range_max + 0.01;
2846  }
2847  else
2848  {
2849  // Turn infinity into large value
2850  bank_put[i] = alpha * msg->ranges[i] + alpha_prev * bank_newest[i];
2851  }
2852  }
2853 // }
2854 // else
2855 // {
2856 // memcpy(&bank_ranges_ema[bank_index_put][0], msg->ranges.data(), bank_ranges_bytes);
2857 // }
2858 
2859  advanceIndex();
2860  if (!bank_is_filled && bank_index_put < bank_index_newest)
2861  {
2862  bank_is_filled = true;
2863  }
2864 
2865  return 0;
2866 }
2867 
2868 
2869 // Init bank based on PointCloud2 msg
2870 // This function only works on local copies of the input parameters, so it can safely be called again using the same
2871 // parameters if offsets and bytes could not be read from the message
2872 long Bank::init(BankArgument bank_argument, const sensor_msgs::PointCloud2 * msg,
2873  const bool discard_message_if_no_points_added)
2874 {
2875  ROS_DEBUG("Init bank (%s)", msg->header.frame_id.c_str());
2876  bank_argument.sensor_frame = msg->header.frame_id;
2877  if (!bank_is_initialized)
2878  {
2879  if (!bank_argument.sensor_frame_has_z_axis_forward && strstr(msg->header.frame_id.c_str(), "_optical") != NULL)
2880  {
2881  ROS_WARN_STREAM("The sensor frame (" << msg->header.frame_id.c_str() << ") seems to be a camera/optical frame. "
2882  "Perhaps sensor_frame_has_z_axis_forward should be set?");
2883  }
2884  else if (bank_argument.sensor_frame_has_z_axis_forward)
2885  {
2886  ROS_WARN_STREAM("Please note that setting sensor_frame_has_z_axis_forward will cause the ema and "
2887  "objects_closest_points messages to be shown incorrectly.");
2888  }
2889  }
2890 
2891  if (bank_argument.points_per_scan <= 1)
2892  {
2893  bank_argument.angle_increment = 0.0000001;
2894  }
2895  else
2896  {
2897  bank_argument.angle_increment = (bank_argument.angle_max - bank_argument.angle_min) /
2898  (bank_argument.points_per_scan - 1);
2899  }
2900  bank_argument.time_increment = 0;
2901  bank_argument.scan_time = 0;
2902  bank_argument.range_min = 0.01;
2903  bank_argument.range_max = bank_argument.object_threshold_max_distance;
2904 
2905  resolution = bank_argument.angle_increment;
2906 
2907  if (getOffsetsAndBytes(bank_argument, msg))
2908  {
2909  ROS_ERROR("Cannot read offsets and bytes from message!");
2910  return -1;
2911  }
2912 
2913  bank_argument.check_PC2();
2914  initBank(bank_argument); // Will return immediately in case it has been called before
2915  return addFirstMessage(msg, discard_message_if_no_points_added);
2916 }
2917 
2918 
2919 // Add FIRST PointCloud2 message to bank - no EMA
2920 long Bank::addFirstMessage(const sensor_msgs::PointCloud2 * msg,
2921  const bool discard_message_if_no_points_added)
2922 {
2923  // Save timestamp
2924  bank_stamp[0] = msg->header.stamp.toSec();
2925 
2926  // Set put index so that we can use the helper functions
2927  bank_index_put = 0;
2928 
2929  // Reset ranges so that new values can be added to bank position
2930  resetPutPoints();
2931 
2932  // Add points if possible
2933  const unsigned int added_points = putPoints(msg);
2934 
2935  // If no points were added, then redo the process for this message
2936  if (added_points == 0)
2937  {
2938  ROS_WARN("Could not add any points from the PointCloud2 message");
2939  if (discard_message_if_no_points_added)
2940  {
2941  return -1;
2942  }
2943  }
2944 
2945  ROS_DEBUG_STREAM("First message (PointCloud2):" << std::endl << *msg);
2946 
2947  ROS_DEBUG("%s", getStringPutPoints().c_str());
2948 
2949  // Set put to 1 and newest to 0
2950  initIndex();
2951  bank_is_filled = false;
2952 
2953  return 0;
2954 }
2955 
2956 // Add PointCloud2 message and perform EMA
2957 long Bank::addMessage(const sensor_msgs::PointCloud2 * msg,
2958  const bool discard_message_if_no_points_added)
2959 {
2960  // Copy timestamp
2961  bank_stamp[bank_index_put] = msg->header.stamp.toSec();
2962 
2963  // Reset ranges so that new values can be added to bank position
2964  resetPutPoints();
2965 
2966  // Read the message and put the points in the bank
2967  const unsigned int added_points = putPoints(msg);
2968 
2969  // If no points were added, then redo the process for this message
2970  if (added_points == 0)
2971  {
2972  ROS_WARN("Could not add any points from the PointCloud2 message");
2973  if (discard_message_if_no_points_added)
2974  {
2975  return -1;
2976  }
2977  }
2978 
2979  ROS_DEBUG("%s", getStringPutPoints().c_str());
2980 
2981  // EMA-adapt the new ranges
2982  emaPutMessage();
2983 
2984  // Update indices
2985  advanceIndex();
2986  if (bank_index_put < bank_index_newest)
2987  {
2988  bank_is_filled = true;
2989  }
2990 
2991  return 0;
2992 }
2993 
2994 } // namespace find_moving_objects
find_moving_objects::BankArgument::object_threshold_max_distance
double object_threshold_max_distance
Definition: bank.h:112
find_moving_objects::BankArgument::publish_objects_velocity_arrows
bool publish_objects_velocity_arrows
Definition: bank.h:153
find_moving_objects::BankArgument::object_threshold_min_speed
double object_threshold_min_speed
Definition: bank.h:116
find_moving_objects::BankArgument::PC2_message_y_coordinate_field_name
std::string PC2_message_y_coordinate_field_name
Definition: bank.h:256
find_moving_objects::BankArgument::velocity_arrows_use_full_gray_scale
bool velocity_arrows_use_full_gray_scale
Definition: bank.h:168
transform_listener.h
find_moving_objects::BankArgument::nr_scans_in_bank
int nr_scans_in_bank
Definition: bank.h:67
find_moving_objects::BankArgument::publish_objects_closest_point_markers
bool publish_objects_closest_point_markers
Definition: bank.h:148
find_moving_objects::Bank::emaPutMessage
void emaPutMessage()
Definition: bank.cpp:2687
ros::NodeHandle
find_moving_objects::BankArgument::topic_objects_closest_point_markers
std::string topic_objects_closest_point_markers
Definition: bank.h:209
find_moving_objects::Bank::initIndex
void initIndex()
Definition: bank.cpp:2724
find_moving_objects::BankArgument::delta_position_line_ns
std::string delta_position_line_ns
Definition: bank.h:193
find_moving_objects::BankArgument::object_threshold_bank_tracking_max_delta_distance
double object_threshold_bank_tracking_max_delta_distance
Definition: bank.h:129
find_moving_objects::Bank::init
virtual long init(BankArgument bank_argument, const sensor_msgs::LaserScan *msg)
Definition: bank.cpp:2740
find_moving_objects::Bank::addMessage
virtual long addMessage(const sensor_msgs::LaserScan *msg)
Definition: bank.cpp:2820
find_moving_objects::BankArgument::publish_objects_delta_position_lines
bool publish_objects_delta_position_lines
Definition: bank.h:158
find_moving_objects::Bank::getStringPutPoints
std::string getStringPutPoints()
Definition: bank.cpp:2708
find_moving_objects::Bank::initBank
void initBank(BankArgument bank_argument)
Definition: bank.cpp:408
find_moving_objects::BankArgument::PC2_voxel_leaf_size
double PC2_voxel_leaf_size
Definition: bank.h:264
find_moving_objects::BankArgument::publish_ema
bool publish_ema
Definition: bank.h:143
find_moving_objects::Bank::getOldIndices
void getOldIndices(const float range_min, const float range_max, const unsigned int object_width_in_points, const int current_level, const unsigned int levels_searched, const unsigned int index_mean, const unsigned int consecutive_failures_to_find_object, const unsigned int threshold_consecutive_failures_to_find_object, int *index_min_old, int *index_mean_old, int *index_max_old, float *range_sum_old, float *range_at_min_index_old, float *range_at_max_index_old)
Definition: bank.cpp:600
find_moving_objects::Bank::Bank
Bank(tf2_ros::Buffer *buffer)
Definition: bank.cpp:248
find_moving_objects::Bank::findAndReportMovingObjects
void findAndReportMovingObjects()
Definition: bank.cpp:921
find_moving_objects::BankArgument::sensor_frame_has_z_axis_forward
bool sensor_frame_has_z_axis_forward
Definition: bank.h:95
find_moving_objects::BankArgument::BankArgument
BankArgument()
Definition: bank.cpp:114
find_moving_objects::BankArgument::range_max
float range_max
Definition: bank.h:325
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
find_moving_objects::BankArgument::velocity_arrows_use_sensor_frame
bool velocity_arrows_use_sensor_frame
Definition: bank.h:174
find_moving_objects::Bank::addFirstMessage
virtual long addFirstMessage(const sensor_msgs::LaserScan *)
Definition: bank.cpp:2776
tf2_ros::Buffer
find_moving_objects::Bank::resetPutPoints
void resetPutPoints()
Definition: bank.cpp:2453
ROS_WARN
#define ROS_WARN(...)
find_moving_objects::BankArgument::width_line_ns
std::string width_line_ns
Definition: bank.h:197
find_moving_objects::BankArgument::angle_max
double angle_max
Definition: bank.h:86
find_moving_objects::BankArgument::operator<<
friend std::ostream & operator<<(std::ostream &os, const BankArgument &ba)
Definition: bank.cpp:167
find_moving_objects::Bank::byte_t
uint8_t byte_t
Definition: bank.h:433
find_moving_objects::BankArgument::object_threshold_edge_max_delta_range
double object_threshold_edge_max_delta_range
Definition: bank.h:104
find_moving_objects::BankArgument::node_name_suffix
std::string node_name_suffix
Definition: bank.h:281
find_moving_objects::BankArgument::topic_objects_width_lines
std::string topic_objects_width_lines
Definition: bank.h:221
find_moving_objects::BankArgument::points_per_scan
int points_per_scan
Definition: bank.h:71
find_moving_objects::Bank::advanceIndex
void advanceIndex()
Definition: bank.cpp:2732
find_moving_objects::BankArgument::velocity_arrow_ns
std::string velocity_arrow_ns
Definition: bank.h:189
find_moving_objects::BankArgument
Definition: bank.h:59
find_moving_objects::BankArgument::base_frame
std::string base_frame
Definition: bank.h:237
atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
find_moving_objects
Definition: bank.h:49
find_moving_objects::BankArgument::PC2_message_x_coordinate_field_name
std::string PC2_message_x_coordinate_field_name
Definition: bank.h:252
find_moving_objects::BankArgument::angle_increment
float angle_increment
Definition: bank.h:303
find_moving_objects::BankArgument::scan_time
float scan_time
Definition: bank.h:315
ROS_ASSERT_MSG
#define ROS_ASSERT_MSG(cond,...)
find_moving_objects::BankArgument::PC2_threshold_z_max
double PC2_threshold_z_max
Definition: bank.h:274
find_moving_objects::Bank::~Bank
~Bank()
Definition: bank.cpp:268
find_moving_objects::Bank::getOffsetsAndBytes
int getOffsetsAndBytes(BankArgument bank_argument, const sensor_msgs::PointCloud2::ConstPtr msg)
Definition: bank.cpp:2070
tf2_geometry_msgs.h
find_moving_objects::BankArgument::topic_objects_velocity_arrows
std::string topic_objects_velocity_arrows
Definition: bank.h:213
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
find_moving_objects::BankArgument::PC2_threshold_z_min
double PC2_threshold_z_min
Definition: bank.h:268
find_moving_objects::BankArgument::topic_objects_delta_position_lines
std::string topic_objects_delta_position_lines
Definition: bank.h:217
find_moving_objects::BankArgument::base_confidence
double base_confidence
Definition: bank.h:134
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
find_moving_objects::BankArgument::velocity_arrows_use_fixed_frame
bool velocity_arrows_use_fixed_frame
Definition: bank.h:184
find_moving_objects::BankArgument::PC2_message_z_coordinate_field_name
std::string PC2_message_z_coordinate_field_name
Definition: bank.h:260
find_moving_objects::BankArgument::time_increment
float time_increment
Definition: bank.h:309
message_filter.h
find_moving_objects::BankArgument::ema_alpha
double ema_alpha
Definition: bank.h:63
find_moving_objects::BankArgument::topic_ema
std::string topic_ema
Definition: bank.h:205
find_moving_objects::BankArgument::sensor_frame
std::string sensor_frame
Definition: bank.h:300
find_moving_objects::TWO_PI
const float TWO_PI
Definition: bank.cpp:109
abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
find_moving_objects::BankArgument::publish_objects
bool publish_objects
Definition: bank.h:138
find_moving_objects::BankArgument::angle_min
double angle_min
Definition: bank.h:77
find_moving_objects::BankArgument::range_min
float range_min
Definition: bank.h:320
find_moving_objects::BankArgument::object_threshold_max_delta_width_in_points
int object_threshold_max_delta_width_in_points
Definition: bank.h:120
ros::Time::now
static Time now()
find_moving_objects::BankArgument::check
void check()
Definition: bank.cpp:282
tf2_ros::BufferInterface::transform
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
find_moving_objects::BankArgument::check_PC2
void check_PC2()
Definition: bank.cpp:386
find_moving_objects::BankArgument::object_threshold_min_nr_points
int object_threshold_min_nr_points
Definition: bank.h:108
find_moving_objects::BankArgument::publish_buffer_size
int publish_buffer_size
Definition: bank.h:225
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
find_moving_objects::BankArgument::sensor_is_360_degrees
bool sensor_is_360_degrees
Definition: bank.h:330
find_moving_objects::BankArgument::publish_objects_width_lines
bool publish_objects_width_lines
Definition: bank.h:163
find_moving_objects::tf_buffer
tf2_ros::Buffer * tf_buffer
Definition: LaserScanInterpreter.h:56
ROS_ERROR
#define ROS_ERROR(...)
find_moving_objects::BankArgument::velocity_arrows_use_base_frame
bool velocity_arrows_use_base_frame
Definition: bank.h:179
find_moving_objects::BankArgument::topic_objects
std::string topic_objects
Definition: bank.h:201
find_moving_objects::Bank::readPoint
void readPoint(const byte_t *start_of_point, const bool must_reverse_bytes, double *x, double *y, double *z)
Definition: bank.cpp:2378
find_moving_objects::Bank::reverseBytes
void reverseBytes(byte_t *bytes, unsigned int nr_bytes)
Definition: bank.cpp:2055
find_moving_objects::BankArgument::fixed_frame
std::string fixed_frame
Definition: bank.h:233
find_moving_objects::Bank::putPoints
unsigned int putPoints(const sensor_msgs::PointCloud2::ConstPtr msg)
Definition: bank.cpp:2469
find_moving_objects::BankArgument::map_frame
std::string map_frame
Definition: bank.h:229
ROS_DEBUG
#define ROS_DEBUG(...)
find_moving_objects::BankArgument::object_threshold_min_confidence
double object_threshold_min_confidence
Definition: bank.h:125

find_moving_objects
Author(s): Andreas Gustavsson
autogenerated on Mon Jun 10 2019 13:13:19