1
- /* ******************************************************************************
2
- * Copyright (c) 2016, Hitachi-LG Data Storage
3
- * Copyright (c) 2017, ROBOTIS
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 notice, this
10
- * list of conditions and the following disclaimer.
11
- *
12
- * * Redistributions in binary form must reproduce the above copyright notice,
13
- * this list of conditions and the following disclaimer in the documentation
14
- * and/or other materials provided with the distribution.
15
- *
16
- * * Neither the name of the copyright holder nor the names of its
17
- * contributors may be used to endorse or promote products derived from
18
- * this software without specific prior written permission.
19
- *
20
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30
- *******************************************************************************/
31
-
32
- /* Authors: SP Kong, JH Yang, Pyo */
33
- /* maintainer: Pyo */
34
-
35
- #include " lds_driver.h"
1
+ // Copyright (c) 2016, Hitachi-LG Data Storage
2
+ // Copyright (c) 2017, ROBOTIS
3
+ //
4
+ // Use of this source code is governed by a BSD-style
5
+ // license that can be found in the LICENSE file or at
6
+ // https://developers.google.com/open-source/licenses/bsd
7
+ //
8
+ // Redistribution and use in source and binary forms, with or without
9
+ // modification, are permitted provided that the following conditions are met:
10
+ //
11
+ // * Redistributions of source code must retain the above copyright
12
+ // notice, this list of conditions and the following disclaimer.
13
+ //
14
+ // * Redistributions in binary form must reproduce the above copyright
15
+ // notice, this list of conditions and the following disclaimer in the
16
+ // documentation and/or other materials provided with the distribution.
17
+ //
18
+ // * Neither the name of the {copyright_holder} nor the names of its
19
+ // contributors may be used to endorse or promote products derived from
20
+ // this software without specific prior written permission.
21
+ //
22
+ // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
23
+ // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
24
+ // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
25
+ // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
26
+ // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
27
+ // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
28
+ // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
29
+ // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
30
+ // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
31
+ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32
+ // POSSIBILITY OF SUCH DAMAGE.
33
+
34
+ /* Authors: SP Kong, JH Yang, Pyo */
35
+ /* maintainer: Pyo */
36
+
37
+ #include " ../applications/lds_driver/lds_driver.hpp"
36
38
37
39
namespace lds
38
40
{
39
- LFCDLaser::LFCDLaser (const std::string& port, uint32_t baud_rate, boost::asio::io_service& io)
40
- : port_(port), baud_rate_(baud_rate), shutting_down_(false ), serial_(io, port_)
41
+ LFCDLaser::LFCDLaser (const std::string & port, uint32_t baud_rate, boost::asio::io_service & io)
42
+ : port_(port), baud_rate_(baud_rate), shutting_down_(false ), serial_(io, port_)
41
43
{
42
44
serial_.set_option (boost::asio::serial_port_base::baud_rate (baud_rate_));
43
45
@@ -58,31 +60,25 @@ void LFCDLaser::poll()
58
60
boost::array<uint8_t , 2520 > raw_bytes;
59
61
uint8_t good_sets = 0 ;
60
62
uint32_t motor_speed = 0 ;
61
- rpms= 0 ;
63
+ rpms = 0 ;
62
64
int index;
63
65
64
- while (!shutting_down_ && !got_scan)
65
- {
66
+ while (!shutting_down_ && !got_scan) {
66
67
// Wait until first data sync of frame: 0xFA, 0xA0
67
- boost::asio::read (serial_, boost::asio::buffer (&raw_bytes[start_count],1 ));
68
+ boost::asio::read (serial_, boost::asio::buffer (&raw_bytes[start_count], 1 ));
68
69
69
- if (start_count == 0 )
70
- {
71
- if (raw_bytes[start_count] == 0xFA )
72
- {
70
+ if (start_count == 0 ) {
71
+ if (raw_bytes[start_count] == 0xFA ) {
73
72
start_count = 1 ;
74
73
}
75
- }
76
- else if (start_count == 1 )
77
- {
78
- if (raw_bytes[start_count] == 0xA0 )
79
- {
74
+ } else if (start_count == 1 ) {
75
+ if (raw_bytes[start_count] == 0xA0 ) {
80
76
start_count = 0 ;
81
77
82
78
// Now that entire start sequence has been found, read in the rest of the message
83
79
got_scan = true ;
84
80
85
- boost::asio::read (serial_,boost::asio::buffer (&raw_bytes[2 ], 2518 ));
81
+ boost::asio::read (serial_, boost::asio::buffer (&raw_bytes[2 ], 2518 ));
86
82
87
83
// scan->angle_min = 0.0;
88
84
// scan->angle_max = 2.0*M_PI;
@@ -92,51 +88,48 @@ void LFCDLaser::poll()
92
88
// scan->ranges.resize(360);
93
89
// scan->intensities.resize(360);
94
90
95
- // read data in sets of 6
96
- for (uint16_t i = 0 ; i < raw_bytes.size (); i=i+42 )
97
- {
98
- if (raw_bytes[i] == 0xFA && raw_bytes[i+1 ] == (0xA0 + i / 42 )) // && CRC check
99
- {
91
+ // read data in sets of 6
92
+
93
+ for (uint16_t i = 0 ; i < raw_bytes.size (); i = i + 42 ) {
94
+ if (raw_bytes[i] == 0xFA && raw_bytes[i + 1 ] == (0xA0 + i / 42 )) {
100
95
good_sets++;
101
- motor_speed += (raw_bytes[i+ 3 ] << 8 ) + raw_bytes[i+ 2 ]; // accumulate count for avg. time increment
102
- rpms= (raw_bytes[i+ 3 ]<< 8 | raw_bytes[i+ 2 ])/ 10 ;
96
+ motor_speed += (raw_bytes[i + 3 ] << 8 ) + raw_bytes[i + 2 ];
97
+ rpms = (raw_bytes[i + 3 ] << 8 | raw_bytes[i + 2 ]) / 10 ;
103
98
104
- for (uint16_t j = i+4 ; j < i+40 ; j=j+6 )
105
- {
106
- index = 6 *(i/42 ) + (j-4 -i)/6 ;
99
+ for (uint16_t j = i + 4 ; j < i + 40 ; j = j + 6 ) {
100
+ index = 6 * (i / 42 ) + (j - 4 - i) / 6 ;
107
101
108
102
// Four bytes per reading
109
103
uint8_t byte0 = raw_bytes[j];
110
- uint8_t byte1 = raw_bytes[j+ 1 ];
111
- uint8_t byte2 = raw_bytes[j+ 2 ];
112
- uint8_t byte3 = raw_bytes[j+ 3 ];
104
+ uint8_t byte1 = raw_bytes[j + 1 ];
105
+ uint8_t byte2 = raw_bytes[j + 2 ];
106
+ uint8_t byte3 = raw_bytes[j + 3 ];
113
107
114
108
// Remaining bits are the range in mm
115
109
uint16_t intensity = (byte1 << 8 ) + byte0;
116
110
117
- // Last two bytes represent the uncertanty or intensity, might also be pixel area of target...
111
+ // Last two bytes represent the uncertanty or intensity,
112
+ // might also be pixel area of target...
118
113
// uint16_t intensity = (byte3 << 8) + byte2;
119
114
uint16_t range = (byte3 << 8 ) + byte2;
120
115
121
116
// scan->ranges[359-index] = range / 1000.0;
122
117
// scan->intensities[359-index] = intensity;
123
- printf (" r[%d]=%f," ,359 - index, range / 1000.0 );
118
+ printf (" r[%d]=%f," , 359 - index, range / 1000.0 );
124
119
}
125
120
}
126
121
}
127
122
128
123
// scan->time_increment = motor_speed/good_sets/1e8;
129
- }
130
- else
131
- {
124
+ } else {
132
125
start_count = 0 ;
133
126
}
134
127
}
135
128
}
136
129
}
137
- }
130
+ } // namespace lds
138
131
139
- int main (int argc, char **argv)
132
+ int main (int argc, char ** argv)
140
133
{
141
134
std::string port;
142
135
int baud_rate;
@@ -145,20 +138,16 @@ int main(int argc, char **argv)
145
138
baud_rate = 230400 ;
146
139
boost::asio::io_service io;
147
140
148
- try
149
- {
141
+ try {
150
142
lds::LFCDLaser laser (port, baud_rate, io);
151
143
152
- while (1 )
153
- {
144
+ while (1 ) {
154
145
laser.poll ();
155
146
}
156
147
laser.close ();
157
148
158
149
return 0 ;
159
- }
160
- catch (boost::system::system_error ex)
161
- {
150
+ } catch (boost::system::system_error ex) {
162
151
printf (" An exception was thrown: %s" , ex.what ());
163
152
return -1 ;
164
153
}
0 commit comments