sick_generic_field_mon.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018, Ing.-Buero Dr. Michael Lehning, Hildesheim
3  * Copyright (C) 2018, SICK AG, Waldkirch
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * * Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in the
13  * documentation and/or other materials provided with the distribution.
14  * * Neither the name of Osnabrück University nor the names of its
15  * contributors may be used to endorse or promote products derived from
16  * this software without specific prior written permission.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28  * POSSIBILITY OF SUCH DAMAGE.
29  *
30  * Created on: 28th May 2018
31  *
32  * Authors:
33  * Michael Lehning <michael.lehning@lehning.de>
34  *
35  */
36 
37 #ifndef SICK_GENERIC_FIELD_MON_H_
38 #define SICK_GENERIC_FIELD_MON_H_
39 
40 #include <stdio.h>
41 #include <stdlib.h>
42 #include <string>
43 #include <string.h>
44 #include <vector>
45 
46 #include <ros/ros.h>
47 #include <sensor_msgs/LaserScan.h>
48 #include <sensor_msgs/PointCloud.h>
49 #include <sensor_msgs/PointCloud2.h>
50 #include <std_msgs/String.h>
51 
54 /*
55 #include <sick_scan/sick_scan_common_nw.h>
56 #include <sick_scan/RadarScan.h> // generated by msg-generator
57 
58 #ifndef _MSC_VER
59 
60 #include <dynamic_reconfigure/server.h>
61 #include <sick_scan/SickScanConfig.h>
62 
63 #endif
64 
65 #include "sick_scan/sick_generic_parser.h"
66 #include "sick_scan/sick_scan_common_nw.h"
67 */
68 
69 namespace sick_scan
70 {
72  {
73  MON_FIELD_RADIAL = 0, // not supported
77  };
78 
80  {
81  public:
82  /*
83  ** @brief Converts a point of a segmented field to carthesian coordinates
84  ** @param[in] range range in meter
85  ** @param[in] angle_rad angle in radians
86  ** @param[out] x x in meter in ros coordinate system
87  ** @param[out] y y in meter in ros coordinate system
88  */
89  static void segmentedFieldPointToCarthesian(float range, float angle_rad, float& x, float& y);
90 
91  /*
92  ** @brief Converts a rectangular field to carthesian coordinates, i.e. to 4 corner points carthesian (ros) coordinates
93  ** @param[in] distRefPointMeter range of ref point (rect center) in meter
94  ** @param[in] angleRefPointRad angle of ref point (rect center) in radians
95  ** @param[in] rotAngleRad rotation of rectangle in radians
96  ** @param[in] rectWidthMeter width of rectangle in meter
97  ** @param[in] rectLengthMeter width of rectangle in meter
98  ** @param[out] points_x x of corner points in meter in ros coordinate system
99  ** @param[out] points_y y of corner points in meter in ros coordinate system
100  */
101  static void rectangularFieldToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float points_x[4], float points_y[4]);
102 
103  /*
104  ** @brief Converts a dynamic field to carthesian coordinates. The dynamic field is just converted into 2 rectangular fields,
105  ** each rectangular field described by to 4 corner points carthesian (ros) coordinates.
106  ** The first rectangular field has size rectWidthMeter x maxLengthMeter, the second rectangular field has size rectWidthMeter x rectLengthMeter.
107  ** The rectangular fields are returned by 4 corner points (points_x[n], points_y[n]) with 0 <= n < 4 for the first (big) rectangle and 4 <= n < 8 for the second (smaller) rectangle.
108  ** @param[in] distRefPointMeter range of ref point (rect center) in meter
109  ** @param[in] angleRefPointRad angle of ref point (rect center) in radians
110  ** @param[in] rotAngleRad rotation of rectangle in radians
111  ** @param[in] rectWidthMeter width of rectangle in meter
112  ** @param[in] rectLengthMeter width of rectangle in meter at v = 0
113  ** @param[in] maxSpeedMeterPerSec max speed (unused)
114  ** @param[in] maxLengthMeter width of rectangle in meter at v = max. speed
115  ** @param[out] points_x x of corner points in meter in ros coordinate system
116  ** @param[out] points_y y of corner points in meter in ros coordinate system
117  */
118  static void dynamicFieldPointToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float maxSpeedMeterPerSec, float maxLengthMeter, float points_x[8], float points_y[8]);
119 
120  };
121 
123  {
124  public:
125 
127  const SickScanMonFieldType& fieldType(void) const { return m_fieldType; }
128 
129  void pushFieldPointCarthesian(float x, float y)
130  {
131  m_fieldPoints_X.push_back(x);
132  m_fieldPoints_Y.push_back(y);
133  }
134 
135  int getPointCount(void) const
136  {
137  assert(m_fieldPoints_X.size() == m_fieldPoints_Y.size());
138  return m_fieldPoints_X.size();
139  }
140 
141  const std::vector<float>& getFieldPointsX(void) const { return m_fieldPoints_X; }
142  const std::vector<float>& getFieldPointsY(void) const { return m_fieldPoints_Y; }
143 
144  private:
146  std::vector<float> m_fieldPoints_X;
147  std::vector<float> m_fieldPoints_Y;
148  };
149 
150 
152  {
153  private:
154  /* Here will be the instance stored. */
156 
157  /* Private constructor to prevent instancing. */
159 
162  std::vector<SickScanMonField>monFields;
164 
165  public:
166  /* Static access method. */
168 
169  const std::vector<SickScanMonField>& getMonFields(void) const { return monFields; }
170 
171  void setActiveFieldset(int active_fieldset) { active_mon_fieldset = active_fieldset; }
173 
174  int parseAsciiLIDinputstateMsg(unsigned char* datagram, int datagram_length);
175  int parseBinaryLIDinputstateMsg(unsigned char* datagram, int datagram_length);
176 
177  int parseBinaryDatagram(std::vector<unsigned char> datagramm);
178 
179  int parseAsciiDatagram(std::vector<unsigned char> datagramm); /* , SickScanConfig &config, */ // sensor_msgs::LaserScan &msg, int &numEchos, int &echoMask);
180  };
181 
182 
183 #if 0
184  class SickScanRadar
185  {
186  public:
187  SickScanRadar(SickScanCommon *commonPtr_)
188  {
189  commonPtr = commonPtr_;
190  }
191  void setEmulation(bool _emul);
192  bool getEmulation(void);
193  int parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol);
194  int parseAsciiDatagram(char* datagram, size_t datagram_length, sick_scan::RadarScan *msgPtr, std::vector<SickScanRadarObject> &objectList, std::vector<SickScanRadarRawTarget> &rawTargetList); /* , SickScanConfig &config, */ // sensor_msgs::LaserScan &msg, int &numEchos, int &echoMask);
195  void simulateAsciiDatagram(unsigned char * receiveBuffer, int* actual_length);
196  private:
197 // SickScanCommon *commonPtr;
198  void simulateAsciiDatagramFromFile(unsigned char *receiveBuffer, int *actual_length, std::string filePattern);
199  bool emul;
200  };
201 #endif
202 
203 } /* namespace sick_scan */
204 #endif // SICK_GENERIC_RADAR_H_
sick_scan::SickScanFieldMonSingleton::getInstance
static SickScanFieldMonSingleton * getInstance()
Definition: sick_generic_field_mon.cpp:176
sick_scan::SickScanMonField::getFieldPointsX
const std::vector< float > & getFieldPointsX(void) const
Definition: sick_generic_field_mon.h:141
sick_scan::MON_FIELD_RECTANGLE
@ MON_FIELD_RECTANGLE
Definition: sick_generic_field_mon.h:74
ros::Publisher
sick_scan::MON_FIELD_DYNAMIC
@ MON_FIELD_DYNAMIC
Definition: sick_generic_field_mon.h:76
sick_scan::SickScanMonFieldConverter::dynamicFieldPointToCarthesian
static void dynamicFieldPointToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float maxSpeedMeterPerSec, float maxLengthMeter, float points_x[8], float points_y[8])
Definition: sick_generic_field_mon.cpp:158
ros.h
sick_scan::SickScanFieldMonSingleton::setActiveFieldset
void setActiveFieldset(int active_fieldset)
Definition: sick_generic_field_mon.h:171
sick_scan::MON_FIELD_RADIAL
@ MON_FIELD_RADIAL
Definition: sick_generic_field_mon.h:73
sick_scan::SickScanFieldMonSingleton::nh_
ros::NodeHandle nh_
Definition: sick_generic_field_mon.h:160
sick_scan::SickScanFieldMonSingleton::chatter_pub
ros::Publisher chatter_pub
Definition: sick_generic_field_mon.h:161
sick_scan::SickScanFieldMonSingleton::active_mon_fieldset
int active_mon_fieldset
Definition: sick_generic_field_mon.h:163
sick_scan::SickScanFieldMonSingleton::instance
static SickScanFieldMonSingleton * instance
Definition: sick_generic_field_mon.h:155
sick_scan::SickScanMonField::m_fieldPoints_Y
std::vector< float > m_fieldPoints_Y
Definition: sick_generic_field_mon.h:147
sick_scan::SickScanFieldMonSingleton::parseBinaryLIDinputstateMsg
int parseBinaryLIDinputstateMsg(unsigned char *datagram, int datagram_length)
Parse binary LIDinputstate message and set active field set.
Definition: sick_generic_field_mon.cpp:210
sick_scan::MON_FIELD_SEGMENTED
@ MON_FIELD_SEGMENTED
Definition: sick_generic_field_mon.h:75
sick_scan::SickScanMonFieldConverter
Definition: sick_generic_field_mon.h:79
publisher.h
sick_scan::SickScanFieldMonSingleton::getMonFields
const std::vector< SickScanMonField > & getMonFields(void) const
Definition: sick_generic_field_mon.h:169
diagnostic_updater.h
sick_scan::SickScanMonField::getFieldPointsY
const std::vector< float > & getFieldPointsY(void) const
Definition: sick_generic_field_mon.h:142
sick_scan::SickScanMonField::m_fieldType
SickScanMonFieldType m_fieldType
Definition: sick_generic_field_mon.h:145
sick_scan::SickScanMonField::fieldType
SickScanMonFieldType & fieldType(void)
Definition: sick_generic_field_mon.h:126
sick_scan::SickScanMonFieldType
SickScanMonFieldType
Definition: sick_generic_field_mon.h:71
sick_scan
Definition: abstract_parser.cpp:50
sick_scan::SickScanFieldMonSingleton::getActiveFieldset
int getActiveFieldset(void)
Definition: sick_generic_field_mon.h:172
sick_scan::SickScanFieldMonSingleton
Definition: sick_generic_field_mon.h:151
sick_scan::SickScanMonField::fieldType
const SickScanMonFieldType & fieldType(void) const
Definition: sick_generic_field_mon.h:127
sick_scan::SickScanMonField::getPointCount
int getPointCount(void) const
Definition: sick_generic_field_mon.h:135
sick_scan::SickScanFieldMonSingleton::monFields
std::vector< SickScanMonField > monFields
Definition: sick_generic_field_mon.h:162
ros::Time
sick_scan::SickScanMonField
Definition: sick_generic_field_mon.h:122
sick_scan::SickScanMonFieldConverter::segmentedFieldPointToCarthesian
static void segmentedFieldPointToCarthesian(float range, float angle_rad, float &x, float &y)
Definition: sick_generic_field_mon.cpp:96
sick_scan::SickScanMonField::pushFieldPointCarthesian
void pushFieldPointCarthesian(float x, float y)
Definition: sick_generic_field_mon.h:129
sick_scan::SickScanFieldMonSingleton::parseAsciiLIDinputstateMsg
int parseAsciiLIDinputstateMsg(unsigned char *datagram, int datagram_length)
Parse binary LIDinputstate message and set active field set.
Definition: sick_generic_field_mon.cpp:198
sick_scan::SickScanFieldMonSingleton::SickScanFieldMonSingleton
SickScanFieldMonSingleton()
Definition: sick_generic_field_mon.cpp:186
sick_scan::SickScanMonField::m_fieldPoints_X
std::vector< float > m_fieldPoints_X
Definition: sick_generic_field_mon.h:146
sick_scan::SickScanMonFieldConverter::rectangularFieldToCarthesian
static void rectangularFieldToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float points_x[4], float points_y[4])
Definition: sick_generic_field_mon.cpp:112
sick_scan::SickScanFieldMonSingleton::parseBinaryDatagram
int parseBinaryDatagram(std::vector< unsigned char > datagramm)
Definition: sick_generic_field_mon.cpp:242
sick_scan::SickScanFieldMonSingleton::parseAsciiDatagram
int parseAsciiDatagram(std::vector< unsigned char > datagramm)
Parsing Ascii datagram.
Definition: sick_generic_field_mon.cpp:235
ros::NodeHandle


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Thu Sep 8 2022 02:30:19