os32c.h

Go to the documentation of this file.

#ifndef OMRON_OS32C_DRIVER_OS32C_H
#define OMRON_OS32C_DRIVER_OS32C_H

#include <gtest/gtest_prod.h>
#include <string>
#include <vector>
#include <boost/shared_ptr.hpp>
#include <sensor_msgs/LaserScan.h>

#include "odva_ethernetip/session.h"
#include "odva_ethernetip/socket/socket.h"
#include "omron_os32c_driver/measurement_report.h"
#include "omron_os32c_driver/measurement_report_config.h"
#include "omron_os32c_driver/range_and_reflectance_measurement.h"

using std::vector;
using boost::shared_ptr;
using sensor_msgs::LaserScan;
using eip::Session;
using eip::socket::Socket;

#define DEG2RAD(a) (a * M_PI / 180)
#define RAD2DEG(a) (a * 180 / M_PI)

namespace omron_os32c_driver {


typedef enum
{
  NO_TOF_MEASUREMENTS       = 0,
  RANGE_MEASURE_50M         = 1,
  RANGE_MEASURE_32M_PZ      = 2,
  RANGE_MEASURE_16M_WZ1PZ   = 3,
  RANGE_MEASURE_8M_WZ2WZ1PZ = 4,
  RANGE_MEASURE_TOF_4PS     = 5,
} OS32C_RANGE_FORMAT;

typedef enum
{
  NO_TOT_MEASUREMENTS               = 0,
  REFLECTIVITY_MEASURE_TOT_ENCODED  = 1,
  REFLECTIVITY_MEASURE_TOT_4PS      = 2,
} OS32C_REFLECTIVITY_FORMAT;

class OS32C : public Session
{
public:
  OS32C(shared_ptr<Socket> socket, shared_ptr<Socket> io_socket)
    : Session(socket, io_socket), start_angle_(ANGLE_MAX), end_angle_(ANGLE_MIN),
      connection_num_(-1), mrc_sequence_num_(1)
  {
  }

  static const double ANGLE_MIN;
  static const double ANGLE_MAX;
  static const double ANGLE_INC;
  static const double DISTANCE_MIN;
  static const double DISTANCE_MAX;

  EIP_UINT getRangeFormat();

  void setRangeFormat(EIP_UINT format);

  EIP_UINT getReflectivityFormat();

  void setReflectivityFormat(EIP_UINT format);


  void selectBeams(double start_angle, double end_angle);

  RangeAndReflectanceMeasurement getSingleRRScan();

  static inline int calcBeamNumber(double angle)
  {
    return (ANGLE_MAX - angle + ANGLE_INC / 2) / ANGLE_INC;
  }

  static inline double calcBeamCentre(int beam_num)
  {
    return ANGLE_MAX - beam_num * ANGLE_INC;
  }

  void fillLaserScanStaticConfig(sensor_msgs::LaserScan* ls);

  static void convertToLaserScan(const RangeAndReflectanceMeasurement& rr, sensor_msgs::LaserScan* ls);

  static void convertToLaserScan(const MeasurementReport& mr, sensor_msgs::LaserScan* ls);

  void sendMeasurmentReportConfigUDP();

  MeasurementReport receiveMeasurementReportUDP();

  void startUDPIO();

private:
  // allow unit tests to access the helpers below for direct testing
  FRIEND_TEST(OS32CTest, test_calc_beam_mask_all);
  FRIEND_TEST(OS32CTest, test_calc_beam_at_90);
  FRIEND_TEST(OS32CTest, test_calc_beam_boundaries);
  FRIEND_TEST(OS32CTest, test_calc_beam_invalid_args);
  FRIEND_TEST(OS32CTest, test_convert_to_laserscan);

  double start_angle_;
  double end_angle_;

  // data for sending to lidar to keep UDP session alive
  int connection_num_;
  MeasurementReportConfig mrc_;
  EIP_UDINT mrc_sequence_num_;

  void calcBeamMask(double start_angle, double end_angle, EIP_BYTE mask[]);
};

} // namespace omron_os32c_driver

#endif  // OMRON_OS32C_DRIVER_OS32C_H