Public Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
find_moving_objects::Bank Class Reference

#include <bank.h>

Public Member Functions

virtual long addMessage (const sensor_msgs::LaserScan *msg)
 
virtual long addMessage (const sensor_msgs::PointCloud2 *msg, const bool discard_message_if_no_points_added=true)
 
 Bank (tf2_ros::Buffer *buffer)
 
virtual double calculateConfidence (const find_moving_objects::MovingObject &mo, const find_moving_objects::BankArgument &ba, const double delta_time, const double mo_old_width)
 
void findAndReportMovingObjects ()
 
virtual long init (BankArgument bank_argument, const sensor_msgs::LaserScan *msg)
 
virtual long init (BankArgument bank_argument, const sensor_msgs::PointCloud2 *msg, const bool discard_message_if_no_points_added=true)
 
 ~Bank ()
 

Private Types

typedef uint8_t byte_t
 

Private Member Functions

virtual long addFirstMessage (const sensor_msgs::LaserScan *)
 
virtual long addFirstMessage (const sensor_msgs::PointCloud2 *, const bool discard_message_if_no_points_added)
 
void advanceIndex ()
 
void emaPutMessage ()
 
int getOffsetsAndBytes (BankArgument bank_argument, const sensor_msgs::PointCloud2::ConstPtr msg)
 
int getOffsetsAndBytes (BankArgument bank_argument, const sensor_msgs::PointCloud2 *msg)
 
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)
 
std::string getStringPutPoints ()
 
void initBank (BankArgument bank_argument)
 
void initIndex ()
 
unsigned int putPoints (const sensor_msgs::PointCloud2::ConstPtr msg)
 
unsigned int putPoints (const sensor_msgs::PointCloud2 *msg)
 
void readPoint (const byte_t *start_of_point, const bool must_reverse_bytes, double *x, double *y, double *z)
 
void resetPutPoints ()
 
void reverseBytes (byte_t *bytes, unsigned int nr_bytes)
 

Private Attributes

BankArgument bank_argument
 
int bank_index_newest
 
int bank_index_put
 
bool bank_is_filled
 
bool bank_is_initialized
 
unsigned int bank_ranges_bytes
 
float ** bank_ranges_ema
 
double * bank_stamp
 
bool machine_is_little_endian
 
unsigned int moa_seq
 
sensor_msgs::LaserScan msg_ema
 
sensor_msgs::LaserScan msg_objects_closest_point_markers
 
visualization_msgs::Marker msg_objects_delta_position_line
 
visualization_msgs::MarkerArray msg_objects_delta_position_lines
 
visualization_msgs::Marker msg_objects_velocity_arrow
 
visualization_msgs::MarkerArray msg_objects_velocity_arrows
 
visualization_msgs::Marker msg_objects_width_line
 
visualization_msgs::MarkerArray msg_objects_width_lines
 
ros::NodeHandlenode
 
int32_t PC2_message_x_bytes
 
int32_t PC2_message_x_offset
 
int32_t PC2_message_y_bytes
 
int32_t PC2_message_y_offset
 
int32_t PC2_message_z_bytes
 
int32_t PC2_message_z_offset
 
ros::Publisher pub_ema
 
ros::Publisher pub_objects
 
ros::Publisher pub_objects_closest_point_markers
 
ros::Publisher pub_objects_delta_position_lines
 
ros::Publisher pub_objects_velocity_arrows
 
ros::Publisher pub_objects_width_lines
 
double resolution
 
tf2_ros::Buffertf_buffer
 

Detailed Description

The bank contains a number of scan messages and is able to find the position and velocity of moving objects based on these scans.

Note that only ONE single source is supposed to feed the bank with messages and that the "static" parameters (such as the number of scan points and the view angle (cf. FoV) of the sensor etc) must be equal between messages. If they are not, then the behavior of the bank is undefined and the bank might even cause a segmentation fault.

Also note that you might need to sleep for a couple of seconds after creating the Bank object so that the started tf::tfListener can collect transformation data.

Definition at line 356 of file bank.h.

Member Typedef Documentation

typedef uint8_t find_moving_objects::Bank::byte_t
private

Definition at line 433 of file bank.h.

Constructor & Destructor Documentation

find_moving_objects::Bank::Bank ( tf2_ros::Buffer buffer)

Creates an instance of Bank and starts a tf::TransformListener. Creates an instance of Bank and sets its transform listener and transform buffer to the given ones.

Definition at line 248 of file bank.cpp.

find_moving_objects::Bank::~Bank ( )

De-allocates reserved memory for the bank.

Definition at line 268 of file bank.cpp.

Member Function Documentation

long find_moving_objects::Bank::addFirstMessage ( const sensor_msgs::LaserScan *  msg)
privatevirtual

Definition at line 2776 of file bank.cpp.

long find_moving_objects::Bank::addFirstMessage ( const sensor_msgs::PointCloud2 *  msg,
const bool  discard_message_if_no_points_added 
)
privatevirtual

Definition at line 2920 of file bank.cpp.

long find_moving_objects::Bank::addMessage ( const sensor_msgs::LaserScan *  msg)
virtual

Add a sensor_msgs::LaserScan message to the bank (replace the oldest scan message).

Parameters
msgPointer to the received message to be added to the bank.
Returns
0. Add a sensor_msgs::LaserScan message to the bank (replace the oldest scan message).
Parameters
msgPointer to the received message to be added to the bank.
Returns
0.

Definition at line 2820 of file bank.cpp.

long find_moving_objects::Bank::addMessage ( const sensor_msgs::PointCloud2 *  msg,
const bool  discard_message_if_no_points_added = true 
)
virtual

Add a sensor_msgs::PointCloud2 message to the bank (replace the oldest scan message).

Parameters
msgPointer to the received message to be added to the bank.
Returns
0 on success, -1 if adding this message failed. Add a sensor_msgs::PointCloud2 message to the bank (replace the oldest scan message).
Parameters
msgPointer to the received message to be added to the bank.
discard_message_if_no_points_addedDiscard msg if none of the points in its cloud could be transformed into a point within the field of view specified in bank_argument.
Returns
0 on success, -1 if adding this message failed.

Definition at line 2957 of file bank.cpp.

void find_moving_objects::Bank::advanceIndex ( )
inlineprivate

Definition at line 2732 of file bank.cpp.

double find_moving_objects::Bank::calculateConfidence ( const find_moving_objects::MovingObject &  mo,
const find_moving_objects::BankArgument ba,
const double  delta_time,
const double  mo_old_width 
)
virtual

Confidence calculation.

This function must be implemented by the application using the bank.

Parameters
moThe object for which to calculate confidence.
baAn instance of BankArgument with specified details.
delta_timeThe difference in time between the oldest and newest scans in the bank.
mo_old_widthThe old width of the object.
Returns
The user-defined confidence value.

Definition at line 87 of file laserscan_interpreter.cpp.

void find_moving_objects::Bank::emaPutMessage ( )
private

Definition at line 2687 of file bank.cpp.

void find_moving_objects::Bank::findAndReportMovingObjects ( )

Find moving objects based on the contents of the bank, and possibly report them.

Definition at line 921 of file bank.cpp.

int find_moving_objects::Bank::getOffsetsAndBytes ( BankArgument  bank_argument,
const sensor_msgs::PointCloud2::ConstPtr  msg 
)
private

Definition at line 2070 of file bank.cpp.

int find_moving_objects::Bank::getOffsetsAndBytes ( BankArgument  bank_argument,
const sensor_msgs::PointCloud2 *  msg 
)
private

Definition at line 2223 of file bank.cpp.

void find_moving_objects::Bank::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 
)
private

Definition at line 600 of file bank.cpp.

std::string find_moving_objects::Bank::getStringPutPoints ( )
private

Definition at line 2708 of file bank.cpp.

long find_moving_objects::Bank::init ( BankArgument  bank_argument,
const sensor_msgs::LaserScan *  msg 
)
virtual

Initiate bank with received data, if possible.

This function should be called repeatedly until it succeeds (i.e. returns 0), after this, it should not be called again (doing so would compromise the stored data)!

Parameters
bank_argumentAn instance of BankArgument, specifying the behavior of the bank.
msgPointer to the first received message to be added to the bank.
Returns
0 on success, -1 if this function must be called again. Initiate bank with received data, if possible.

This function should be called repeatedly until it succeeds (i.e. returns 0), after this, it should not be called again (doing so would compromise the stored data)!

Parameters
bank_argumentAn instance of BankArgument, specifying the behavior of the bank.
msgPointer to the first received message to be added to the bank.
Returns
0 on success, -1 if this function must be called again.

Definition at line 2740 of file bank.cpp.

long find_moving_objects::Bank::init ( BankArgument  bank_argument,
const sensor_msgs::PointCloud2 *  msg,
const bool  discard_message_if_no_points_added = true 
)
virtual

Initiate bank with received data, if possible.

This function should be called repeatedly until it succeeds (i.e. returns 0), after this, it should not be called again (doing so would compromise the stored data)!

Parameters
bank_argumentAn instance of BankArgument, specifying the behavior of the bank.
msgPointer to the first received message to be added to the bank.
Returns
0 on success, -1 if this function must be called again. Initiate bank with received data, if possible.

This function should be called repeatedly until it succeeds (i.e. returns 0), after this, it should not be called again (doing so would compromise the stored data)!

Parameters
bank_argumentAn instance of BankArgument, specifying the behavior of the bank.
msgPointer to the first received message to be added to the bank.
discard_message_if_no_points_addedDiscard msg if none of the points in its cloud could be transformed into a point within the field of view specified in bank_argument.
Returns
0 on success, -1 if this function must be called again.

Definition at line 2872 of file bank.cpp.

void find_moving_objects::Bank::initBank ( BankArgument  bank_argument)
private

Definition at line 408 of file bank.cpp.

void find_moving_objects::Bank::initIndex ( )
inlineprivate

Definition at line 2724 of file bank.cpp.

unsigned int find_moving_objects::Bank::putPoints ( const sensor_msgs::PointCloud2::ConstPtr  msg)
private

Definition at line 2469 of file bank.cpp.

unsigned int find_moving_objects::Bank::putPoints ( const sensor_msgs::PointCloud2 *  msg)
private

Definition at line 2576 of file bank.cpp.

void find_moving_objects::Bank::readPoint ( const byte_t start_of_point,
const bool  must_reverse_bytes,
double *  x,
double *  y,
double *  z 
)
private

Definition at line 2378 of file bank.cpp.

void find_moving_objects::Bank::resetPutPoints ( )
private

Definition at line 2453 of file bank.cpp.

void find_moving_objects::Bank::reverseBytes ( byte_t bytes,
unsigned int  nr_bytes 
)
private

Definition at line 2055 of file bank.cpp.

Member Data Documentation

BankArgument find_moving_objects::Bank::bank_argument
private

Definition at line 360 of file bank.h.

int find_moving_objects::Bank::bank_index_newest
private

Definition at line 371 of file bank.h.

int find_moving_objects::Bank::bank_index_put
private

Definition at line 372 of file bank.h.

bool find_moving_objects::Bank::bank_is_filled
private

Definition at line 367 of file bank.h.

bool find_moving_objects::Bank::bank_is_initialized
private

Definition at line 366 of file bank.h.

unsigned int find_moving_objects::Bank::bank_ranges_bytes
private

Definition at line 365 of file bank.h.

float** find_moving_objects::Bank::bank_ranges_ema
private

Definition at line 363 of file bank.h.

double* find_moving_objects::Bank::bank_stamp
private

Definition at line 364 of file bank.h.

bool find_moving_objects::Bank::machine_is_little_endian
private

Definition at line 442 of file bank.h.

unsigned int find_moving_objects::Bank::moa_seq
private

Definition at line 389 of file bank.h.

sensor_msgs::LaserScan find_moving_objects::Bank::msg_ema
private

Definition at line 392 of file bank.h.

sensor_msgs::LaserScan find_moving_objects::Bank::msg_objects_closest_point_markers
private

Definition at line 393 of file bank.h.

visualization_msgs::Marker find_moving_objects::Bank::msg_objects_delta_position_line
private

Definition at line 397 of file bank.h.

visualization_msgs::MarkerArray find_moving_objects::Bank::msg_objects_delta_position_lines
private

Definition at line 396 of file bank.h.

visualization_msgs::Marker find_moving_objects::Bank::msg_objects_velocity_arrow
private

Definition at line 395 of file bank.h.

visualization_msgs::MarkerArray find_moving_objects::Bank::msg_objects_velocity_arrows
private

Definition at line 394 of file bank.h.

visualization_msgs::Marker find_moving_objects::Bank::msg_objects_width_line
private

Definition at line 399 of file bank.h.

visualization_msgs::MarkerArray find_moving_objects::Bank::msg_objects_width_lines
private

Definition at line 398 of file bank.h.

ros::NodeHandle* find_moving_objects::Bank::node
private

Definition at line 375 of file bank.h.

int32_t find_moving_objects::Bank::PC2_message_x_bytes
private

Definition at line 437 of file bank.h.

int32_t find_moving_objects::Bank::PC2_message_x_offset
private

Definition at line 434 of file bank.h.

int32_t find_moving_objects::Bank::PC2_message_y_bytes
private

Definition at line 438 of file bank.h.

int32_t find_moving_objects::Bank::PC2_message_y_offset
private

Definition at line 435 of file bank.h.

int32_t find_moving_objects::Bank::PC2_message_z_bytes
private

Definition at line 439 of file bank.h.

int32_t find_moving_objects::Bank::PC2_message_z_offset
private

Definition at line 436 of file bank.h.

ros::Publisher find_moving_objects::Bank::pub_ema
private

Definition at line 381 of file bank.h.

ros::Publisher find_moving_objects::Bank::pub_objects
private

Definition at line 386 of file bank.h.

ros::Publisher find_moving_objects::Bank::pub_objects_closest_point_markers
private

Definition at line 382 of file bank.h.

ros::Publisher find_moving_objects::Bank::pub_objects_delta_position_lines
private

Definition at line 384 of file bank.h.

ros::Publisher find_moving_objects::Bank::pub_objects_velocity_arrows
private

Definition at line 383 of file bank.h.

ros::Publisher find_moving_objects::Bank::pub_objects_width_lines
private

Definition at line 385 of file bank.h.

double find_moving_objects::Bank::resolution
private

Definition at line 368 of file bank.h.

tf2_ros::Buffer* find_moving_objects::Bank::tf_buffer
private

Definition at line 378 of file bank.h.


The documentation for this class was generated from the following files:


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