update README.md

.gitignore

@@ -0,0 +1,6 @@
+.vscode
+build
+devel
+data
+bag
+.catkin_workspace

CMakeLists.txt

@@ -56,7 +56,6 @@ message(status  ${PCL_INCLUDE_DIRS})
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(${PROJECT_NAME}_node src/lidar_undistortion_2d.cpp)
 
-
 ## Specify libraries to link a library or executable target against
  target_link_libraries(${PROJECT_NAME}_node
   ${catkin_LIBRARIES}

README.md

@@ -16,6 +16,26 @@ This ros package uses odom transform data to correct motion distortion of a 2D L
 
 in this picture, the yellow rectangle represents the pose of robot, the red poindcloud represents the origin lidar data, and the white pointcloud represents the lidar data after compensation.
 
+## Test with rosbag
+1. compile the project and `source devel/setup.sh`
+2. 
+```
+roslaunch lidar_undistortion_2d test_lidar_undistortion_2d.launch enable_undistortion:=true
+```
+3. find `/bag/sensor_data.bag`
+```
+rosbag play --clock sensor_data.bag
+```
+4. result 
+
+the gif showed below represents location with orign lidar data.
+
+![](doc/lidar_orign.gif)
+
+the gif showed below represents location with undistortion lidar data.
+
+![](doc/lidar_undistortion.gif)
+
 ## Reference
 
 https://github.com/elewu/2d_lidar_undistortion/

launch/lidar_undistortion_2d.launch

@@ -1,13 +1,16 @@
 <launch>
    <param name="use_sim_time" value="true"/>
 
-   <node name="lidar_undistortion_2d_node" pkg="lidar_undistortion_2d"  type="lidar_undistortion_2d_node" output="log">
+   <node name="lidar_undistortion_2d_node" pkg="lidar_undistortion_2d" type="lidar_undistortion_2d_node">
-      <param name="lidar_sub_topic" value="/scan"/>
+      <param name="scan_sub_topic" value="/scan"/>
-      <param name="lidar_pub_topic" value="/lidar_undistortion/scan"/>
+      <param name="scan_pub_topic" value="/scan_undistortion"/>
       <param name="enable_pub_pointcloud" value="true"/>
-      <param name="pointcloud_pub_topic" value="/lidar_undistortion/pointcloud"/>
+      <param name="pointcloud_pub_topic" value="/pointcloud_undistortion"/>
       <param name="lidar_frame" value="laser_link"/>
       <param name="odom_frame" value="odom"/>
+      <param name="lidar_scan_time_gain" value="1.0"/>
    </node>
 
+   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lidar_undistortion)/launch/racecar_full.rviz"/>
+
 </launch>

launch/test_lidar_undistortion_2d.launch

@@ -0,0 +1,84 @@
+<!-- 通过传感器采集到的rosbag数据，重新进行数据处理 -->
+<launch>
+
+  <arg name="scan_orign_topic" default="/scan"/>
+  <arg name="scan_topic" default="/scan_undistortion"/>
+  <arg name="imu_topic" default="/imu_data"/>
+  <arg name="map_topic" default="/map"/>
+  <arg name="odom_topic" default="/odom"/>
+  <arg name="odom_rf2o_topic" default="/odom_rf2o"/>
+  <arg name="base_frame" default="base_footprint"/>
+  <arg name="map_frame" default="map"/>
+  <arg name="odom_frame" default="odom"/>
+  <arg name="imu_frame" default="imu_link"/>
+  <arg name="laser_frame" default="laser_link"/>
+
+  <arg name="enable_undistortion" default="false"/>
+
+  <param name="/use_sim_time" value="true"/>
+
+
+  <node name="odom_ekf_relay" pkg="topic_tools" type="relay" unless="$(arg enable_undistortion)"
+        args="$(arg scan_orign_topic) $(arg scan_topic)"/>
+  <node name="lidar_undistortion_2d_node" pkg="lidar_undistortion_2d" type="lidar_undistortion_2d_node" if="$(arg enable_undistortion)" output="screen">
+    <param name="scan_sub_topic" value="$(arg scan_orign_topic)"/>
+    <param name="scan_pub_topic" value="$(arg scan_topic)"/>
+    <param name="enable_pub_pointcloud" value="false"/>
+    <param name="lidar_frame" value="$(arg laser_frame)"/>
+    <param name="odom_frame" value="$(arg odom_frame)"/>
+    <param name="lidar_scan_time_gain" value="1000.0"/>
+  </node>
+
+  <node pkg="amcl" type="amcl" name="amcl" output="screen">
+
+    <remap from="map" to="$(arg map_topic)"/>
+    <remap from="scan" to="$(arg scan_topic)"/>
+    <param name="odom_frame_id" value="$(arg odom_frame)"/>
+    <param name="base_frame_id" value="$(arg base_frame)"/>
+    <param name="global_frame_id" value="$(arg map_frame)"/>
+    <param name="tf_broadcast" value="true"/>
+    <param name="use_map_topic" value="true"/> 
+    <param name="first_map_only" value="true"/>
+
+    <param name="min_particles" value="500"/>
+    <param name="max_particles" value="5000"/>
+    <param name="kld_err" value="0.05"/>
+    <param name="kld_z" value="0.99"/>
+
+    <param name="update_min_d" value="0.0"/>
+    <param name="update_min_a" value="0.1"/>
+    <param name="resample_interval" value="2"/>
+    <param name="transform_tolerance" value="0.2"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.1"/>
+
+    <param name="initial_pose_x" value="8.01734161377"/>
+    <param name="initial_pose_y" value="-14.7525091171"/>
+    <param name="initial_pose_a" value="1.576264"/>
+    <param name="initial_cov_xx" value="0.25"/>
+    <param name="initial_cov_yy" value="0.25"/>
+    <param name="initial_cov_aa" value="0.2"/>
+
+    <param name="laser_min_range" value="-1.0"/>
+    <param name="laser_max_range" value="-1.0"/>
+    <param name="laser_max_beams" value="30"/>
+    <param name="laser_z_hit" value="0.5"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.5"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+
+    <param name="odom_model_type" value="diff-corrected"/>
+    <param name="odom_alpha1" value="0.2"/>
+    <param name="odom_alpha2" value="0.2"/>
+    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha4" value="0.2"/>
+    <param name="odom_alpha5" value="0.2"/>
+  </node>
+
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lidar_undistortion_2d)/launch/test_lidar_undistortion_2d.rviz"/>
+
+</launch>

launch/test_lidar_undistortion_2d.rviz

@@ -0,0 +1,230 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+      Splitter Ratio: 0.602305472
+    Tree Height: 775
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+      - /Current View1/Focal Point1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScanCorrect
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: false
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: false
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+      Marker Scale: 0.800000012
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: false
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 726
+      Min Color: 0; 0; 0
+      Min Intensity: 2
+      Name: LaserScanOrign
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.0500000007
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 402
+      Min Color: 0; 0; 0
+      Min Intensity: 2
+      Name: LaserScanCorrect
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.0500000007
+      Style: Flat Squares
+      Topic: /scan_undistortion
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Acceleration properties:
+        Acc. vector alpha: 1
+        Acc. vector color: 255; 0; 0
+        Acc. vector scale: 1
+        Derotate acceleration: true
+        Enable acceleration: false
+      Axes properties:
+        Axes scale: 1
+        Enable axes: true
+      Box properties:
+        Box alpha: 1
+        Box color: 255; 0; 0
+        Enable box: false
+        x_scale: 1
+        y_scale: 1
+        z_scale: 1
+      Class: rviz_imu_plugin/Imu
+      Enabled: false
+      Name: Imu
+      Topic: /imu_data
+      Unreliable: false
+      Value: false
+      fixed_frame_orientation: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: local_footprint
+      Topic: /move_base/local_costmap/footprint
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 12.6959887
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 3.92016053
+        Y: -7.73538065
+        Z: 2.29020095
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 1.4847964
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 6.24544525
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000006100000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1920
+  X: 0
+  Y: 24

src/lidar_undistortion_2d.cpp

@@ -32,6 +32,7 @@ private:
     std::string odom_frame_;
 
     bool enable_pub_pointcloud_;
+    double lidar_scan_time_gain_;
 
 public:
 
@@ -47,10 +48,11 @@ public:
         nh_param.param<std::string>("point_cloud_pub_topic", pointcloud_pub_topic_, "/lidar_undistortion/pointcloud");
         nh_param.param<std::string>("lidar_frame", lidar_frame_, "laser_link");
         nh_param.param<std::string>("odom_frame", odom_frame_, "oodm");
+        nh_param.param<double>("lidar_scan_time_gain", lidar_scan_time_gain_, 1.0);
 
         scan_sub_ = nh_.subscribe(scan_sub_topic_, 10, &LidarMotionCalibrator::ScanCallBack, this);
         scan_pub_ = nh_.advertise<sensor_msgs::LaserScan>(scan_pub_topic_, 1);
-        pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(pointcloud_pub_topic_, 1);
+        if(enable_pub_pointcloud_) pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(pointcloud_pub_topic_, 1);
     }
 
 
@@ -68,7 +70,7 @@ public:
 
         // 得到最终点的时间
         int beamNum = scan_msg->ranges.size();
-        endTime = startTime + ros::Duration(scan_msg->time_increment * beamNum); // 0.00024953688262
+        endTime = startTime + ros::Duration(scan_msg->time_increment * lidar_scan_time_gain_ * beamNum);
 
         // 将数据复制出来
         std::vector<double> angles;