質問編集履歴
4
解決したので、経緯を追記
title
CHANGED
|
File without changes
|
body
CHANGED
|
@@ -156,4 +156,118 @@
|
|
|
156
156
|
ソースも一部修正をしています。
|
|
157
157
|
具体的には、if ブロック内の以下の点です。
|
|
158
158
|
- 無視するheader部分の行数が間違っていたので、`ignore_line > 9` を、` ignore_line > 8` に変更
|
|
159
|
-
- `line = f.readline()` を削除
|
|
159
|
+
- `line = f.readline()` を削除
|
|
160
|
+
|
|
161
|
+
### 修正：解決編（2020/2/28 14:55）
|
|
162
|
+
yuki23様のご指摘にあった、**出力されている角度が360を超えている問題**を解決すべく、元々のC++のソースと、自身が改変したC++を見比べ、可能な限り元の状態を維持しつつ、必要なデータを加えていくように修正を行いました。
|
|
163
|
+
その結果、360度以上のデータが出力されることはなくなりました。
|
|
164
|
+
そのままの形では、依然θと距離しか出力されてませんので、上述の修正後ソースをさらに修正し、Ⅹ座標とY座標に変換したところ、以下のように、部屋の形がきちんと描画されました。
|
|
165
|
+
|
|
166
|
+
|
|
167
|
+
元のソース、修正しておかしくなっていたソース、最終的に正常に動いたソースは以下の通りです。
|
|
168
|
+
|
|
169
|
+
[オリジナルC++ソース(抜粋）]
|
|
170
|
+
```C++
|
|
171
|
+
while (1) {
|
|
172
|
+
rplidar_response_measurement_node_hq_t nodes[8192];
|
|
173
|
+
size_t count = _countof(nodes);
|
|
174
|
+
|
|
175
|
+
op_result = drv->grabScanDataHq(nodes, count);
|
|
176
|
+
if (IS_OK(op_result)) {
|
|
177
|
+
drv->ascendScanData(nodes, count);
|
|
178
|
+
for (int pos = 0; pos < (int)count ; ++pos) {
|
|
179
|
+
printf("%s theta: %03.2f Dist: %08.2f Q: %d \n",
|
|
180
|
+
(nodes[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ?"S ":" ",
|
|
181
|
+
(nodes[pos].angle_z_q14 * 90.f / (1 << 14)),
|
|
182
|
+
nodes[pos].dist_mm_q2/4.0f,
|
|
183
|
+
nodes[pos].quality);
|
|
184
|
+
}
|
|
185
|
+
}
|
|
186
|
+
|
|
187
|
+
if (ctrl_c_pressed){
|
|
188
|
+
break;
|
|
189
|
+
}
|
|
190
|
+
}
|
|
191
|
+
|
|
192
|
+
```
|
|
193
|
+
|
|
194
|
+
[修正失敗版C++ソース(抜粋）]
|
|
195
|
+
```C++
|
|
196
|
+
while (1) {
|
|
197
|
+
rplidar_response_measurement_node_t nodes[8192];
|
|
198
|
+
size_t count = _countof(nodes);
|
|
199
|
+
|
|
200
|
+
op_result = drv->grabScanData(nodes, count);
|
|
201
|
+
|
|
202
|
+
if (IS_OK(op_result)) {
|
|
203
|
+
drv->ascendScanData(nodes, count);
|
|
204
|
+
for (int pos = 0; pos < (int)count ; ++pos) {
|
|
205
|
+
clock_gettime(CLOCK_REALTIME, &ts);
|
|
206
|
+
std::strftime(buf, sizeof buf, "%F %T", std::localtime(&ts.tv_sec));
|
|
207
|
+
|
|
208
|
+
millisec = ts.tv_nsec / 100000000;
|
|
209
|
+
theta = (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
|
|
210
|
+
dist = nodes[pos].distance_q2/4.0f;
|
|
211
|
+
|
|
212
|
+
outfile << buf
|
|
213
|
+
<< ":"
|
|
214
|
+
<< millisec
|
|
215
|
+
<< ","
|
|
216
|
+
<< std::to_string(theta)
|
|
217
|
+
<< ","
|
|
218
|
+
<< std::to_string(dist)
|
|
219
|
+
<< "\n";
|
|
220
|
+
|
|
221
|
+
printf("%s.%ld,%03.2f,%08.2f\n",
|
|
222
|
+
buf,
|
|
223
|
+
millisec,
|
|
224
|
+
(nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f,
|
|
225
|
+
nodes[pos].distance_q2/4.0f);
|
|
226
|
+
}
|
|
227
|
+
}
|
|
228
|
+
|
|
229
|
+
if (ctrl_c_pressed){
|
|
230
|
+
break;
|
|
231
|
+
}
|
|
232
|
+
}
|
|
233
|
+
|
|
234
|
+
```
|
|
235
|
+
[最終版C++ソース(抜粋）]
|
|
236
|
+
```C++
|
|
237
|
+
while (1) {
|
|
238
|
+
rplidar_response_measurement_node_hq_t nodes[8192];
|
|
239
|
+
size_t count = _countof(nodes);
|
|
240
|
+
|
|
241
|
+
op_result = drv->grabScanDataHq(nodes, count);
|
|
242
|
+
if (IS_OK(op_result)) {
|
|
243
|
+
drv->ascendScanData(nodes, count);
|
|
244
|
+
for (int pos = 0; pos < (int)count ; ++pos) {
|
|
245
|
+
clock_gettime(CLOCK_REALTIME, &ts);
|
|
246
|
+
std::strftime(buf, sizeof buf, "%F %T", std::localtime(&ts.tv_sec));
|
|
247
|
+
millisec = ts.tv_nsec / 100000000;
|
|
248
|
+
printf("%s.%ld,%03.2f,%08.2f\n",
|
|
249
|
+
buf,
|
|
250
|
+
millsec,
|
|
251
|
+
(nodes[pos].angle_z_q14 * 90.f / (1 << 14)),
|
|
252
|
+
nodes[pos].dist_mm_q2/4.0f);
|
|
253
|
+
}
|
|
254
|
+
}
|
|
255
|
+
|
|
256
|
+
if (ctrl_c_pressed){
|
|
257
|
+
break;
|
|
258
|
+
}
|
|
259
|
+
}
|
|
260
|
+
|
|
261
|
+
```
|
|
262
|
+
|
|
263
|
+
whileブロックに入ってすぐの、` rplidar_response_measurement_node_hq_t nodes[8192]; `というところで、hqがついたりつかなかったりしていますが、
|
|
264
|
+
修正失敗版を最初にmakeした時、「hqありは使えない、ないのを使え」みたいなエラーをはかれたため、修正し、その後発生するエラーに対応していったら、結果的に上述のようになった次第です。
|
|
265
|
+
今回、改めてSDKを落とすと、バージョンが上がっており、それが原因か、単に以前が自分のミスだったのかは不明ですが、するっとmakeできてしまい、動くに至りました。
|
|
266
|
+
もっと細かく、何がどうしてエラーだったか、ご報告するのが本来の在り方だとは承知しておりますが、いかんせん、この問題とはこの1年、何度か中断しつつ付き合ってきたため、最初の頃の記憶があいまいで、説明のしようがありません。
|
|
267
|
+
ご理解、ご容赦の程よろしくお願いいたします。
|
|
268
|
+
|
|
269
|
+
### 謝辞
|
|
270
|
+
最後に、皆様のご助言のおかげで、無事にきれいな点群（と言っても16000点前後/秒程度ですが、これはLidarの限界です）の取得に成功しましたこと、心より感謝いたします。
|
|
271
|
+
ずっと一人で作業をしていたら、動いているからと、元のソースを見直すことを頑なに拒んでいたかもしれません。
|
|
272
|
+
こちらで、私などよりもはるかに知見のある皆様の意見に、素直に耳を傾けることができ、解決することができました。
|
|
273
|
+
本当にありがとうございました。
|
3
ファイルアップロード完了とソース修正の通知を追加
title
CHANGED
|
File without changes
|
body
CHANGED
|
@@ -149,4 +149,11 @@
|
|
|
149
149
|
|
|
150
150
|
f.close()
|
|
151
151
|
|
|
152
|
-
```
|
|
152
|
+
```
|
|
153
|
+
|
|
154
|
+
### 修正と追加（2020/2/28 11:05）
|
|
155
|
+
Githubに[専用のリポジトリ](https://github.com/taobody/RPLidar_A3)を作って、Pythonのソースと部屋の中で取得した点群データをアップしました。
|
|
156
|
+
ソースも一部修正をしています。
|
|
157
|
+
具体的には、if ブロック内の以下の点です。
|
|
158
|
+
- 無視するheader部分の行数が間違っていたので、`ignore_line > 9` を、` ignore_line > 8` に変更
|
|
159
|
+
- `line = f.readline()` を削除
|
2
掲載する修正後ソースが間違っていたのだ、訂正して再掲。
title
CHANGED
|
File without changes
|
body
CHANGED
|
@@ -109,4 +109,44 @@
|
|
|
109
109
|
|
|
110
110
|
### 最後に
|
|
111
111
|
何をどう書いていいかもよくわからないまま、思いつくことを書いてまいりましたが、ほかに必要な情報がありましたら、ご指摘いただければ幸いです。
|
|
112
|
-
よろしくお願いいたします。
|
|
112
|
+
よろしくお願いいたします。
|
|
113
|
+
|
|
114
|
+
### 修正（2020/2/27 16:30）
|
|
115
|
+
修正後のソースで、角度をラジアンに変換して、三角関数の引数のする箇所を間違っていました。
|
|
116
|
+
ただ、この点に関して、ラジアンに変換せずにthetaを直接使用するように改変したかといいうと、
|
|
117
|
+
1. Lidarの仕様書に、thetaが角度として図示されており、
|
|
118
|
+
1. 参考ソースで変換した際に点群が前述の画像の通り、歪んでいたため
|
|
119
|
+
|
|
120
|
+
です。
|
|
121
|
+
最初に使用した修正後ソースは以下の通りです。
|
|
122
|
+
|
|
123
|
+
[修正後ソース]
|
|
124
|
+
```Python
|
|
125
|
+
#!/usr/bin/python3
|
|
126
|
+
|
|
127
|
+
import math
|
|
128
|
+
|
|
129
|
+
file = "lidarlog2.txt" # Lidarログ出力ファイル
|
|
130
|
+
|
|
131
|
+
f = open(file, 'r')
|
|
132
|
+
line = f.readline()
|
|
133
|
+
ignore_line = 1
|
|
134
|
+
|
|
135
|
+
for line in f:
|
|
136
|
+
if ignore_line > 9: # headerを無視
|
|
137
|
+
line = line.strip().rstrip('\n')
|
|
138
|
+
line_data = line.split(",")
|
|
139
|
+
s = float(line_data[1])
|
|
140
|
+
c = float(line_data[2])
|
|
141
|
+
print(s)
|
|
142
|
+
r = s * math.pi /180
|
|
143
|
+
a = c * math.cos(r) # X座標
|
|
144
|
+
b = c * math.sin(r) # Y座標
|
|
145
|
+
print(line_data[0] + " " + str(a) + " " + str(b))
|
|
146
|
+
line = f.readline()
|
|
147
|
+
else:
|
|
148
|
+
ignore_line = ignore_line + 1
|
|
149
|
+
|
|
150
|
+
f.close()
|
|
151
|
+
|
|
152
|
+
```
|
1
Slamtec社の社名を修正（Slamtech → Slamtec）
title
CHANGED
|
File without changes
|
body
CHANGED
|
@@ -1,7 +1,7 @@
|
|
|
1
1
|
### 前提・実現したいこと
|
|
2
2
|
|
|
3
3
|
初めて質問させていただきます。
|
|
4
|
-
現在、
|
|
4
|
+
現在、Slamtec社のRPLidar A3(https://www.slamtec.com/en/Lidar/A3)を用いた、マッピングシステムを、RaspberryPi3 B+ にて作成しております。
|
|
5
5
|
SDKが公開されているので、その中のultra_simpleというアプリケーションでテストを行い、C++には不慣れながらも、出力データに日付と時刻を付与するところまではできました。
|
|
6
6
|
出力結果は以下の通りです。
|
|
7
7
|
|