unsync msg 未处理

Author: jyakaranda

include/imageProjection.h

@@ -12,9 +12,9 @@ class ImageProjection : public nodelet::Nodelet
   ros::Publisher pub_segmented_cloud_;
   ros::Publisher pub_seg_info_;
   ros::Publisher pub_outlier_;
-  sensor_msgs::PointCloud2 segmented_cloud_msg_;
-  alego::cloud_info seg_info_msg_;
-  sensor_msgs::PointCloud2 outlier_cloud_msg_;
+  sensor_msgs::PointCloud2Ptr segmented_cloud_msg_;
+  alego::cloud_infoPtr seg_info_msg_;
+  sensor_msgs::PointCloud2Ptr outlier_cloud_msg_;
 
   PointCloudT::Ptr full_cloud_;
   PointCloudT::Ptr segmented_cloud_;

include/laserOdometry.h

@@ -2,11 +2,65 @@
 
 namespace loam
 {
+using namespace std;
 class LaserOdometry : public nodelet::Nodelet
 {
 public:
   virtual void onInit();
+
+private:
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
+  ros::Subscriber sub_segmented_cloud_;
+  ros::Subscriber sub_segmented_info_;
+  ros::Subscriber sub_outlier_;
+  ros::Subscriber sub_imu_;
+  ros::Subscriber sub_odom_;
+  ros::Publisher pub_surf_;
+  ros::Publisher pub_surf_less_;
+  ros::Publisher pub_corner_;
+  ros::Publisher pub_corner_less_;
+  ros::Publisher pub_undistorted_pc_;
+
+  queue<sensor_msgs::PointCloud2ConstPtr> seg_cloud_buf_;
+  queue<alego::cloud_infoConstPtr> seg_info_buf_;
+  queue<sensor_msgs::PointCloud2ConstPtr> outlier_buf_;
+  queue<nav_msgs::OdometryConstPtr> odom_buf_;
+  queue<sensor_msgs::ImuConstPtr> imu_buf_;
+
+  int imu_ptr_front_, imu_ptr_last_, imu_ptr_last_iter_;
+  std::array<double, imu_queue_length> imu_time_;
+  std::array<float, imu_queue_length> imu_roll_;
+  std::array<float, imu_queue_length> imu_pitch_;
+  std::array<float, imu_queue_length> imu_yaw_;
+  std::array<float, imu_queue_length> imu_shift_x_;
+  std::array<float, imu_queue_length> imu_shift_y_;
+  std::array<float, imu_queue_length> imu_shift_z_;
+  std::array<float, imu_queue_length> imu_velo_x_;
+  std::array<float, imu_queue_length> imu_velo_y_;
+  std::array<float, imu_queue_length> imu_velo_z_;
+
+  // 里程计相关
+  int odom_ptr_front_, odom_ptr_last_, odom_ptr_last_iter_;
+  std::array<nav_msgs::OdometryConstPtr, odom_queue_length> odom_queue_;
+  std::array<float, odom_queue_length> odom_roll_;
+  std::array<float, odom_queue_length> odom_pitch_;
+  std::array<float, odom_queue_length> odom_yaw_;
+
+  std::mutex m_buf_;
+
+  void mainLoop();
+
+  void adjustDistortion(PointCloudT::Ptr cloud, double scan_time);
+  void calculateSmoothness();
+  void markOcculudedPoints();
+  void extractFeatures();
+
+  void segCloudHandler(const sensor_msgs::PointCloud2ConstPtr &msg);
+  void segInfoHandler(const alego::cloud_infoConstPtr &msg);
+  void outlierHandler(const sensor_msgs::PointCloud2ConstPtr &msg);
+
+  void imuHandler(const sensor_msgs::ImuConstPtr &msg);
+  void odomHandler(const nav_msgs::OdometryConstPtr &msg);
 };
 } // namespace loam
-
-PLUGINLIB_EXPORT_CLASS(loam::LaserOdometry, nodelet::Nodelet)

include/utility.h

@@ -42,23 +42,27 @@ using PointCloudT = pcl::PointCloud<PointT>;
 #define ANGLE2RAD(x) ((x) / 180.0 * M_PI)
 
 const int N_SCAN = 16;
-const float ang_res_x = 0.18;
+const float ang_res_x = 0.18; // 10Hz
 const float ang_res_y = 2.0;
+const float scan_period = 0.1; // 10Hz
 
-const int Horizon_SCAN = 360.0 / ang_res_x + 0.5; // 10Hz
+const int Horizon_SCAN = 360.0 / ang_res_x + 0.5;
 const float ang_bottom = 15.0;
 const int ground_scan_id = 7;
 const float sensor_mount_ang = 0.;  // 向下为正，向上为负
 
-// 可以调调参数
 const float seg_alpha_x = ANGLE2RAD(ang_res_x);
 const float seg_alpha_y = ANGLE2RAD(ang_res_y);
+// 可以调调参数
 const float seg_theta = 1.0472;
 const int seg_valid_point_num = 5;
 const int seg_valid_line_num = 3;
 
+// (use_imu && use_odom) == false
 const bool use_imu = true;
-const bool use_odom = true;
+const bool use_odom = false;
+const int imu_queue_length = 1000;
+const int odom_queue_length = 1000;
 
 class TicToc
 {

launch/test.launch

@@ -4,4 +4,6 @@
 
   <node pkg="nodelet" type="nodelet" name="lego_manager" args="manager" output="log" />
   <node pkg="nodelet" type="nodelet" name="ImageProjection" args="load loam/ImageProjection lego_manager" output="log" />
+  <node pkg="nodelet" type="nodelet" name="LaserOdometry" args="load loam/LaserOdometry lego_manager" output="log" />
+
 </launch>

nodelet_plugins.xml

@@ -1,9 +1,11 @@
 <library path="lib/libimageProjection">
-  <class name="loam/ImageProjection" type="loam::ImageProjection" base_class_type="nodelet::Nodelet">
-    <description>
-  ImageProjection
-    </description>
-  </class>
-  <class name="loam/LaserOdometry" type="loam::LaserOdometry" base_class_type="nodelet::Nodelet"/>
+  <class name="loam/ImageProjection" type="loam::ImageProjection" base_class_type="nodelet::Nodelet" />
+</library>
+
+<library path="lib/liblaserOdometry">
+  <class name="loam/LaserOdometry" type="loam::LaserOdometry" base_class_type="nodelet::Nodelet" />
+</library>
+
+<library path="lib/liblaserMapping">
   <class name="loam/LaserMapping" type="loam::LaserMapping" base_class_type="nodelet::Nodelet" />
 </library>

rviz/test.rviz

@@ -6,6 +6,8 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /outlier1/Status1
+        - /PointCloud21
       Splitter Ratio: 0.5
     Tree Height: 773
   - Class: rviz/Selection
@@ -100,7 +102,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: FlatColor
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 255
@@ -117,7 +119,7 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -130,12 +132,12 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: Intensity
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 15.1721001
+      Max Intensity: 15.1498003
       Min Color: 0; 0; 0
-      Min Intensity: 9.99999975e-05
+      Min Intensity: 0
       Name: segmented
       Position Transformer: XYZ
       Queue Size: 10
@@ -147,7 +149,7 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -178,6 +180,36 @@ Visualization Manager:
       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/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 15.1975002
+      Min Color: 0; 0; 0
+      Min Intensity: 9.99999975e-05
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.100000001
+      Style: Flat Squares
+      Topic: /undistorted
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -203,7 +235,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 77.0821152
+      Distance: 61.62677
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
@@ -218,10 +250,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.864796996
+      Pitch: 0.53979665
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.12711835
+      Yaw: 2.67211771
     Saved: ~
 Window Geometry:
   Displays:

src/imageProjection.cpp

@@ -7,14 +7,19 @@ namespace loam
 
 void ImageProjection::onInit()
 {
+  NODELET_INFO("---------- ImageProjection init -----------");
   TicToc t_init;
-  nh_ = getNodeHandle();
+  nh_ = getMTNodeHandle();
 
-  seg_info_msg_.startRingIndex.resize(N_SCAN);
-  seg_info_msg_.endRingIndex.resize(N_SCAN);
-  seg_info_msg_.segmentedCloudColInd.assign(N_SCAN * Horizon_SCAN, false);
-  seg_info_msg_.segmentedCloudGroundFlag.assign(N_SCAN * Horizon_SCAN, 0);
-  seg_info_msg_.segmentedCloudRange.assign(N_SCAN * Horizon_SCAN, 0);
+  segmented_cloud_msg_.reset(new sensor_msgs::PointCloud2());
+  seg_info_msg_.reset(new alego::cloud_info());
+  outlier_cloud_msg_.reset(new sensor_msgs::PointCloud2());
+
+  seg_info_msg_->startRingIndex.resize(N_SCAN);
+  seg_info_msg_->endRingIndex.resize(N_SCAN);
+  seg_info_msg_->segmentedCloudColInd.assign(N_SCAN * Horizon_SCAN, false);
+  seg_info_msg_->segmentedCloudGroundFlag.assign(N_SCAN * Horizon_SCAN, 0);
+  seg_info_msg_->segmentedCloudRange.assign(N_SCAN * Horizon_SCAN, 0);
   full_cloud_.reset(new PointCloudT());
   segmented_cloud_.reset(new PointCloudT());
   outlier_cloud_.reset(new PointCloudT());
@@ -33,34 +38,34 @@ void ImageProjection::onInit()
   neighbor_iter_.emplace_back(0, 1);
 
   pub_segmented_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("/segmented_cloud", 10);
-  pub_seg_info_ = nh_.advertise<sensor_msgs::PointCloud2>("/seg_info", 10);
+  pub_seg_info_ = nh_.advertise<alego::cloud_info>("/seg_info", 10);
   pub_outlier_ = nh_.advertise<sensor_msgs::PointCloud2>("/outlier", 10);
   sub_pc_ = nh_.subscribe<sensor_msgs::PointCloud2>("/lslidar_point_cloud", 10, boost::bind(&ImageProjection::pcCB, this, _1));
-  cout << "onInit end: " << t_init.toc() << endl;
+  NODELET_INFO_STREAM("ImageProjection onInit end: " << t_init.toc());
 }
 
 void ImageProjection::pcCB(const sensor_msgs::PointCloud2ConstPtr &msg)
 {
-  cout << "pcCB" << endl;
+  // cout << "pcCB" << endl;
   TicToc t_whole;
 
-  seg_info_msg_.header = msg->header;
+  seg_info_msg_->header = msg->header;
   PointCloudT::Ptr cloud_in(new PointCloudT());
   pcl::fromROSMsg(*msg, *cloud_in);
   // ROS_INFO("cloud_in size: %d", cloud_in->points.size());
 
   int cloud_size = cloud_in->points.size();
-  seg_info_msg_.startOrientation = -atan2(cloud_in->points[0].y, cloud_in->points[0].x);
-  seg_info_msg_.endOrientation = -atan2(cloud_in->points[cloud_size - 1].y, cloud_in->points[cloud_size - 1].x) + 2 * M_PI;
-  if (seg_info_msg_.endOrientation - seg_info_msg_.startOrientation > 3 * M_PI)
+  seg_info_msg_->startOrientation = -atan2(cloud_in->points[0].y, cloud_in->points[0].x);
+  seg_info_msg_->endOrientation = -atan2(cloud_in->points[cloud_size - 1].y, cloud_in->points[cloud_size - 1].x) + 2 * M_PI;
+  if (seg_info_msg_->endOrientation - seg_info_msg_->startOrientation > 3 * M_PI)
   {
-    seg_info_msg_.endOrientation -= 2 * M_PI;
+    seg_info_msg_->endOrientation -= 2 * M_PI;
   }
-  else if (seg_info_msg_.endOrientation - seg_info_msg_.startOrientation < M_PI)
+  else if (seg_info_msg_->endOrientation - seg_info_msg_->startOrientation < M_PI)
   {
-    seg_info_msg_.endOrientation += 2 * M_PI;
+    seg_info_msg_->endOrientation += 2 * M_PI;
   }
-  seg_info_msg_.orientationDiff = seg_info_msg_.endOrientation - seg_info_msg_.startOrientation;
+  seg_info_msg_->orientationDiff = seg_info_msg_->endOrientation - seg_info_msg_->startOrientation;
 
   float vertical_ang, horizon_ang;
   int row_id, col_id, index;
@@ -145,7 +150,7 @@ void ImageProjection::pcCB(const sensor_msgs::PointCloud2ConstPtr &msg)
   int line_size = 0;
   for (int i = 0; i < N_SCAN; ++i)
   {
-    seg_info_msg_.startRingIndex[i] = line_size + 5;
+    seg_info_msg_->startRingIndex[i] = line_size + 5;
     for (int j = 0; j < Horizon_SCAN; ++j)
     {
       if (label_mat_(i, j) > 0 || ground_mat_(i, j) == 1)
@@ -167,18 +172,14 @@ void ImageProjection::pcCB(const sensor_msgs::PointCloud2ConstPtr &msg)
           }
         }
 
-        seg_info_msg_.segmentedCloudGroundFlag[line_size] = (ground_mat_(i, j) == 1);
-        seg_info_msg_.segmentedCloudColInd[line_size] = j;
-        seg_info_msg_.segmentedCloudRange[line_size] = range_mat_(i, j);
+        seg_info_msg_->segmentedCloudGroundFlag[line_size] = (ground_mat_(i, j) == 1);
+        seg_info_msg_->segmentedCloudColInd[line_size] = j;
+        seg_info_msg_->segmentedCloudRange[line_size] = range_mat_(i, j);
         segmented_cloud_->points.push_back(full_cloud_->points[j + i * Horizon_SCAN]);
         ++line_size;
       }
-      else if (label_mat_(i, j) == 0 && range_mat_(i, j) != std::numeric_limits<float>::max())
-      {
-        ROS_WARN("error");
-      }
     }
-    seg_info_msg_.endRingIndex[i] = line_size - 1 - 5;
+    seg_info_msg_->endRingIndex[i] = line_size - 1 - 5;
   }
   publish();
 
@@ -195,7 +196,7 @@ void ImageProjection::pcCB(const sensor_msgs::PointCloud2ConstPtr &msg)
   nan_p.intensity = -1;
   std::fill(full_cloud_->points.begin(), full_cloud_->points.end(), nan_p);
 
-  std::cout << "image projection time: " << t_whole.toc() << " ms" << std::endl;
+  // std::cout << "image projection time: " << t_whole.toc() << " ms" << std::endl;
 }
 
 void ImageProjection::labelComponents(int row, int col)
@@ -314,14 +315,14 @@ void ImageProjection::publish()
   }
   if (pub_segmented_cloud_.getNumSubscribers() > 0)
   {
-    pcl::toROSMsg(*segmented_cloud_, segmented_cloud_msg_);
-    segmented_cloud_msg_.header = seg_info_msg_.header;
+    pcl::toROSMsg(*segmented_cloud_, *segmented_cloud_msg_);
+    segmented_cloud_msg_->header = seg_info_msg_->header;
     pub_segmented_cloud_.publish(segmented_cloud_msg_);
   }
   if (pub_outlier_.getNumSubscribers() > 0)
   {
-    pcl::toROSMsg(*outlier_cloud_, outlier_cloud_msg_);
-    outlier_cloud_msg_.header = seg_info_msg_.header;
+    pcl::toROSMsg(*outlier_cloud_, *outlier_cloud_msg_);
+    outlier_cloud_msg_->header = seg_info_msg_->header;
     pub_outlier_.publish(outlier_cloud_msg_);
   }
 }