ライシェン 32 ライン ライダーの ROS で fromRosMsg() を実行すると、「フィールド "強度" の一致が見つかりませんでした」というエラーが発生しました。そのとき、私はライシェン C32 レーダーに対応するフィールドがないと判断し、それを記録しました
。ブログ[学習記録] Lei ROS で fromRosMsg() を実行すると、Shen 32 ライン LIDAR はエラー Failed to find match for field “intensity” を報告します。
今回は、生のレーダー データを受け入れ、それを対応する形式に変換する ros ノード コードを作成しました。
完全なコード: https://github.com/LarryDong/lslidar_PointXYZ2PointXYZIR
説明: レーダーから送信された元のデータを受け入れ、リング フィールド情報を割り当て、このフィールドを含む点群を公開するように別のノードが作成されます。
しかし、オリジナルには強度フィールドが含まれておらず、この情報は失われているため、少なくとも形式が正しいことを確認するために、盲目的に強度に 0 を追加しました。
基本的
各ポイントに対応する角度を計算し、どのリングが LIDAR によって定義されたリングに最も近いかを確認します。
どれが一番近いかをどうやって見分けるのでしょうか？定義した2つのリングの平均値を計算し、左右の平均値の間にあればそのリングとみなされます。
レーシェン 32 ライン レーダーには 2 つの角度モードがあるため、左側の一様分布は角度を四捨五入するだけで、比較的簡単です。ただし、右側の不等分布については、マニュアルに記載されている角度情報に従って、どのルートに属するかを計算する必要があります。
コードの説明
まずレーダーによって定義された角度をリストし、前/次のハーネスとの平均を計算します。
const vector<float> g_ring_angle = {
-18, -15, -12, -10, -8, -7, -6, -5,
-4, -3.33, -3, -2.66, -2.33, -2, -1.66, -1.33,
-1, -0.66, -0.33, 0, 0.33, 0.66, 1, 1.33,
1.66, 2, 3, 4, 6, 8, 11, 14}; // ring angles defined by leishen
vector<float> g_angle_range; // define a range between each ring angle.
void initRingAngleRange(void){
// calculate angle range
assert(RING_NUMBER==g_ring_angle.size());
g_angle_range.push_back(-100); // assign a very large value
for(int i=0; i<RING_NUMBER-1; ++i){
float middle_value = (g_ring_angle[i] + g_ring_angle[i + 1]) / 2; // calculate the average value between two ring.
g_angle_range.push_back(middle_value);
}
g_angle_range.push_back(100);
}
次に、実際の角度を計算してハーネス ID に割り当てます。
void lidarCallback(const sensor_msgs::PointCloud2ConstPtr &msg_pc){
pcl::PointCloud<pcl::PointXYZ> pc;
pcl::PointCloud<myPointXYZIR> pc_new;
pcl::fromROSMsg(*msg_pc, pc);
// convert to PointXYZIR.
pc_new.points.reserve(pc.points.size());
myPointXYZIR pt_new;
for(const pcl::PointXYZ& p : pc.points){
float angle = atan(p.z / sqrt(p.x * p.x + p.y * p.y)) * 180 / M_PI;
if(std::isnan(angle)) // remove nan point.
continue;
// int scanID = int(angle + 17); // 对于1°分辨率的雷达，直接用这行指令就可以了，不需要计算下面的不均匀分布角度。
// for 0.33 degree mode.
int scanID = -1;
for(int i=0; i<RING_NUMBER; ++i){
if(angle > g_angle_range[i] && angle <= g_angle_range[i+1]){
scanID = i;
break;
}
}
pt_new.x = p.x;
pt_new.y = p.y;
pt_new.z = p.z;
pt_new.intensity = 0; // intensity is not used.
pt_new.ring = scanID;
pc_new.points.push_back(pt_new);
}
sensor_msgs::PointCloud2 msg_pc_new;
pcl::toROSMsg(pc_new, msg_pc_new);
msg_pc_new.header.frame_id = "laser_link";
msg_pc_new.header.stamp = msg_pc->header.stamp;
g_pub_pc.publish(msg_pc_new);
ROS_INFO("Published new pointcloud.");
}
ピットレコードを踏みにじる行為
- 最初はレーダーを1°モードスキャンに設定すれば十分だと思っていましたが、精度が異常であることがわかりました。カスタマーサービスに問い合わせたところ、1°モードと0.33°モードは調整できないことが分かりました。なのでこの変換コードを書き換えるしかありません。でも大丈夫、かなり正確です。
- rviz のPointCloud2
intensityオプションをクリックすると、XYZ、強度などを含むオプションが表示されますchannel。最初は意味がわかりませんでした。説明は次のとおりです。 -
RViz について: PointCloud2 ディスプレイを開いて「Intensity」カラー トランスフォーマーを選択すると、表示するチャンネルを選択できます。これは強度である必要はなく、実際には点群の任意のチャネルにすることができます。「強度境界を自動計算」をチェックしたままにすると、各点群の最小値と最大値が個別に計算され、カラー スペクトルがその範囲にスケールされます。チェック ボックスをオフにすると、最小値と最大値の強度を手動で入力できます (最小値と最大値が点群間で大きく異なり、点群間で色を一貫させたい場合に適しています)。