bank.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2018, Andreas Gustavsson.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Andreas Gustavsson
*********************************************************************/

#ifndef BANK_H
#define BANK_H
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <find_moving_objects/MovingObject.h>
#include <find_moving_objects/MovingObjectArray.h>


namespace find_moving_objects
{

class BankArgument
{
public:
  double ema_alpha; 
  int nr_scans_in_bank; 
  int points_per_scan;  
  double angle_min; 
  double angle_max; 
  bool sensor_frame_has_z_axis_forward;
  double object_threshold_edge_max_delta_range; 
  int object_threshold_min_nr_points; 
  double object_threshold_max_distance; 
  double object_threshold_min_speed; 
  int object_threshold_max_delta_width_in_points; 
  double object_threshold_min_confidence;
  double object_threshold_bank_tracking_max_delta_distance; 
  double base_confidence; 
  bool publish_objects; 
  bool publish_ema; 
  bool publish_objects_closest_point_markers; 
  bool publish_objects_velocity_arrows; 
  bool publish_objects_delta_position_lines; 
  bool publish_objects_width_lines; 
  bool velocity_arrows_use_full_gray_scale; 
  bool velocity_arrows_use_sensor_frame; 
  bool velocity_arrows_use_base_frame; 
  bool velocity_arrows_use_fixed_frame; 
  std::string velocity_arrow_ns; 
  std::string delta_position_line_ns; 
  std::string width_line_ns; 
  std::string topic_objects; 
  std::string topic_ema;
  std::string topic_objects_closest_point_markers; 
  std::string topic_objects_velocity_arrows; 
  std::string topic_objects_delta_position_lines;
  std::string topic_objects_width_lines;
  int publish_buffer_size; 
  std::string map_frame; 
  std::string fixed_frame; 
  std::string base_frame;
//   // TODO: dox
//   float merge_threshold_max_angle_gap;
//   float merge_threshold_max_end_points_distance_delta;
//   float merge_threshold_max_velocity_direction_delta;
//   float merge_threshold_max_speed_delta;
  
  
  /*
   * PointCloud2 message-specific (none of these apply to LaserScan) 
   */
  std::string PC2_message_x_coordinate_field_name; 
  std::string PC2_message_y_coordinate_field_name;
  std::string PC2_message_z_coordinate_field_name;
  double PC2_voxel_leaf_size; 
  double PC2_threshold_z_min; 
  double PC2_threshold_z_max; 
  std::string node_name_suffix;
  BankArgument();
  
  friend class Bank;
  
private:
  friend std::ostream& operator<<(std::ostream& os, const BankArgument& ba);
  std::string sensor_frame; 
  float angle_increment; 
  float time_increment; 
  float scan_time; 
  float range_min;
  float range_max; 
  bool sensor_is_360_degrees;
  void check(); 
  void check_PC2();
};


class Bank
{
private:
  /* BANK ARGUMENTS */
  BankArgument bank_argument;
  
  /* BANK */
  float ** bank_ranges_ema;
  double * bank_stamp;
  unsigned int bank_ranges_bytes;
  bool bank_is_initialized;
  bool bank_is_filled;
  double resolution;
  
  /* INDICES FOR THE BANK */
  int bank_index_newest;
  int bank_index_put; // nr_scans_in_bank is/should be greater than 1!
  
  /* HANDLE TO THIS NODE */
  ros::NodeHandle * node;
  
  /* TRANSFORM BUFFER PTR */
  tf2_ros::Buffer * tf_buffer;
  
  /* PUBLISHERS */
  ros::Publisher pub_ema;
  ros::Publisher pub_objects_closest_point_markers;
  ros::Publisher pub_objects_velocity_arrows;
  ros::Publisher pub_objects_delta_position_lines;
  ros::Publisher pub_objects_width_lines;
  ros::Publisher pub_objects;
  
  /* SEQUENCE NR */
  unsigned int moa_seq;

  /* Additional messages to publish (not the actual moving objects message!) */
  sensor_msgs::LaserScan msg_ema; // EMA-adapted LaserScan with marked moving objects
  sensor_msgs::LaserScan msg_objects_closest_point_markers; // For visualizing objects as squares
  visualization_msgs::MarkerArray msg_objects_velocity_arrows; // For visualizing velocity using arrows...
  visualization_msgs::Marker msg_objects_velocity_arrow;       // ... one per object
  visualization_msgs::MarkerArray msg_objects_delta_position_lines; // For visualizing delta positions using lines...
  visualization_msgs::Marker msg_objects_delta_position_line;       // ... one per object
  visualization_msgs::MarkerArray msg_objects_width_lines; // For visualizing width using lines...
  visualization_msgs::Marker msg_objects_width_line;       // ... one per object
  
  /* Basic functionality used by the functions below*/
  void initBank(BankArgument bank_argument);
//   virtual long addFirstMessage(const sensor_msgs::LaserScan::ConstPtr &);
//   virtual long addFirstMessage(const sensor_msgs::PointCloud2::ConstPtr &, 
//                                const bool discard_message_if_no_points_added);
  virtual long addFirstMessage(const sensor_msgs::LaserScan *);
  virtual long addFirstMessage(const sensor_msgs::PointCloud2 *, 
                               const bool discard_message_if_no_points_added);
  inline void initIndex();
  inline void advanceIndex();
//   void mergeFoundObjects(MovingObjectArray * moa);
  
  /* 
   * Recursive tracking of an object through history to get the indices of its middle, 
   * left and right points in the oldest scans, along with the sum of all ranges etc.
   */
  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);
    
  /* PointCloud2 specifics */
  typedef uint8_t byte_t;
  int32_t PC2_message_x_offset;
  int32_t PC2_message_y_offset;
  int32_t PC2_message_z_offset;
  int32_t PC2_message_x_bytes;
  int32_t PC2_message_y_bytes;
  int32_t PC2_message_z_bytes;
  int getOffsetsAndBytes(BankArgument bank_argument, const sensor_msgs::PointCloud2::ConstPtr msg);
  int getOffsetsAndBytes(BankArgument bank_argument, const sensor_msgs::PointCloud2 * msg);
  bool machine_is_little_endian; // set in constructor
  void reverseBytes(byte_t * bytes, unsigned int nr_bytes);
  void readPoint(const byte_t * start_of_point,
                 const bool must_reverse_bytes,
                 double * x,
                 double * y,
                 double * z);
  void resetPutPoints();
  unsigned int putPoints(const sensor_msgs::PointCloud2::ConstPtr msg);
  unsigned int putPoints(const sensor_msgs::PointCloud2 * msg);
  void emaPutMessage();
  std::string getStringPutPoints();
  
  
  
  
public:
//   /**
//    * Creates an instance of Bank and starts a <code>tf::TransformListener</code>.
//    */
//   Bank();
  
  Bank(tf2_ros::Buffer * buffer);
  
  ~Bank();
  
  
  
//   /**
//    * 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)!
//    * @param bank_argument An instance of <code>BankArgument</code>, specifying the behavior of the bank.
//    * @param msg Pointer to the first received message to be added to the bank.
//    * @return 0 on success, -1 if this function must be called again.
//    */
//   virtual long init(BankArgument bank_argument, const sensor_msgs::LaserScan::ConstPtr & msg);
  
  virtual long init(BankArgument bank_argument, const sensor_msgs::LaserScan * msg);
  
//   /**
//    * 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)!
//    * @param bank_argument An instance of <code>BankArgument</code>, specifying the behavior of the bank.
//    * @param msg Pointer to the first received message to be added to the bank.
//    * @return 0 on success, -1 if this function must be called again.
//    */
//   virtual long init(BankArgument bank_argument, const sensor_msgs::PointCloud2::ConstPtr & msg, 
//                                                 const bool discard_message_if_no_points_added = true);
  
  virtual long init(BankArgument bank_argument, const sensor_msgs::PointCloud2 * msg, 
                                                const bool discard_message_if_no_points_added = true);
  
  
  
//   /**
//    * Add a <code>sensor_msgs::LaserScan</code> message to the bank (replace the oldest scan message).
//    * 
//    * @param msg Pointer to the received message to be added to the bank.
//    * @return 0.
//    */
//   virtual long addMessage(const sensor_msgs::LaserScan::ConstPtr & msg);
  
  virtual long addMessage(const sensor_msgs::LaserScan * msg);
  
//   /**
//    * Add a <code>sensor_msgs::PointCloud2</code> message to the bank (replace the oldest scan message).
//    * 
//    * @param msg Pointer to the received message to be added to the bank.
//    * @return 0 on success, -1 if adding this message failed.
//    */
//   
//   virtual long addMessage(const sensor_msgs::PointCloud2::ConstPtr & msg, 
//                           const bool discard_message_if_no_points_added = true);
  
  virtual long addMessage(const sensor_msgs::PointCloud2 * msg, 
                          const bool discard_message_if_no_points_added = true);
  
  
  
  void findAndReportMovingObjects();
  
  virtual double calculateConfidence(const find_moving_objects::MovingObject & mo,
                                     const find_moving_objects::BankArgument & ba,
                                     const double delta_time,
                                     const double mo_old_width);
};

// template<typename BaseClass, typename T>
// inline bool instanceof(const T *ptr) 
// {
//   return dynamic_cast<const BaseClass*>(ptr) != nullptr;
// }

} // namespace find_moving_objects

#endif // BANK_H