32ラインラジウム神レーダー走りLeGO-LOAM:3 DレーザーSLAM
32ラインラジウム神レーダー走りLeGO-LOAM:3 DレーザーSLAM取付LeGO-LOAM ラジウムレーダーに関する修正 LeGO-LOAMの修正 utilityを修正する.h imageprojectを修正する.cpp
Enjoy youself 走りrosbag リアルタイムオンライン図面 シーケンス 最近、社長がラジウム神の32線レーダーをくれて3 D SLAMをやらせてくれたので、LeGO-LOAMを走ることにしました.しかしLeGO-LOAMはVelodyneレーダーを使っているので、自分で何かを修正する必要があります.修正プロセスを記録します.
LeGO-LOAMのインストール
私のシステム:ubuntu 16.04 ros kinetic
具体的なこの過程は余計な説明にすぎません.多くの投稿がインストールを教えています.以下のブログを参考にしました.
詳しく説明してくれたので、先輩に敬意を表します.また、LeGO-LOAMソースライターgithubを貼り付けます
ラジウムレーダーに関する修正
まずLeGO-LOAMのデフォルトの受け入れ先topic nameはvelodyne_points,点群のframe_idはvelodyne、ラジウム神駆動リリースのtopic nameはlslidar_point_cloud,frame_idはlaser_link、ここでlslidarを修正する必要があります.c32.Launchファイル、remapちょっとtopic name、frameを修正しますidはvelodyneです.これは私がやった最も簡単な方法で、他のプログラムを書いてもいいです.launchファイルを貼り付けます.
修正後LeGO-LOAMでレーダーの点群データを受信できます.
LeGO-LOAMの修正
これは主にutilityを修正する必要がある.h及びimageProjection.cpp[^2]
ソースの著者を見て、修正が必要な場所を教えてください.
New Lidar:The key thing to adapt the code to a new sensor is making sure the point cloud can be properly projected to an range image and ground can be correctly detected. For example, VLP-16 has a angular resolution of 0.2° and 2° along two directions. It has 16 beams. The angle of the bottom beam is -15°. Thus, the parameters in “utility.h” are listed as below. When you implement new sensor, make sure that the ground_cloud has enough points for matching. Before you post any issues, please read this.
新しいレーダー:コードを新しいセンサに適応させる鍵は、点雲が距離画像と地面に正確に投影され、正確に検出できることを確保することです.例えば、VLP−16の2方向の角度分解能は、それぞれ0.2及び2である.16本のビームがありますベースビームの角度は-15です.したがって、ユーティリティのパラメータです.h」は以下のように列挙される.新しいセンサを実装する場合は、地面雲に十分な一致点があることを確認してください.どんな問題を発表する前に、この文章を読んでください.
New: a new useCloudRing flag has been added to help with point cloud projection. Velodyne point cloud has “ring” channel that directly gives the point row id in a range image. Other lidars may have a same type of channel, i.e., “r” in Ouster. If you are using a non-Velodyne lidar but it has a similar “ring” channel, you can change the PointXYZIR definition in utility.h and the corresponding code in imageProjection.cpp.
新しい:新しいuseCloudRingフラグが追加され、点群投影を支援します.Velodyne点群には「リング」チャネルがあり、点行idの範囲内の画像を直接与える.他のlidarsは、同じタイプのチャネルを有する可能性があります.すなわち、「r」は財産侵入です.非velodyneレーザレーダーを使用している場合は、utilityに似たような「リング」チャネルがあります.hでPointXYZIR定義を変更し、imageproject.cppで対応するコードを変更します.
もっと詳しくは源作者githubで見てみましょう
修正するhレーダーパラメータ修正 この先輩はもうはっきり言っているので、以下のブログに移動してもいいです.https://www.cnblogs.com/hgl0417/p/11067660.html
パラメータは必ずペアを設定しなければなりません.そうしないと、性能の差が大きくなります.自分のレーダーの設置する何Hzを確定して、普通は10 Hzを黙認します. useCloudRing修正 ラジウム神はVelodyneレーダーのring channel機能がないようなのでuseCloudRingはfalseに設定されています.
imageprojectを変更します.cpp
srcフォルダにimageprojectが見つかりました.cpp、原作者はずっとコードを更新しているかもしれないので、どの行を変更するかは科学的ではなく、行数が変わる可能性があると言っています.だから、私はやはり修正したコードを貼ります.
copyPointCloud関数の検索
ここでは
Enjoy youself
走れrosbag
これは何も言うことはありません.ラジウムでレーダーを起動します.
そしてrosbagを録画
rosbagを走るにはLeGO-LOAMのlaunchファイルを修正する必要はありません.直接来てください.runを実行します.launch
rosbagを再放送し、imuは使わなかったのでレーダーの話題のみ購読
リアルタイムオンライン図面作成
これは皆さんが一番望んでいるはずです.リアルタイムでlaunchファイルを修正する必要があります.
実は
次はenjoy youself
ブログを書くのも力仕事です.転載は出典を明記してください.ありがとうございます.注:オープンソースは容易ではありません.もしあなたに有益であれば、作者の仕事を支持してください.
LeGO-LOAMのインストール
私のシステム:ubuntu 16.04 ros kinetic
具体的なこの過程は余計な説明にすぎません.多くの投稿がインストールを教えています.以下のブログを参考にしました.
https://blog.csdn.net/weixin_39781401/article/details/89526028
https://blog.csdn.net/learning_tortosie/article/details/86527542
詳しく説明してくれたので、先輩に敬意を表します.また、LeGO-LOAMソースライターgithubを貼り付けます
https://github.com/RobustFieldAutonomyLab/LeGO-LOAM
ラジウムレーダーに関する修正
まずLeGO-LOAMのデフォルトの受け入れ先topic nameはvelodyne_points,点群のframe_idはvelodyne、ラジウム神駆動リリースのtopic nameはlslidar_point_cloud,frame_idはlaser_link、ここでlslidarを修正する必要があります.c32.Launchファイル、remapちょっとtopic name、frameを修正しますidはvelodyneです.これは私がやった最も簡単な方法で、他のプログラムを書いてもいいです.launchファイルを貼り付けます.
<launch>
<arg name="device_ip" default="192.168.1.200" />
<arg name="msop_port" default="2368" />
<arg name="difop_port" default="2369" />
<arg name="return_mode" default="1" />
<arg name="time_synchronization" default="true" />
<node pkg="lslidar_c32_driver" type="lslidar_c32_driver_node" name="lslidar_c32_driver_node" output="screen">
<param name="device_ip" value="$(arg device_ip)" />
<param name="msop_port" value="$(arg msop_port)" />
<param name="difop_port" value="$(arg difop_port)"/>
<param name="frame_id" value="velodyne"/>
<param name="add_multicast" value="false"/>
<param name="group_ip" value="224.1.1.2"/>
<param name="rpm" value="600"/>
<param name="return_mode" value="$(arg return_mode)"/>
<param name="time_synchronization" value="$(arg time_synchronization)"/>
</node>
<node pkg="lslidar_c32_decoder" type="lslidar_c32_decoder_node" name="lslidar_c32_decoder_node" output="screen">
<param name="min_range" value="0.15"/>
<param name="max_range" value="150.0"/>
<param name="return_mode" value="$(arg return_mode)"/>
<param name="time_synchronization" value="$(arg time_synchronization)"/>
<remap from="lslidar_point_cloud" to="/velodyne_points" />
</node>
修正後LeGO-LOAMでレーダーの点群データを受信できます.
LeGO-LOAMの修正
これは主にutilityを修正する必要がある.h及びimageProjection.cpp[^2]
ソースの著者を見て、修正が必要な場所を教えてください.
New Lidar:The key thing to adapt the code to a new sensor is making sure the point cloud can be properly projected to an range image and ground can be correctly detected. For example, VLP-16 has a angular resolution of 0.2° and 2° along two directions. It has 16 beams. The angle of the bottom beam is -15°. Thus, the parameters in “utility.h” are listed as below. When you implement new sensor, make sure that the ground_cloud has enough points for matching. Before you post any issues, please read this.
新しいレーダー:コードを新しいセンサに適応させる鍵は、点雲が距離画像と地面に正確に投影され、正確に検出できることを確保することです.例えば、VLP−16の2方向の角度分解能は、それぞれ0.2及び2である.16本のビームがありますベースビームの角度は-15です.したがって、ユーティリティのパラメータです.h」は以下のように列挙される.新しいセンサを実装する場合は、地面雲に十分な一致点があることを確認してください.どんな問題を発表する前に、この文章を読んでください.
New: a new useCloudRing flag has been added to help with point cloud projection. Velodyne point cloud has “ring” channel that directly gives the point row id in a range image. Other lidars may have a same type of channel, i.e., “r” in Ouster. If you are using a non-Velodyne lidar but it has a similar “ring” channel, you can change the PointXYZIR definition in utility.h and the corresponding code in imageProjection.cpp.
新しい:新しいuseCloudRingフラグが追加され、点群投影を支援します.Velodyne点群には「リング」チャネルがあり、点行idの範囲内の画像を直接与える.他のlidarsは、同じタイプのチャネルを有する可能性があります.すなわち、「r」は財産侵入です.非velodyneレーザレーダーを使用している場合は、utilityに似たような「リング」チャネルがあります.hでPointXYZIR定義を変更し、imageproject.cppで対応するコードを変更します.
もっと詳しくは源作者githubで見てみましょう
修正するh
// LeiShen-32C-5Hz
// extern const int N_SCAN = 32;
// extern const int Horizon_SCAN = 4000;
// extern const float ang_res_x = 360.0 / float(Horizon_SCAN);
// extern const float ang_res_y = 26 / float(N_SCAN-1);
// extern const float ang_bottom = 16.5;
// extern const int groundScanInd = 10; //
// LeiShen-32C-10Hz
extern const int N_SCAN = 32;//32
extern const int Horizon_SCAN = 2000;//
extern const float ang_res_x = 360.0 / float(Horizon_SCAN);//
extern const float ang_res_y = 26.0 / float(N_SCAN-1);//
extern const float ang_bottom = 16.5;
extern const int groundScanInd = 10; //
パラメータは必ずペアを設定しなければなりません.そうしないと、性能の差が大きくなります.自分のレーダーの設置する何Hzを確定して、普通は10 Hzを黙認します.
extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used
imageprojectを変更します.cpp
srcフォルダにimageprojectが見つかりました.cpp、原作者はずっとコードを更新しているかもしれないので、どの行を変更するかは科学的ではなく、行数が変わる可能性があると言っています.だから、私はやはり修正したコードを貼ります.
copyPointCloud関数の検索
void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
cloudHeader = laserCloudMsg->header;
cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
// Remove Nan points
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
// have "ring" channel in the cloud
if (useCloudRing == true){
pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
if (laserCloudInRing->is_dense == false) {
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
}
}
ここでは
cloudHeader.stamp = ros::Time::now();
の注釈を外しました.Enjoy youself
走れrosbag
これは何も言うことはありません.ラジウムでレーダーを起動します.
roslaunch lslidar_c32_decoder lslidar_c32.launch --screen
そしてrosbagを録画
rosbag record -a
rosbagを走るにはLeGO-LOAMのlaunchファイルを修正する必要はありません.直接来てください.runを実行します.launch
roslaunch lego_loam run.launch
rosbagを再放送し、imuは使わなかったのでレーダーの話題のみ購読
rosbag play *.bag --clock --topic /velodyne_points
リアルタイムオンライン図面作成
これは皆さんが一番望んでいるはずです.リアルタイムでlaunchファイルを修正する必要があります.
<launch>
<!--- Sim Time -->
<param name="/use_sim_time" value="false" />
<!--- Run Rviz-->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find lego_loam)/launch/test.rviz" />
<!--- TF -->
<node pkg="tf" type="static_transform_publisher" name="camera_init_to_map" args="0 0 0 1.570795 0 1.570795 /map /camera_init 10" />
<node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" args="0 0 0 -1.570795 -1.570795 0 /camera /base_link 10" />
<!--- LeGO-LOAM -->
<node pkg="lego_loam" type="imageProjection" name="imageProjection" output="screen"/>
<node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/>
<node pkg="lego_loam" type="mapOptmization" name="mapOptmization" output="screen"/>
<node pkg="lego_loam" type="transformFusion" name="transformFusion" output="screen"/>
</launch>
実は
ですtrueからfalseに変更します次はenjoy youself
roslaunch lslidar_c32_decoder lslidar_c32.launch --screen
roslaunch lego_loam run.launch
ブログを書くのも力仕事です.転載は出典を明記してください.ありがとうございます.注:オープンソースは容易ではありません.もしあなたに有益であれば、作者の仕事を支持してください.