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