hfl110dcu.h

Go to the documentation of this file.

// Copyright 2020 Continental AG
// All rights reserved.
//
// Software License Agreement (BSD 2-Clause Simplified License)
//
// 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.
//
// 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 HOLDER 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.


#ifndef IMAGE_PROCESSOR__HFL110DCU_H_
#define IMAGE_PROCESSOR__HFL110DCU_H_

#include <base_hfl110dcu.h>

#include <angles/angles.h>
#include <arpa/inet.h>
#include <camera_info_manager/camera_info_manager.h>
#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/TransformStamped.h>
#include <image_transport/image_transport.h>
#include <image_geometry/pinhole_camera_model.h>
#include <ros/package.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/UInt16MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>

#include <string>
#include <vector>
#include <cmath>
#include <memory>

#include "ros/ros.h"


#define HFL110_MAGIC_NUMBER_16_BIT 0.000762951           // 50 / 2^16

const float NO_RETURN_DISTANCES = NAN;

namespace hfl
{
struct PointCloudReturn
{
  uint16_t range;
  uint16_t intensity;
  uint16_t range2;
  uint16_t intensity2;
};

struct UdpPacketHeader
{
  uint16_t udp_version;
  uint16_t pca_version;
  uint64_t timeStamp;
  uint32_t upd_packet_number;
  uint32_t image_row_number;
};

struct CameraIntrinsics
{
  float_t fx;
  float_t fy;
  float_t ux;
  float_t uy;
  float_t r1;
  float_t r2;
  float_t t1;
  float_t t2;
  float_t r3;
};

struct CameraExtrinsics
{
  float_t intrinsic_yaw;
  float_t intrinsic_pitch;
  float_t extrinsic_yaw;
  float_t extrinsic_pitch;
  float_t extrinsic_roll;
  float_t extrinsic_vertical;
  float_t extrinsic_horizontal;
  float_t extrinsic_distance;
  uint32_t status;
};

struct UdpFrame
{
  UdpPacketHeader header;
  CameraIntrinsics camera_intrinsics;
  CameraExtrinsics camera_extrinsics;
  PointCloudReturn returns[128];
  uint8_t pixel_type[128];
};

struct objGeo
{
  float x_rear_r;
  float y_rear_r;
  float x_rear_l;
  float y_rear_l;
  float x_front_l;
  float y_front_l;
  float height;
  float ground_offset;
  float fDistX;
  float fDistY;
  float yaw;
};

struct objKin
{
  float fVabsX;
  float fVabsY;
  float fVrelX;
  float fVrelY;
  float fAabsX;
  float fDistXDistY;
  float fDistXVx;
  float fDistXVy;
  float fDistXAx;
  float fDistXAy;
  float fDistYVx;
  float fDistYVy;
  float fDistYAx;
  float fDistYAy;
  float fVxVy;
  float fVxAx;
  float fVxAy;
  float fVyAx;
  float fVyAy;
  float fAxAy;
};

struct objState
{
  unsigned TP_OBJ_MT_STATE_DELETED : 1;
  unsigned TP_OBJ_MT_STATE_NEW : 1;
  unsigned TP_OBJ_MT_STATE_MEASURED : 1;
  unsigned TP_OBJ_MT_STATE_PREDICTED : 1;
  unsigned TP_OBJ_MT_STATE_INACTIVE : 1;
  unsigned TP_OBJ_MT_STATE_MAX_DIFF : 1;
};

struct objDyn
{
  unsigned EM_GEN_OBJECT_DYN_PROPERTY_MOVING : 1;
  unsigned EM_GEN_OBJECT_DYN_PROPERTY_STATIONARY : 1;
  unsigned EM_GEN_OBJECT_DYN_PROPERTY_ONCOMING : 1;
  unsigned EM_GEN_OBJECT_DYN_PROPERTY_CROSSING_LEFT : 1;
  unsigned EM_GEN_OBJECT_DYN_PROPERTY_CROSSING_RIGHT : 1;
  unsigned EM_GEN_OBJECT_DYN_PROPERTY_UNKNOWN : 1;
  unsigned EM_GEN_OBJECT_DYN_PROPERTY_STOPPED : 1;
  unsigned EM_GEN_OBJECT_DYN_PROPERTY_MAX_DIFF_TYPES : 1;
};

struct hflObj
{
  objGeo geometry;
  objKin kinematics;
  objState state;
  objDyn dynamic_props;
  uint8_t quality;
  uint8_t classification;
  uint8_t confidence;
};

struct telemetry
{
  uint32_t uiHardwareRevision;
  float fSensorTemp;
  float fHeaterTemp;
  uint32_t uiFrameCounter;
  float fADCUbattSW;
  float fADCUbatt;
  float fADCHeaterLens;
  float fADCHeaterLensHigh;
  float fADCTemp0Lens;
  float fAcquisitionPeriod;
  unsigned uiTempSensorFeedback;
  char au8SerialNumber[26];
};

class HFL110DCU : public BaseHFL110DCU
{
public:
  HFL110DCU(std::string model, std::string version,
            std::string frame_id, ros::NodeHandle& node_handler);

  bool parseFrame(int start_byte, const std::vector<uint8_t>& packet) override;

  bool processFrameData(const std::vector<uint8_t>& data) override;

  //bool parsePDM(int start_byte, const std::vector<uint8_t>& packet) override;
  
  //bool processPDMData(const std::vector<uint8_t>& data) override;
  
  bool parseObjects(int start_byte, const std::vector<uint8_t>& packet) override;

  bool processObjectData(const std::vector<uint8_t>& data) override;

  bool processTelemetryData(const std::vector<uint8_t>& data) override;
  
  bool processSliceData(const std::vector<uint8_t>& data) override;
  
  cv::Mat initTransform(cv::Mat cameraMatrix, cv::Mat distCoeffs,
      int width, int height, bool radial);

  void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat);

private:
  ros::NodeHandle node_handler_;

  int bytes_received_;

  std::shared_ptr<std_msgs::Header> frame_header_message_;

  std::shared_ptr<std_msgs::Header> pdm_header_message_;
  
  std::shared_ptr<std_msgs::Header> object_header_message_;
  
  std::shared_ptr<std_msgs::Header> tele_header_message_;

  std::shared_ptr<std_msgs::Header> slice_header_message_;
  
  std::shared_ptr<std_msgs::Header> tf_header_message_;

  uint8_t row_, col_;

  uint8_t expected_packet_ = 0;

  float focal_length_;

  // Camera info manager
  camera_info_manager::CameraInfoManager *camera_info_manager_;

  cv_bridge::CvImagePtr p_image_depth_;

  cv_bridge::CvImagePtr p_image_intensity_;

  cv_bridge::CvImagePtr p_image_depth2_;

  cv_bridge::CvImagePtr p_image_intensity2_;
  
  cv_bridge::CvImagePtr p_image_crosstalk_;
  
  cv_bridge::CvImagePtr p_image_saturated_;
  
  cv_bridge::CvImagePtr p_image_superimposed_;
  
  cv_bridge::CvImagePtr p_image_crosstalk2_;
  
  cv_bridge::CvImagePtr p_image_saturated2_;
  
  cv_bridge::CvImagePtr p_image_superimposed2_;
  
  image_transport::CameraPublisher pub_depth_;

  image_transport::CameraPublisher pub_depth2_;

  image_transport::CameraPublisher pub_intensity_;

  image_transport::CameraPublisher pub_intensity2_;

  image_transport::CameraPublisher pub_ct_;
  
  image_transport::CameraPublisher pub_ct2_;
  
  image_transport::CameraPublisher pub_sat_;
  
  image_transport::CameraPublisher pub_sat2_;
  
  image_transport::CameraPublisher pub_si_;
  
  image_transport::CameraPublisher pub_si2_;

  ros::Publisher pub_objects_;
  
  ros::Publisher pub_slices_;
  
  std::vector<hflObj> objects_;

  ros::Publisher pub_points_;

  telemetry telem_{};

  std::shared_ptr<sensor_msgs::PointCloud2> pointcloud_;

  std::shared_ptr<std_msgs::UInt16MultiArray> slices_;
  
  geometry_msgs::TransformStamped global_tf_;

  cv::Mat transform_;

  // Diagnostic Updater
  diagnostic_updater::Updater updater_;

};
}  // namespace hfl
#endif  // IMAGE_PROCESSOR__HFL110DCU_H_