編集履歴

質問編集履歴

解決したので、経緯を追記

2020/02/28 06:25

投稿

yoddy

スコア10

test CHANGED Viewed

File without changes

test CHANGED Viewed

@@ -315,3 +315,231 @@
  - 無視するheader部分の行数が間違っていたので、`ignore_line > 9` を、` ignore_line > 8` に変更
  - `line = f.readline()` を削除
+### 修正：解決編（2020/2/28 14:55）
+yuki23様のご指摘にあった、**出力されている角度が360を超えている問題**を解決すべく、元々のC++のソースと、自身が改変したC++を見比べ、可能な限り元の状態を維持しつつ、必要なデータを加えていくように修正を行いました。
+その結果、360度以上のデータが出力されることはなくなりました。
+そのままの形では、依然θと距離しか出力されてませんので、上述の修正後ソースをさらに修正し、Ⅹ座標とY座標に変換したところ、以下のように、部屋の形がきちんと描画されました。
+![点群データ](71d3adec7886822f2bcc9bf131d80e4c.jpeg)
+元のソース、修正しておかしくなっていたソース、最終的に正常に動いたソースは以下の通りです。
+[オリジナルC++ソース(抜粋）]
+```C++
+    while (1) {
+        rplidar_response_measurement_node_hq_t nodes[8192];
+        size_t   count = _countof(nodes);
+        op_result = drv->grabScanDataHq(nodes, count);
+        if (IS_OK(op_result)) {
+            drv->ascendScanData(nodes, count);
+            for (int pos = 0; pos < (int)count ; ++pos) {
+                printf("%s theta: %03.2f Dist: %08.2f Q: %d \n",
+                    (nodes[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ?"S ":"  ",
+                    (nodes[pos].angle_z_q14 * 90.f / (1 << 14)),
+                    nodes[pos].dist_mm_q2/4.0f,
+                    nodes[pos].quality);
+            }
+        }
+        if (ctrl_c_pressed){
+            break;
+        }
+    }
+```
+[修正失敗版C++ソース(抜粋）]
+```C++
+    while (1) {
+        rplidar_response_measurement_node_t nodes[8192];
+        size_t   count = _countof(nodes);
+        op_result = drv->grabScanData(nodes, count);
+        if (IS_OK(op_result)) {
+            drv->ascendScanData(nodes, count);
+            for (int pos = 0; pos < (int)count ; ++pos) {
+                clock_gettime(CLOCK_REALTIME, &ts);
+                std::strftime(buf, sizeof buf, "%F %T", std::localtime(&ts.tv_sec));
+                millisec = ts.tv_nsec / 100000000;
+                theta = (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
+                dist = nodes[pos].distance_q2/4.0f;
+                outfile << buf
+                        << ":"
+                        << millisec
+                        << ","
+                        << std::to_string(theta)
+                        << ","
+                        << std::to_string(dist)
+                        << "\n";
+                printf("%s.%ld,%03.2f,%08.2f\n",
+                        buf,
+                        millisec,
+                        (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f,
+                        nodes[pos].distance_q2/4.0f);
+            }
+        }
+        if (ctrl_c_pressed){
+            break;
+        }
+    }
+```
+[最終版C++ソース(抜粋）]
+```C++
+    while (1) {
+        rplidar_response_measurement_node_hq_t nodes[8192];
+        size_t   count = _countof(nodes);
+        op_result = drv->grabScanDataHq(nodes, count);
+        if (IS_OK(op_result)) {
+            drv->ascendScanData(nodes, count);
+            for (int pos = 0; pos < (int)count ; ++pos) {
+				clock_gettime(CLOCK_REALTIME, &ts);
+				std::strftime(buf, sizeof buf, "%F %T", std::localtime(&ts.tv_sec));
+				millisec = ts.tv_nsec / 100000000;
+                printf("%s.%ld,%03.2f,%08.2f\n",
+                    buf,
+					millsec,
+                    (nodes[pos].angle_z_q14 * 90.f / (1 << 14)),
+                    nodes[pos].dist_mm_q2/4.0f);
+            }
+        }
+        if (ctrl_c_pressed){
+            break;
+        }
+    }
+```
+whileブロックに入ってすぐの、` rplidar_response_measurement_node_hq_t nodes[8192]; `というところで、hqがついたりつかなかったりしていますが、
+修正失敗版を最初にmakeした時、「hqありは使えない、ないのを使え」みたいなエラーをはかれたため、修正し、その後発生するエラーに対応していったら、結果的に上述のようになった次第です。
+今回、改めてSDKを落とすと、バージョンが上がっており、それが原因か、単に以前が自分のミスだったのかは不明ですが、するっとmakeできてしまい、動くに至りました。
+もっと細かく、何がどうしてエラーだったか、ご報告するのが本来の在り方だとは承知しておりますが、いかんせん、この問題とはこの1年、何度か中断しつつ付き合ってきたため、最初の頃の記憶があいまいで、説明のしようがありません。
+ご理解、ご容赦の程よろしくお願いいたします。
+### 謝辞
+最後に、皆様のご助言のおかげで、無事にきれいな点群（と言っても16000点前後/秒程度ですが、これはLidarの限界です）の取得に成功しましたこと、心より感謝いたします。
+ずっと一人で作業をしていたら、動いているからと、元のソースを見直すことを頑なに拒んでいたかもしれません。
+こちらで、私などよりもはるかに知見のある皆様の意見に、素直に耳を傾けることができ、解決することができました。
+本当にありがとうございました。

ファイルアップロード完了とソース修正の通知を追加

2020/02/28 06:25

投稿

yoddy

スコア10

test CHANGED Viewed

File without changes

test CHANGED Viewed

@@ -301,3 +301,17 @@
 ```
+### 修正と追加（2020/2/28 11:05）
+Githubに[専用のリポジトリ](https://github.com/taobody/RPLidar_A3)を作って、Pythonのソースと部屋の中で取得した点群データをアップしました。
+ソースも一部修正をしています。
+具体的には、if ブロック内の以下の点です。
+ - 無視するheader部分の行数が間違っていたので、`ignore_line > 9` を、` ignore_line > 8` に変更
+ - `line = f.readline()` を削除

掲載する修正後ソースが間違っていたのだ、訂正して再掲。

2020/02/28 02:20

投稿

yoddy

スコア10

test CHANGED Viewed

File without changes

test CHANGED Viewed

@@ -221,3 +221,83 @@
 何をどう書いていいかもよくわからないまま、思いつくことを書いてまいりましたが、ほかに必要な情報がありましたら、ご指摘いただければ幸いです。
 よろしくお願いいたします。
+### 修正（2020/2/27 16:30）
+修正後のソースで、角度をラジアンに変換して、三角関数の引数のする箇所を間違っていました。
+ただ、この点に関して、ラジアンに変換せずにthetaを直接使用するように改変したかといいうと、
+1. Lidarの仕様書に、thetaが角度として図示されており、
+1. 参考ソースで変換した際に点群が前述の画像の通り、歪んでいたため
+です。
+最初に使用した修正後ソースは以下の通りです。
+[修正後ソース]
+```Python
+#!/usr/bin/python3
+import math
+file = "lidarlog2.txt" # Lidarログ出力ファイル
+f = open(file, 'r')
+line = f.readline()
+ignore_line = 1
+for line in f:
+    if ignore_line > 9: # headerを無視
+        line = line.strip().rstrip('\n')
+        line_data = line.split(",")
+        s = float(line_data[1])
+        c = float(line_data[2])
+        print(s)
+        r = s * math.pi /180
+        a = c * math.cos(r) # X座標
+        b = c * math.sin(r) # Y座標
+        print(line_data[0] + " " + str(a) + " " + str(b))
+        line = f.readline()
+    else:
+        ignore_line = ignore_line + 1
+f.close()
+```

Slamtec社の社名を修正（Slamtech → Slamtec）

2020/02/27 07:41

投稿

yoddy

スコア10

test CHANGED Viewed

File without changes

test CHANGED Viewed

@@ -4,7 +4,7 @@
 初めて質問させていただきます。
-現在、Slamtech社のRPLidar A3(https://www.slamtec.com/en/Lidar/A3)を用いた、マッピングシステムを、RaspberryPi3 B+ にて作成しております。
+現在、Slamtec社のRPLidar A3(https://www.slamtec.com/en/Lidar/A3)を用いた、マッピングシステムを、RaspberryPi3 B+ にて作成しております。
 SDKが公開されているので、その中のultra_simpleというアプリケーションでテストを行い、C++には不慣れながらも、出力データに日付と時刻を付与するところまではできました。