00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 00002 00003 // -- BEGIN LICENSE BLOCK ---------------------------------------------- 00004 00024 // -- END LICENSE BLOCK ------------------------------------------------ 00025 00026 //---------------------------------------------------------------------- 00033 //---------------------------------------------------------------------- 00034 00035 #ifndef SICK_SAFETYSCANNERS_SICKSAFETYSCANNERSROS_H 00036 #define SICK_SAFETYSCANNERS_SICKSAFETYSCANNERSROS_H 00037 00038 00039 // ROS 00040 #include <ros/ros.h> 00041 #include <sensor_msgs/JointState.h> 00042 #include <sensor_msgs/LaserScan.h> 00043 00044 // STD 00045 #include <string> 00046 #include <vector> 00047 00048 // Package 00049 #include <sick_safetyscanners/ExtendedLaserScanMsg.h> 00050 #include <sick_safetyscanners/FieldData.h> 00051 #include <sick_safetyscanners/OutputPathsMsg.h> 00052 #include <sick_safetyscanners/RawMicroScanDataMsg.h> 00053 #include <sick_safetyscanners/SickSafetyscanners.h> 00054 #include <sick_safetyscanners/SickSafetyscannersConfigurationConfig.h> 00055 #include <sick_safetyscanners/datastructure/CommSettings.h> 00056 #include <sick_safetyscanners/datastructure/FieldData.h> 00057 00058 #include <dynamic_reconfigure/server.h> 00059 00060 #include <cmath> 00061 00062 namespace sick { 00063 00069 inline float degToRad(float deg) 00070 { 00071 return deg * M_PI / 180.0f; 00072 } 00073 00079 inline float radToDeg(float rad) 00080 { 00081 return rad * 180.0f / M_PI; 00082 } 00083 00091 inline uint16_t skipToPublishFrequency(int skip) 00092 { 00093 return skip + 1; 00094 } 00095 00101 class SickSafetyscannersRos 00102 { 00103 public: 00111 SickSafetyscannersRos(); 00112 00117 virtual ~SickSafetyscannersRos(); 00118 00119 private: 00121 ros::NodeHandle m_nh; 00122 00124 ros::NodeHandle m_private_nh; 00125 00127 ros::Publisher m_laser_scan_publisher; 00128 ros::Publisher m_extended_laser_scan_publisher; 00129 ros::Publisher m_raw_data_publisher; 00130 ros::Publisher m_output_path_publisher; 00131 00132 ros::ServiceServer m_field_service_server; 00133 00134 bool m_initialised; 00135 00136 std::shared_ptr<sick::SickSafetyscanners> m_device; 00137 00138 sick::datastructure::CommSettings m_communication_settings; 00139 00140 dynamic_reconfigure::Server<sick_safetyscanners::SickSafetyscannersConfigurationConfig> 00141 m_dynamic_reconfiguration_server; 00142 00143 std::string m_frame_id; 00144 double m_time_offset; 00145 double m_range_min; 00146 double m_range_max; 00147 00148 bool m_use_sick_angles; 00149 float m_angle_offset; 00150 bool m_use_pers_conf; 00151 00156 bool readParameters(); 00157 00162 void receivedUDPPacket(const datastructure::Data& data); 00163 00169 void 00170 reconfigure_callback(const sick_safetyscanners::SickSafetyscannersConfigurationConfig& config, 00171 const uint32_t& level); 00172 00173 bool isInitialised(); 00174 00175 sensor_msgs::LaserScan createLaserScanMessage(const sick::datastructure::Data& data); 00176 sick_safetyscanners::ExtendedLaserScanMsg 00177 createExtendedLaserScanMessage(const sick::datastructure::Data& data); 00178 std::vector<bool> 00179 getMedianReflectors(const std::vector<sick::datastructure::ScanPoint> scan_points); 00180 sick_safetyscanners::OutputPathsMsg 00181 createOutputPathsMessage(const sick::datastructure::Data& data); 00182 sick_safetyscanners::RawMicroScanDataMsg 00183 createRawDataMessage(const sick::datastructure::Data& data); 00184 sick_safetyscanners::DataHeaderMsg createDataHeaderMessage(const sick::datastructure::Data& data); 00185 sick_safetyscanners::DerivedValuesMsg 00186 createDerivedValuesMessage(const sick::datastructure::Data& data); 00187 sick_safetyscanners::GeneralSystemStateMsg 00188 createGeneralSystemStateMessage(const sick::datastructure::Data& data); 00189 sick_safetyscanners::MeasurementDataMsg 00190 createMeasurementDataMessage(const sick::datastructure::Data& data); 00191 std::vector<sick_safetyscanners::ScanPointMsg> 00192 createScanPointMessageVector(const sick::datastructure::Data& data); 00193 sick_safetyscanners::IntrusionDataMsg 00194 createIntrusionDataMessage(const sick::datastructure::Data& data); 00195 std::vector<sick_safetyscanners::IntrusionDatumMsg> 00196 createIntrusionDatumMessageVector(const sick::datastructure::Data& data); 00197 sick_safetyscanners::ApplicationDataMsg 00198 createApplicationDataMessage(const sick::datastructure::Data& data); 00199 sick_safetyscanners::ApplicationInputsMsg 00200 createApplicationInputsMessage(const sick::datastructure::Data& data); 00201 sick_safetyscanners::ApplicationOutputsMsg 00202 createApplicationOutputsMessage(const sick::datastructure::Data& data); 00203 void readTypeCodeSettings(); 00204 void readPersistentConfig(); 00205 00206 bool getFieldData(sick_safetyscanners::FieldData::Request& req, 00207 sick_safetyscanners::FieldData::Response& res); 00208 }; 00209 00210 } // namespace sick 00211 00212 #endif // SICK_SAFETYSCANNERS_SICKSAFETYSCANNERSROS_H