sick_generic_imu.h
Go to the documentation of this file.
1 //
2 // Created by michael on 10/4/18.
3 //
4 
5 #ifndef SICK_SCAN_SICK_GENERIC_IMU_H
6 #define SICK_SCAN_SICK_GENERIC_IMU_H
7 
8 /*
9  * Copyright (C) 2018, Ing.-Buero Dr. Michael Lehning, Hildesheim
10  * Copyright (C) 2018, SICK AG, Waldkirch
11  * All rights reserved.
12  *
13  * Redistribution and use in source and binary forms, with or without
14  * modification, are permitted provided that the following conditions are met:
15  *
16  * * Redistributions of source code must retain the above copyright
17  * notice, this list of conditions and the following disclaimer.
18  * * Redistributions in binary form must reproduce the above copyright
19  * notice, this list of conditions and the following disclaimer in the
20  * documentation and/or other materials provided with the distribution.
21  * * Neither the name of Osnabrück University nor the names of its
22  * contributors may be used to endorse or promote products derived from
23  * this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
26  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
27  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
28  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
29  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
30  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
31  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
32  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
33  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
34  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * Created on: 28th May 2018
38  *
39  * Authors:
40  * Michael Lehning <michael.lehning@lehning.de>
41  *
42  */
43 
44 #include <stdio.h>
45 #include <stdlib.h>
46 #include <string>
47 #include <string.h>
48 #include <vector>
49 
50 #include <ros/ros.h>
51 #include <sensor_msgs/LaserScan.h>
52 #include <sensor_msgs/PointCloud.h>
53 #include <sensor_msgs/PointCloud2.h>
54 #include <std_msgs/String.h>
55 
59 #include <sick_scan/RadarScan.h> // generated by msg-generator
60 
61 #ifndef _MSC_VER
62 
63 #include <dynamic_reconfigure/server.h>
64 #include <sick_scan/SickScanConfig.h>
65 
66 #endif
67 
70 #include <ros/ros.h>
71 #include <sensor_msgs/Imu.h>
72 #include "softwarePLL.h"
73 
74 namespace sick_scan
75 {
76 
77 
79  {
80  public:
81  UINT64 TimeStamp() const
82  { return timeStamp; }
83 
84  void TimeStamp(UINT64 val)
85  { timeStamp = val; }
86 
87  float QuaternionX() const
88  { return quaternionX; }
89 
90  void QuaternionX(float val)
91  { quaternionX = val; }
92 
93  float QuaternionY() const
94  { return quaternionY; }
95 
96  void QuaternionY(float val)
97  { quaternionY = val; }
98 
99  float QuaternionZ() const
100  { return quaternionZ; }
101 
102  void QuaternionZ(float val)
103  { quaternionZ = val; }
104 
105  float QuaternionW() const
106  { return quaternionW; }
107 
108  void QuaternionW(float val)
109  { quaternionW = val; }
110 
111  float QuaternionAccuracy() const
112  { return quaternionAccuracy; }
113 
114  void QuaternionAccuracy(float val)
115  { quaternionAccuracy = val; }
116 
117 
118  float AngularVelocityX() const
119  { return velocityX; }
120 
121  void AngularVelocityX(float val)
122  { velocityX = val; }
123 
124  float AngularVelocityY() const
125  { return velocityY; }
126 
127  void AngularVelocityY(float val)
128  { velocityY = val; }
129 
130  float AngularVelocityZ() const
131  { return velocityZ; }
132 
133  void AngularVelocityZ(float val)
134  { velocityZ = val; }
135 
137  { return velocityReliability; }
138 
140  { velocityReliability = val; }
141 
142  float LinearAccelerationX() const
143  { return linearAccelerationX; }
144 
145  void LinearAccelerationX(float val)
146  { linearAccelerationX = val; }
147 
148  float LinearAccelerationY() const
149  { return linearAccelerationY; }
150 
151  void LinearAccelerationY(float val)
152  { linearAccelerationY = val; }
153 
154  float LinearAccelerationZ() const
155  { return linearAccelerationZ; }
156 
157  void LinearAccelerationZ(float val)
158  { linearAccelerationZ = val; }
159 
162 
165 
166  private:
168  float quaternionX;
169  float quaternionY;
170  float quaternionZ;
171  float quaternionW;
173  float velocityX;
174  float velocityY;
175  float velocityZ;
181 
182  };
183 
185  {
186  public:
188  {
189  commonPtr = commonPtr_;
190  }
191 
192  bool isImuDatagram(char *datagram, size_t datagram_length);
193 
194  bool isImuBinaryDatagram(char *datagram, size_t datagram_length);
195 
196  bool isImuAsciiDatagram(char *datagram, size_t datagram_length);
197 
198  bool isImuAckDatagram(char *datagram, size_t datagram_length);
199 
200  int parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol);
201 
202  int parseAsciiDatagram(char *datagram, size_t datagram_length, SickScanImuValue *imValuePtr);
203 
204  int parseBinaryDatagram(char *datagram, size_t datagram_length, SickScanImuValue *imValuePtr);
205 
206  static void imuParserTest();
207 
208  static void quaternion2rpyTest(); // test for converting quaternion to rpy
209 
210  double simpleFmodTwoPi(double angle);
211 
212  private:
214  bool emul;
215  };
216 
217 } /* namespace sick_scan */
218 
219 
220 #endif //SICK_SCAN_SICK_GENERIC_IMU_H
uint16_t UINT16
void LinearAccelerationReliability(UINT16 val)
uint32_t UINT32
SickScanCommon * commonPtr
void LinearAccelerationX(float val)
void QuaternionAccuracy(float val)
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
void LinearAccelerationY(float val)
void LinearAccelerationZ(float val)
void AngularVelocityReliability(UINT16 val)
UINT16 AngularVelocityReliability() const
UINT16 LinearAccelerationReliability() const
uint64_t UINT64
SickScanImu(SickScanCommon *commonPtr_)


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Wed Sep 7 2022 02:25:06