SickSafetyscannersRos.h

Go to the documentation of this file.

// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

// -- BEGIN LICENSE BLOCK ----------------------------------------------

// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
//----------------------------------------------------------------------

#ifndef SICK_SAFETYSCANNERS_SICKSAFETYSCANNERSROS_H
#define SICK_SAFETYSCANNERS_SICKSAFETYSCANNERSROS_H


// ROS
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/LaserScan.h>

// STD
#include <string>
#include <vector>

// Package
#include <sick_safetyscanners/ConfigMetadata.h>
#include <sick_safetyscanners/ExtendedLaserScanMsg.h>
#include <sick_safetyscanners/FieldData.h>
#include <sick_safetyscanners/OutputPathsMsg.h>
#include <sick_safetyscanners/RawMicroScanDataMsg.h>
#include <sick_safetyscanners/SickSafetyscanners.h>
#include <sick_safetyscanners/SickSafetyscannersConfigurationConfig.h>
#include <sick_safetyscanners/StatusOverview.h>
#include <sick_safetyscanners/datastructure/CommSettings.h>
#include <sick_safetyscanners/datastructure/FieldData.h>

#include <dynamic_reconfigure/server.h>

#include <cmath>

namespace sick {

inline float degToRad(float deg)
{
  return deg * M_PI / 180.0f;
}

inline float radToDeg(float rad)
{
  return rad * 180.0f / M_PI;
}

inline uint16_t skipToPublishFrequency(int skip)
{
  return skip + 1;
}

typedef diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan> DiagnosedLaserScanPublisher;

class SickSafetyscannersRos
{
public:
  SickSafetyscannersRos();

  virtual ~SickSafetyscannersRos();

private:
  ros::NodeHandle m_nh;

  ros::NodeHandle m_private_nh;

  ros::Publisher m_laser_scan_publisher;
  ros::Publisher m_extended_laser_scan_publisher;
  ros::Publisher m_raw_data_publisher;
  ros::Publisher m_output_path_publisher;

  // Diagnostics
  diagnostic_updater::Updater m_diagnostic_updater;
  std::shared_ptr<DiagnosedLaserScanPublisher> m_diagnosed_laser_scan_publisher;
  sick_safetyscanners::RawMicroScanDataMsg m_last_raw_data;
  sick::datastructure::ConfigMetadata config_meta_data;
  sick::datastructure::FirmwareVersion firmware_version;
  void sensorDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& diagnostic_status);

  ros::ServiceServer m_field_service_server;
  ros::ServiceServer m_config_metadata_server;
  ros::ServiceServer m_status_overview_server;

  bool m_initialised;

  std::shared_ptr<sick::SickSafetyscanners> m_device;

  sick::datastructure::CommSettings m_communication_settings;
  boost::asio::ip::address_v4 m_interface_ip;

  dynamic_reconfigure::Server<sick_safetyscanners::SickSafetyscannersConfigurationConfig>
    m_dynamic_reconfiguration_server;

  std::string m_frame_id;
  double m_time_offset;
  double m_range_min;
  double m_range_max;
  double m_frequency_tolerance      = 0.1;
  double m_expected_frequency       = 20.0;
  double m_timestamp_min_acceptable = -1.0;
  double m_timestamp_max_acceptable = 1.0;
  double m_min_intensities          = 0.0; 
  bool m_use_sick_angles;
  float m_angle_offset;
  bool m_use_pers_conf;

  bool readParameters();

  void receivedUDPPacket(const datastructure::Data& data);

  void reconfigureCallback(const sick_safetyscanners::SickSafetyscannersConfigurationConfig& config,
                           const uint32_t& level);

  bool isInitialised();

  sensor_msgs::LaserScan createLaserScanMessage(const sick::datastructure::Data& data);
  sick_safetyscanners::ExtendedLaserScanMsg
  createExtendedLaserScanMessage(const sick::datastructure::Data& data);
  std::vector<bool>
  getMedianReflectors(const std::vector<sick::datastructure::ScanPoint> scan_points);
  sick_safetyscanners::OutputPathsMsg
  createOutputPathsMessage(const sick::datastructure::Data& data);
  sick_safetyscanners::RawMicroScanDataMsg
  createRawDataMessage(const sick::datastructure::Data& data);
  sick_safetyscanners::DataHeaderMsg createDataHeaderMessage(const sick::datastructure::Data& data);
  sick_safetyscanners::DerivedValuesMsg
  createDerivedValuesMessage(const sick::datastructure::Data& data);
  sick_safetyscanners::GeneralSystemStateMsg
  createGeneralSystemStateMessage(const sick::datastructure::Data& data);
  sick_safetyscanners::MeasurementDataMsg
  createMeasurementDataMessage(const sick::datastructure::Data& data);
  std::vector<sick_safetyscanners::ScanPointMsg>
  createScanPointMessageVector(const sick::datastructure::Data& data);
  sick_safetyscanners::IntrusionDataMsg
  createIntrusionDataMessage(const sick::datastructure::Data& data);
  std::vector<sick_safetyscanners::IntrusionDatumMsg>
  createIntrusionDatumMessageVector(const sick::datastructure::Data& data);
  sick_safetyscanners::ApplicationDataMsg
  createApplicationDataMessage(const sick::datastructure::Data& data);
  sick_safetyscanners::ApplicationInputsMsg
  createApplicationInputsMessage(const sick::datastructure::Data& data);
  sick_safetyscanners::ApplicationOutputsMsg
  createApplicationOutputsMessage(const sick::datastructure::Data& data);
  void readTypeCodeSettings();
  void readPersistentConfig();

  bool getFieldData(sick_safetyscanners::FieldData::Request& req,
                    sick_safetyscanners::FieldData::Response& res);

  bool getConfigMetadata(sick_safetyscanners::ConfigMetadata::Request& req,
                         sick_safetyscanners::ConfigMetadata::Response& res);

  bool getStatusOverview(sick_safetyscanners::StatusOverview::Request& req,
                         sick_safetyscanners::StatusOverview::Response& res);

  std::string getCheckSumString(uint32_t checksum);

  std::string getDateString(uint32_t days_since_1972, uint32_t milli_seconds);
};

} // namespace sick

#endif // SICK_SAFETYSCANNERS_SICKSAFETYSCANNERSROS_H