sick_generic_radar.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_RADAR_H_
38 #define SICK_GENERIC_RADAR_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 
55 #include <sick_scan/RadarScan.h> // generated by msg-generator
56 
57 #ifndef _MSC_VER
58 
59 #include <dynamic_reconfigure/server.h>
60 #include <sick_scan/SickScanConfig.h>
61 
62 #endif
63 
66 
67 namespace sick_scan
68 {
69 
71  {
72  public:
73  float Dist() const
74  { return dist; }
75 
76  void Dist(float val)
77  { dist = val; }
78 
79  float Azimuth() const
80  { return azimuth; }
81 
82  void Azimuth(float val)
83  { azimuth = val; }
84 
85  float Vrad() const
86  { return vrad; }
87 
88  void Vrad(float val)
89  { vrad = val; }
90 
91  float Ampl() const
92  { return ampl; }
93 
94  void Ampl(float val)
95  { ampl = val; }
96 
97  int Mode() const
98  { return mode; }
99 
100  void Mode(int val)
101  { mode = val; }
102 
103  private:
104  float dist;
105  float azimuth;
106  float vrad;
107  float ampl;
108  int mode;
109  };
110 
112  {
113  public:
114  float P3Dx() const
115  { return p3Dx; }
116 
117  void P3Dx(float val)
118  { p3Dx = val; }
119 
120  float P3Dy() const
121  { return p3Dy; }
122 
123  void P3Dy(float val)
124  { p3Dy = val; }
125 
126  float V3Dx() const
127  { return v3Dx; }
128 
129  void V3Dx(float val)
130  { v3Dx = val; }
131 
132  float V3Dy() const
133  { return v3Dy; }
134 
135  void V3Dy(float val)
136  { v3Dy = val; }
137 
138  float ObjLength() const
139  { return objLength; }
140 
141  void ObjLength(float val)
142  { objLength = val; }
143 
144  int ObjId() const
145  { return objId; }
146 
147  void ObjId(int val)
148  { objId = val; }
149 
150  private:
151  float p3Dx;
152  float p3Dy;
153  float v3Dx;
154  float v3Dy;
155  float objLength;
156  int objId;
157  };
158 
159 
161  {
162  private:
163  /* Here will be the instance stored. */
165 
166  /* Private constructor to prevent instancing. */
168 
169  void simulateAsciiDatagramFromFile(unsigned char *receiveBuffer, int *actual_length, std::string filePattern);
170 
171  bool emul = false;
172 
177 
179 
180  public:
181  /* Static access method. */
183 
184  void setEmulation(bool _emul);
185 
186  bool getEmulation(void);
187 
188  int parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol);
189 
190  int parseAsciiDatagram(char *datagram, size_t datagram_length, sick_scan::RadarScan *msgPtr,
191  std::vector<SickScanRadarObject> &objectList,
192  std::vector<SickScanRadarRawTarget> &rawTargetList); /* , SickScanConfig &config, */ // sensor_msgs::LaserScan &msg, int &numEchos, int &echoMask);
193  void simulateAsciiDatagram(unsigned char *receiveBuffer, int *actual_length);
194  };
195 
196 
197 #if 0
198  class SickScanRadar
199  {
200  public:
201  SickScanRadar(SickScanCommon *commonPtr_)
202  {
203  commonPtr = commonPtr_;
204  }
205  void setEmulation(bool _emul);
206  bool getEmulation(void);
207  int parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol);
208  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);
209  void simulateAsciiDatagram(unsigned char * receiveBuffer, int* actual_length);
210  private:
211 // SickScanCommon *commonPtr;
212  void simulateAsciiDatagramFromFile(unsigned char *receiveBuffer, int *actual_length, std::string filePattern);
213  bool emul;
214  };
215 #endif
216 
217 } /* namespace sick_scan */
218 #endif // SICK_GENERIC_RADAR_H_
sick_scan::SickScanRadarRawTarget::mode
int mode
Definition: sick_generic_radar.h:108
sick_scan::SickScanRadarObject::P3Dx
float P3Dx() const
Definition: sick_generic_radar.h:114
sick_scan::SickScanRadarSingleton::getEmulation
bool getEmulation(void)
Definition: sick_generic_radar.cpp:166
sick_scan::SickScanRadarSingleton::parseAsciiDatagram
int parseAsciiDatagram(char *datagram, size_t datagram_length, sick_scan::RadarScan *msgPtr, std::vector< SickScanRadarObject > &objectList, std::vector< SickScanRadarRawTarget > &rawTargetList)
Parsing Ascii datagram.
Definition: sick_generic_radar.cpp:181
sick_scan::SickScanRadarRawTarget::dist
float dist
Definition: sick_generic_radar.h:104
ros::Publisher
sick_scan::SickScanRadarRawTarget::Vrad
float Vrad() const
Definition: sick_generic_radar.h:85
sick_scan::SickScanRadarRawTarget::ampl
float ampl
Definition: sick_generic_radar.h:107
sick_scan::SickScanRadarSingleton::SickScanRadarSingleton
SickScanRadarSingleton()
Definition: sick_generic_radar.cpp:88
sick_scan::SickScanRadarObject::P3Dy
void P3Dy(float val)
Definition: sick_generic_radar.h:123
sick_scan::SickScanRadarRawTarget::Dist
void Dist(float val)
Definition: sick_generic_radar.h:76
sick_scan::SickScanRadarSingleton::cloud_radar_track_pub_
ros::Publisher cloud_radar_track_pub_
Definition: sick_generic_radar.h:175
sick_scan::SickScanRadarObject::ObjId
void ObjId(int val)
Definition: sick_generic_radar.h:147
ros.h
sick_scan::SickScanRadarObject::v3Dy
float v3Dy
Definition: sick_generic_radar.h:154
sick_scan::SickScanRadarSingleton::simulateAsciiDatagram
void simulateAsciiDatagram(unsigned char *receiveBuffer, int *actual_length)
Definition: sick_generic_radar.cpp:678
sick_scan::SickScanRadarObject::V3Dy
void V3Dy(float val)
Definition: sick_generic_radar.h:135
sick_scan::SickScanCommon
Definition: sick_scan_common.h:86
sick_scan::SickScanRadarSingleton::emul
bool emul
Definition: sick_generic_radar.h:171
sick_scan::SickScanRadarObject::objId
int objId
Definition: sick_generic_radar.h:156
sick_scan::SickScanRadarRawTarget::azimuth
float azimuth
Definition: sick_generic_radar.h:105
sick_scan::SickScanRadarObject::ObjId
int ObjId() const
Definition: sick_generic_radar.h:144
sick_scan::SickScanRadarSingleton
Definition: sick_generic_radar.h:160
sick_scan::SickScanRadarSingleton::setEmulation
void setEmulation(bool _emul)
Definition: sick_generic_radar.cpp:161
publisher.h
diagnostic_updater.h
sick_scan::SickScanRadarRawTarget::Ampl
void Ampl(float val)
Definition: sick_generic_radar.h:94
sick_scan::SickScanRadarObject::ObjLength
void ObjLength(float val)
Definition: sick_generic_radar.h:141
sick_scan::SickScanRadarObject::P3Dy
float P3Dy() const
Definition: sick_generic_radar.h:120
sick_scan::SickScanRadarRawTarget::Ampl
float Ampl() const
Definition: sick_generic_radar.h:91
sick_scan::SickScanRadarSingleton::instance
static SickScanRadarSingleton * instance
Definition: sick_generic_radar.h:164
sick_scan::SickScanRadarSingleton::chatter_pub
ros::Publisher chatter_pub
Definition: sick_generic_radar.h:178
sick_generic_parser.h
sick_scan::SickScanRadarSingleton::parseDatagram
int parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol)
Definition: sick_generic_radar.cpp:1044
sick_scan::SickScanRadarRawTarget::Mode
int Mode() const
Definition: sick_generic_radar.h:97
sick_scan::SickScanRadarRawTarget
Definition: sick_generic_radar.h:70
sick_scan::SickScanRadarObject::p3Dx
float p3Dx
Definition: sick_generic_radar.h:151
sick_scan::SickScanRadarSingleton::getInstance
static SickScanRadarSingleton * getInstance()
Definition: sick_generic_radar.cpp:78
sick_scan
Definition: abstract_parser.cpp:50
sick_scan::SickScanRadarRawTarget::Mode
void Mode(int val)
Definition: sick_generic_radar.h:100
sick_scan::SickScanRadarObject::v3Dx
float v3Dx
Definition: sick_generic_radar.h:153
sick_scan::SickScanRadarRawTarget::Azimuth
float Azimuth() const
Definition: sick_generic_radar.h:79
sick_scan::SickScanRadarRawTarget::Dist
float Dist() const
Definition: sick_generic_radar.h:73
sick_scan::SickScanRadarRawTarget::vrad
float vrad
Definition: sick_generic_radar.h:106
sick_scan::SickScanRadarObject
Definition: sick_generic_radar.h:111
sick_scan::SickScanRadarObject::P3Dx
void P3Dx(float val)
Definition: sick_generic_radar.h:117
sick_scan::SickScanRadarObject::ObjLength
float ObjLength() const
Definition: sick_generic_radar.h:138
ros::Time
sick_scan::SickScanRadarObject::V3Dx
float V3Dx() const
Definition: sick_generic_radar.h:126
sick_scan::SickScanRadarObject::p3Dy
float p3Dy
Definition: sick_generic_radar.h:152
sick_scan::SickScanRadarSingleton::cloud_radar_rawtarget_pub_
ros::Publisher cloud_radar_rawtarget_pub_
Definition: sick_generic_radar.h:174
sick_scan::SickScanRadarRawTarget::Azimuth
void Azimuth(float val)
Definition: sick_generic_radar.h:82
sick_scan_common_nw.h
sick_scan::SickScanRadarObject::V3Dy
float V3Dy() const
Definition: sick_generic_radar.h:132
sick_scan::SickScanRadarSingleton::radarScan_pub_
ros::Publisher radarScan_pub_
Definition: sick_generic_radar.h:176
sick_scan::SickScanRadarObject::V3Dx
void V3Dx(float val)
Definition: sick_generic_radar.h:129
sick_scan::SickScanRadarSingleton::simulateAsciiDatagramFromFile
void simulateAsciiDatagramFromFile(unsigned char *receiveBuffer, int *actual_length, std::string filePattern)
Definition: sick_generic_radar.cpp:978
sick_scan::SickScanRadarObject::objLength
float objLength
Definition: sick_generic_radar.h:155
sick_scan::SickScanRadarSingleton::nh_
ros::NodeHandle nh_
Definition: sick_generic_radar.h:173
ros::NodeHandle
sick_scan::SickScanRadarRawTarget::Vrad
void Vrad(float val)
Definition: sick_generic_radar.h:88


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