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();
 
  void closeActiveConnection();
 
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