SickSafetyscannersRos.h
Go to the documentation of this file.
1 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
2 
3 // -- BEGIN LICENSE BLOCK ----------------------------------------------
4 
24 // -- END LICENSE BLOCK ------------------------------------------------
25 
26 //----------------------------------------------------------------------
33 //----------------------------------------------------------------------
34 
35 #ifndef SICK_SAFETYSCANNERS_SICKSAFETYSCANNERSROS_H
36 #define SICK_SAFETYSCANNERS_SICKSAFETYSCANNERSROS_H
37 
38 
39 // ROS
42 #include <ros/ros.h>
43 #include <sensor_msgs/JointState.h>
44 #include <sensor_msgs/LaserScan.h>
45 
46 // STD
47 #include <string>
48 #include <vector>
49 
50 // Package
51 #include <sick_safetyscanners/ExtendedLaserScanMsg.h>
52 #include <sick_safetyscanners/FieldData.h>
53 #include <sick_safetyscanners/OutputPathsMsg.h>
54 #include <sick_safetyscanners/RawMicroScanDataMsg.h>
56 #include <sick_safetyscanners/SickSafetyscannersConfigurationConfig.h>
59 
60 #include <dynamic_reconfigure/server.h>
61 
62 #include <cmath>
63 
64 namespace sick {
65 
71 inline float degToRad(float deg)
72 {
73  return deg * M_PI / 180.0f;
74 }
75 
81 inline float radToDeg(float rad)
82 {
83  return rad * 180.0f / M_PI;
84 }
85 
93 inline uint16_t skipToPublishFrequency(int skip)
94 {
95  return skip + 1;
96 }
97 
99 
106 {
107 public:
116 
121  virtual ~SickSafetyscannersRos();
122 
123 private:
126 
129 
135 
136  // Diagnostics
138  std::shared_ptr<DiagnosedLaserScanPublisher> m_diagnosed_laser_scan_publisher;
139  sick_safetyscanners::RawMicroScanDataMsg m_last_raw_data;
141 
143 
145 
146  std::shared_ptr<sick::SickSafetyscanners> m_device;
147 
149  boost::asio::ip::address_v4 m_interface_ip;
150 
151  dynamic_reconfigure::Server<sick_safetyscanners::SickSafetyscannersConfigurationConfig>
153 
154  std::string m_frame_id;
156  double m_range_min;
157  double m_range_max;
158  double m_frequency_tolerance = 0.1;
159  double m_expected_frequency = 20.0;
162  double m_min_intensities = 0.0;
167 
172  bool readParameters();
173 
178  void receivedUDPPacket(const datastructure::Data& data);
179 
185  void reconfigureCallback(const sick_safetyscanners::SickSafetyscannersConfigurationConfig& config,
186  const uint32_t& level);
187 
188  bool isInitialised();
189 
190  sensor_msgs::LaserScan createLaserScanMessage(const sick::datastructure::Data& data);
191  sick_safetyscanners::ExtendedLaserScanMsg
193  std::vector<bool>
194  getMedianReflectors(const std::vector<sick::datastructure::ScanPoint> scan_points);
195  sick_safetyscanners::OutputPathsMsg
197  sick_safetyscanners::RawMicroScanDataMsg
199  sick_safetyscanners::DataHeaderMsg createDataHeaderMessage(const sick::datastructure::Data& data);
200  sick_safetyscanners::DerivedValuesMsg
202  sick_safetyscanners::GeneralSystemStateMsg
204  sick_safetyscanners::MeasurementDataMsg
206  std::vector<sick_safetyscanners::ScanPointMsg>
208  sick_safetyscanners::IntrusionDataMsg
210  std::vector<sick_safetyscanners::IntrusionDatumMsg>
212  sick_safetyscanners::ApplicationDataMsg
214  sick_safetyscanners::ApplicationInputsMsg
216  sick_safetyscanners::ApplicationOutputsMsg
218  void readTypeCodeSettings();
219  void readPersistentConfig();
220 
221  bool getFieldData(sick_safetyscanners::FieldData::Request& req,
222  sick_safetyscanners::FieldData::Response& res);
223 };
224 
225 } // namespace sick
226 
227 #endif // SICK_SAFETYSCANNERS_SICKSAFETYSCANNERSROS_H
sick_safetyscanners::ApplicationOutputsMsg createApplicationOutputsMessage(const sick::datastructure::Data &data)
sick_safetyscanners::MeasurementDataMsg createMeasurementDataMessage(const sick::datastructure::Data &data)
diagnostic_updater::Updater m_diagnostic_updater
sick_safetyscanners::DataHeaderMsg createDataHeaderMessage(const sick::datastructure::Data &data)
ros::NodeHandle m_nh
ROS node handle.
void reconfigureCallback(const sick_safetyscanners::SickSafetyscannersConfigurationConfig &config, const uint32_t &level)
Function which is triggered when a dynamic reconfiguration is performed.
sick_safetyscanners::GeneralSystemStateMsg createGeneralSystemStateMessage(const sick::datastructure::Data &data)
uint16_t skipToPublishFrequency(int skip)
Converts a skip value into a "publish frequency" value.
std::vector< sick_safetyscanners::IntrusionDatumMsg > createIntrusionDatumMessageVector(const sick::datastructure::Data &data)
std::shared_ptr< DiagnosedLaserScanPublisher > m_diagnosed_laser_scan_publisher
void sensorDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &diagnostic_status)
ros::ServiceServer m_field_service_server
sick_safetyscanners::RawMicroScanDataMsg createRawDataMessage(const sick::datastructure::Data &data)
Containing the communication settings for the sensor which can be changed on runtime.
Definition: CommSettings.h:48
sick_safetyscanners::ApplicationDataMsg createApplicationDataMessage(const sick::datastructure::Data &data)
The data class containing all data blocks of a measurement.
Definition: Data.h:55
dynamic_reconfigure::Server< sick_safetyscanners::SickSafetyscannersConfigurationConfig > m_dynamic_reconfiguration_server
sick_safetyscanners::ExtendedLaserScanMsg createExtendedLaserScanMessage(const sick::datastructure::Data &data)
sick_safetyscanners::DerivedValuesMsg createDerivedValuesMessage(const sick::datastructure::Data &data)
bool getFieldData(sick_safetyscanners::FieldData::Request &req, sick_safetyscanners::FieldData::Response &res)
void receivedUDPPacket(const datastructure::Data &data)
Function which is called when a new complete UDP Packet is received.
float degToRad(float deg)
Converts degrees to radians.
ros::Publisher m_laser_scan_publisher
ROS topic publisher.
The SickSafetyscannersRos class.
sick_safetyscanners::IntrusionDataMsg createIntrusionDataMessage(const sick::datastructure::Data &data)
ros::NodeHandle m_private_nh
ROS private node handle.
std::vector< bool > getMedianReflectors(const std::vector< sick::datastructure::ScanPoint > scan_points)
sick_safetyscanners::ApplicationInputsMsg createApplicationInputsMessage(const sick::datastructure::Data &data)
sick_safetyscanners::OutputPathsMsg createOutputPathsMessage(const sick::datastructure::Data &data)
std::shared_ptr< sick::SickSafetyscanners > m_device
sick::datastructure::CommSettings m_communication_settings
sensor_msgs::LaserScan createLaserScanMessage(const sick::datastructure::Data &data)
float radToDeg(float rad)
Converts radians to degrees.
sick_safetyscanners::RawMicroScanDataMsg m_last_raw_data
boost::asio::ip::address_v4 m_interface_ip
diagnostic_updater::DiagnosedPublisher< sensor_msgs::LaserScan > DiagnosedLaserScanPublisher
virtual ~SickSafetyscannersRos()
~SickSafetyscannersRos Destructor if the SickSafetyscanners ROS
bool readParameters()
Reads and verifies the ROS parameters.
ros::Publisher m_extended_laser_scan_publisher
std::vector< sick_safetyscanners::ScanPointMsg > createScanPointMessageVector(const sick::datastructure::Data &data)
SickSafetyscannersRos()
Constructor of the SickSafetyscannersRos.


sick_safetyscanners
Author(s): Lennart Puck
autogenerated on Fri Apr 2 2021 02:45:41