add resultion setting and add support for velodyne VLP-16

wh200720041 · Jul 7, 2020 · 643b821 · 643b821
1 parent 94c523d
commit 643b821
Show file tree

Hide file tree

Showing 13 changed files with 373 additions and 45 deletions.
diff --git a/README.md b/README.md
@@ -90,13 +90,26 @@ if you would like to create the map at the same time, you can run (more cpu cost
 ```
     roslaunch floam floam_mapping.launch
 ```
+If the mapping process is slow, you may wish to change the rosbag speed by replacing "--clock -r 0.5" with "--clock -r 0.2" in your launch file, or you can change the map publish frequency manually (default is 10 Hz)
 
 
-## 5. Test other sequence
+## 5. Test on other sequence
 To generate rosbag file of kitti dataset, you may use the tools provided by 
 [kitti_to_rosbag](https://github.com/ethz-asl/kitti_to_rosbag) or [kitti2bag](https://github.com/tomas789/kitti2bag) 
 
-## 6.Acknowledgements
-Thanks for [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM) and LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time) and [LOAM_NOTED](https://github.com/cuitaixiang/LOAM_NOTED).
+## 6. Test on Velodyne VLP-16 or HDL-32
+You may wish to test FLOAM on your own platform and sensor such as VLP-16
+You can install the velodyne sensor driver by 
+```
+sudo apt-get install ros-melodic-velodyne-pointcloud
+```
+launch floam for your own velodyne sensor
+```
+    roslaunch floam floam_velodyne.launch
+```
+If you are using HDL-32 or other sensor, please change the scan_line in the launch file 
+
 
+## 7.Acknowledgements
+Thanks for [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM) and LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time) and [LOAM_NOTED](https://github.com/cuitaixiang/LOAM_NOTED).
 
diff --git a/include/laserMappingClass.h b/include/laserMappingClass.h
@@ -38,7 +38,7 @@ class LaserMappingClass
 
     public:
     	LaserMappingClass();
-		void init(void);
+		void init(double map_resolution);
 		void updateCurrentPointsToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, const Eigen::Isometry3d& pose_current);
 		pcl::PointCloud<pcl::PointXYZI>::Ptr getMap(void);
 

diff --git a/include/odomEstimationClass.h b/include/odomEstimationClass.h
@@ -39,7 +39,7 @@ class OdomEstimationClass
     public:
     	OdomEstimationClass();
 
-		void init(lidar::Lidar lidar_param);	
+		void init(lidar::Lidar lidar_param, double map_resolution);	
 		void initMapWithPoints(const pcl::PointCloud<pcl::PointXYZI>::Ptr& edge_in, const pcl::PointCloud<pcl::PointXYZI>::Ptr& surf_in);
 		void updatePointsToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& edge_in, const pcl::PointCloud<pcl::PointXYZI>::Ptr& surf_in);
 		void getMap(pcl::PointCloud<pcl::PointXYZI>::Ptr& laserCloudMap);

diff --git a/launch/floam.launch b/launch/floam.launch
@@ -3,7 +3,7 @@
 
 
     <node pkg="rosbag" type="play" name="rosbag_play" args="--clock $(find floam)/dataset/2011_09_30_0018.bag"/> 
-    
+
     <!-- For Velodyne VLP-16 
     <param name="scan_line" value="16" />
     -->
@@ -15,13 +15,15 @@
     <!-- For Velodyne HDL-64 -->
     <param name="scan_line" value="64" />
 
+
     <!--- Sim Time -->
     <param name="/use_sim_time" value="true" />
-    <param name="scan_period" value="0.1" />
+    <param name="scan_period" value="0.1" /> 
     <param name="vertical_angle" type="double" value="2.0" />
     <param name="max_dis" type="double" value="90.0" />
     <param name="min_dis" type="double" value="3.0" />
 
+    <!--- -->
     <node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/>
     <node pkg="floam" type="floam_laser_processing_node" name="floam_laser_processing_node" output="screen"/>
 
@@ -38,12 +40,11 @@
         <param name="/trajectory_update_rate" value="10.0" />
         <param name="/trajectory_publish_rate" value="10.0" />
     </node>
-    <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="aft_mapped" >
+    <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="base_link" >
         <param name="/target_frame_name" value="world" />
-        <param name="/source_frame_name" value="aft_mapped" />
+        <param name="/source_frame_name" value="base_link" />
         <param name="/trajectory_update_rate" value="10.0" />
         <param name="/trajectory_publish_rate" value="10.0" />
     </node>
 
 </launch>
-
diff --git a/launch/floam_mapping.launch b/launch/floam_mapping.launch
@@ -2,7 +2,7 @@
 <launch>
 
 
-    <node pkg="rosbag" type="play" name="rosbag_play" args="--clock $(find floam)/dataset/2011_09_30_0018.bag"/> 
+    <node pkg="rosbag" type="play" name="rosbag_play" args="--clock -r 0.5 $(find floam)/dataset/2011_09_30_0018.bag"/> 
 
     <!-- For Velodyne VLP-16 
     <param name="scan_line" value="16" />
@@ -14,14 +14,16 @@
 
     <!-- For Velodyne HDL-64 -->
     <param name="scan_line" value="64" />
-
+    
     <!--- Sim Time -->
     <param name="/use_sim_time" value="true" />
     <param name="scan_period" value="0.1" />
+
     <param name="vertical_angle" type="double" value="2.0" />
     <param name="max_dis" type="double" value="90.0" />
+    <param name="map_resolution" type="double" value="0.4" />
     <param name="min_dis" type="double" value="3.0" />
-
+    <!--- -->
     <node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/>
     <node pkg="floam" type="floam_laser_processing_node" name="floam_laser_processing_node" output="screen"/>
     <node pkg="floam" type="floam_laser_mapping_node" name="floam_laser_mapping_node" output="screen"/>
@@ -40,12 +42,11 @@
         <param name="/trajectory_update_rate" value="10.0" />
         <param name="/trajectory_publish_rate" value="10.0" />
     </node>
-    <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="aft_mapped" >
+    <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="base_link" >
         <param name="/target_frame_name" value="world" />
-        <param name="/source_frame_name" value="aft_mapped" />
+        <param name="/source_frame_name" value="base_link" />
         <param name="/trajectory_update_rate" value="10.0" />
         <param name="/trajectory_publish_rate" value="10.0" />
     </node>
 
 </launch>
-
diff --git a/launch/floam_velodyne.launch b/launch/floam_velodyne.launch
@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<launch>
+
+    <!-- launch your velodyne here -->
+    <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch"/>
+
+    <!-- For Velodyne VLP-16 -->
+    <param name="scan_line" value="16" />
+
+
+    <!-- For Velodyne HDL-32 
+    <param name="scan_line" value="32" />
+    -->
+
+    <!-- For Velodyne HDL-64 
+    <param name="scan_line" value="64" />
+    -->
+
+    <!--- Sim Time -->
+    <param name="scan_period" value="0.1" />
+
+    <param name="vertical_angle" type="double" value="2.0" />
+    <param name="map_resolution" type="double" value="0.2" />
+    <param name="max_dis" type="double" value="90.0" />
+    <param name="min_dis" type="double" value="0.5" />
+    <!--- -->
+    <node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/>
+    <node pkg="floam" type="floam_laser_processing_node" name="floam_laser_processing_node" output="screen"/>
+    <node pkg="floam" type="floam_laser_mapping_node" name="floam_laser_mapping_node" output="screen"/>
+
+    <node pkg="tf" type="static_transform_publisher" name="word2map_tf"  args="0 0 0 0 0 0 /world /map 10" />
+
+    <arg name="rviz" default="true" />
+    <group if="$(arg rviz)">
+        <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find floam)/rviz/floam_velodyne.rviz" />
+    </group>
+
+    <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="base_link" >
+        <param name="/target_frame_name" value="world" />
+        <param name="/source_frame_name" value="base_link" />
+        <param name="/trajectory_update_rate" value="10.0" />
+        <param name="/trajectory_publish_rate" value="10.0" />
+    </node>
+
+</launch>
diff --git a/rviz/floam.rviz b/rviz/floam.rviz
@@ -4,7 +4,9 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Status1
         - /Grid1
+        - /TF1
         - /TF1/Frames1
       Splitter Ratio: 0.5
     Tree Height: 530
@@ -69,7 +71,7 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        aft_mapped:
+        base_link:
           Value: true
         cam00:
           Value: false
@@ -106,7 +108,7 @@ Visualization Manager:
             velodyne:
               {}
           map:
-            aft_mapped:
+            base_link:
               {}
       Update Interval: 0
       Value: true
@@ -125,7 +127,7 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 0.9300000071525574
+      Max Intensity: 0.9900000095367432
       Min Color: 0; 0; 0
       Min Intensity: 0
       Name: raw_data
@@ -183,7 +185,7 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
-      Topic: /aft_mapped/trajectory
+      Topic: /base_link/trajectory
       Unreliable: false
       Value: true
   Enabled: true
@@ -213,7 +215,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: -1.5649943351745605
+      Angle: -1.5749943256378174
       Class: rviz/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
@@ -223,7 +225,7 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 1.8188380002975464
+      Scale: 2.7801454067230225
       Target Frame: body
       Value: TopDownOrtho (rviz)
       X: 84.71299743652344

diff --git a/rviz/floam_mapping.rviz b/rviz/floam_mapping.rviz
@@ -66,19 +66,51 @@ Visualization Manager:
       Unreliable: false
       Value: true
     - Class: rviz/TF
-      Enabled: false
+      Enabled: true
       Frame Timeout: 15
       Frames:
         All Enabled: false
-      Marker Scale: 100
+        base_link:
+          Value: true
+        cam00:
+          Value: false
+        cam01:
+          Value: false
+        cam02:
+          Value: false
+        cam03:
+          Value: false
+        imu:
+          Value: false
+        map:
+          Value: false
+        velodyne:
+          Value: false
+        world:
+          Value: true
+      Marker Scale: 50
       Name: TF
       Show Arrows: false
       Show Axes: true
       Show Names: true
       Tree:
-        {}
+        world:
+          imu:
+            cam00:
+              {}
+            cam01:
+              {}
+            cam02:
+              {}
+            cam03:
+              {}
+            velodyne:
+              {}
+          map:
+            base_link:
+              {}
       Update Interval: 0
-      Value: false
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -132,7 +164,7 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 1
-      Size (m): 0.44999998807907104
+      Size (m): 0.3499999940395355
       Style: Squares
       Topic: /map
       Unreliable: false
@@ -182,14 +214,14 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
-      Topic: /aft_mapped/trajectory
+      Topic: /base_link/trajectory
       Unreliable: false
       Value: true
   Enabled: true
   Global Options:
     Background Color: 136; 138; 133
     Default Light: true
-    Fixed Frame: aft_mapped
+    Fixed Frame: base_link
     Frame Rate: 30
   Name: root
   Tools:
@@ -228,10 +260,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.9753983020782471
+      Pitch: 0.9953982830047607
       Target Frame: body
       Value: ThirdPersonFollower (rviz)
-      Yaw: 3.310410261154175
+      Yaw: 3.2104086875915527
     Saved: ~
 Window Geometry:
   Displays: