Removed active/passive modi again...

...as they may be confusing and it's not clear yet how the potential offline version is supposed to work.

src/lib/LaserOdometry.cpp

@@ -104,9 +104,6 @@ bool LaserOdometry::setup(ros::NodeHandle &node,
   _subImuTrans = node.subscribe<sensor_msgs::PointCloud2>
       ("/imu_trans", 5, &LaserOdometry::imuTransHandler, this);
 
-  // set active mode
-  _activeMode = true;
-
   return true;
 }
 
@@ -344,10 +341,6 @@ void LaserOdometry::imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuT
 
 void LaserOdometry::spin()
 {
-  if (!_activeMode) {
-    return;
-  }
-
   ros::Rate rate(100);
   bool status = ros::ok();
 
@@ -819,9 +812,7 @@ void LaserOdometry::process()
   _laserOdometry.pose.pose.position.x = trans.x();
   _laserOdometry.pose.pose.position.y = trans.y();
   _laserOdometry.pose.pose.position.z = trans.z();
-  if (_activeMode) {
-    _pubLaserOdometry.publish(_laserOdometry);
-  }
+  _pubLaserOdometry.publish(_laserOdometry);
 
   _laserOdometryTrans.stamp_ = ros::Time().fromSec(_timeSurfPointsLessFlat);
   _laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
@@ -849,36 +840,31 @@ void LaserOdometry::process()
 
 void LaserOdometry::publishResult()
 {
-  // only publish messages in active mode
-  if (!_activeMode) {
-    // transform each full resolution cloud in passive mode as we don't know who will be using it
-    transformToEnd(_laserCloud);
+  // publish results according to the input output ratio
+  if (_ioRatio > 1 && _frameCount % _ioRatio != 1) {
     return;
   }
 
-  // publish results according to the input output ratio
-  if (_ioRatio < 2 || _frameCount % _ioRatio == 1) {
-    ros::Time sweepTime = ros::Time().fromSec(_timeSurfPointsLessFlat);
-
-    sensor_msgs::PointCloud2 laserCloudCornerLast2;
-    pcl::toROSMsg(*_lastCornerCloud, laserCloudCornerLast2);
-    laserCloudCornerLast2.header.stamp = sweepTime;
-    laserCloudCornerLast2.header.frame_id = "/camera";
-    _pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
-
-    sensor_msgs::PointCloud2 laserCloudSurfLast2;
-    pcl::toROSMsg(*_lastSurfaceCloud, laserCloudSurfLast2);
-    laserCloudSurfLast2.header.stamp = sweepTime;
-    laserCloudSurfLast2.header.frame_id = "/camera";
-    _pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
-
-    sensor_msgs::PointCloud2 laserCloudFullRes3;
-    transformToEnd(_laserCloud);  // transform full resolution cloud before sending it
-    pcl::toROSMsg(*_laserCloud, laserCloudFullRes3);
-    laserCloudFullRes3.header.stamp = sweepTime;
-    laserCloudFullRes3.header.frame_id = "/camera";
-    _pubLaserCloudFullRes.publish(laserCloudFullRes3);
-  }
+  ros::Time sweepTime = ros::Time().fromSec(_timeSurfPointsLessFlat);
+
+  sensor_msgs::PointCloud2 laserCloudCornerLast2;
+  pcl::toROSMsg(*_lastCornerCloud, laserCloudCornerLast2);
+  laserCloudCornerLast2.header.stamp = sweepTime;
+  laserCloudCornerLast2.header.frame_id = "/camera";
+  _pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
+
+  sensor_msgs::PointCloud2 laserCloudSurfLast2;
+  pcl::toROSMsg(*_lastSurfaceCloud, laserCloudSurfLast2);
+  laserCloudSurfLast2.header.stamp = sweepTime;
+  laserCloudSurfLast2.header.frame_id = "/camera";
+  _pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
+
+  sensor_msgs::PointCloud2 laserCloudFullRes3;
+  transformToEnd(_laserCloud);  // transform full resolution cloud before sending it
+  pcl::toROSMsg(*_laserCloud, laserCloudFullRes3);
+  laserCloudFullRes3.header.stamp = sweepTime;
+  laserCloudFullRes3.header.frame_id = "/camera";
+  _pubLaserCloudFullRes.publish(laserCloudFullRes3);
 }
 
 } // end namespace loam

src/lib/LaserOdometry.h

@@ -52,10 +52,10 @@ namespace loam {
  */
 class LaserOdometry {
 public:
-  LaserOdometry(const float& scanPeriod,
-                const uint16_t& ioRatio = 2);
+  explicit LaserOdometry(const float& scanPeriod,
+                         const uint16_t& ioRatio = 2);
 
-  /** \brief Setup component in active mode.
+  /** \brief Setup component.
    *
    * @param node the ROS node handle
    * @param privateNode the private ROS node handle
@@ -196,8 +196,6 @@ class LaserOdometry {
   Vector3 _imuShiftFromStart;
   Vector3 _imuVeloFromStart;
 
-  bool _activeMode;   ///< active or passive mode (in active mode, results are published via ros topics, in passive mode not)
-
   ros::Publisher _pubLaserCloudCornerLast;  ///< last corner cloud message publisher
   ros::Publisher _pubLaserCloudSurfLast;    ///< last surface cloud message publisher
   ros::Publisher _pubLaserCloudFullRes;     ///< full resolution cloud message publisher

src/lib/ScanRegistration.cpp

@@ -59,8 +59,7 @@ ScanRegistration::ScanRegistration(const float& scanPeriod,
         _regionCurvature(),
         _regionLabel(),
         _regionSortIndices(),
-        _scanNeighborPicked(),
-        _activeMode(false)
+        _scanNeighborPicked()
 {
   _scanStartIndices.assign(nScans, 0);
   _scanEndIndices.assign(nScans, 0);
@@ -88,19 +87,6 @@ bool ScanRegistration::setup(ros::NodeHandle& node,
   _pubSurfPointsLessFlat = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2);
   _pubImuTrans = node.advertise<sensor_msgs::PointCloud2>("/imu_trans", 5);
 
-  // set active mode
-  _activeMode = true;
-
-  return true;
-}
-
-
-
-bool ScanRegistration::setup(const RegistrationParams& config)
-{
-  _config = config;
-  _config.print();
-
   return true;
 }
 
@@ -478,11 +464,6 @@ void ScanRegistration::generateROSMsg(sensor_msgs::PointCloud2& msg,
 
 void ScanRegistration::publishResult()
 {
-  if (!_activeMode) {
-    // only publish messages in active mode
-    return;
-  }
-
   // publish full resolution and feature point clouds
   sensor_msgs::PointCloud2 laserCloudOutMsg;
   generateROSMsg(laserCloudOutMsg, _laserCloud);

src/lib/ScanRegistration.h

@@ -254,25 +254,19 @@ typedef struct IMUState {
  */
 class ScanRegistration {
 public:
-  ScanRegistration(const float& scanPeriod,
-                   const uint16_t& nScans = 0,
-                   const size_t& imuHistorySize = 200,
-                   const RegistrationParams& config = RegistrationParams());
+  explicit ScanRegistration(const float& scanPeriod,
+                            const uint16_t& nScans = 0,
+                            const size_t& imuHistorySize = 200,
+                            const RegistrationParams& config = RegistrationParams());
 
-  /** \brief Setup component in active mode.
+  /** \brief Setup component.
    *
    * @param node the ROS node handle
    * @param privateNode the private ROS node handle
    */
   virtual bool setup(ros::NodeHandle& node,
                      ros::NodeHandle& privateNode);
 
-  /** \brief Setup component in passive mode.
-   *
-   * @param config the scan registration configuration parameters
-   */
-  virtual bool setup(const RegistrationParams& config);
-
   /** \brief Handler method for IMU messages.
    *
    * @param imuIn the new IMU message
@@ -393,8 +387,6 @@ class ScanRegistration {
   std::vector<size_t> _regionSortIndices;   ///< sorted region indices based on point curvature
   std::vector<int> _scanNeighborPicked;     ///< flag if neighboring point was already picked
 
-  bool _activeMode;   ///< active or passive mode (in active mode, results are published via ros topics, in passive mode not)
-
   ros::Subscriber _subImu;    ///< IMU message subscriber
 
   ros::Publisher _pubLaserCloud;              ///< full resolution cloud message publisher

src/lib/TransformMaintenance.cpp

@@ -67,9 +67,6 @@ bool TransformMaintenance::setup(ros::NodeHandle &node, ros::NodeHandle &private
   _subOdomAftMapped = node.subscribe<nav_msgs::Odometry>
       ("/aft_mapped_to_init", 5, &TransformMaintenance::odomAftMappedHandler, this);
 
-  // set active mode
-  _activeMode = true;
-
   return true;
 }
 
@@ -201,11 +198,7 @@ void TransformMaintenance::laserOdometryHandler(const nav_msgs::Odometry::ConstP
   _laserOdometry2.pose.pose.position.x = _transformMapped[3];
   _laserOdometry2.pose.pose.position.y = _transformMapped[4];
   _laserOdometry2.pose.pose.position.z = _transformMapped[5];
-
-  if (_activeMode) {
-    // only publish messages in active mode
-    _pubLaserOdometry2.publish(_laserOdometry2);
-  }
+  _pubLaserOdometry2.publish(_laserOdometry2);
 
   _laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;
   _laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));

src/lib/TransformMaintenance.h

@@ -47,7 +47,7 @@ class TransformMaintenance {
 public:
   TransformMaintenance();
 
-  /** \brief Setup component in active mode.
+  /** \brief Setup component.
    *
    * @param node the ROS node handle
    * @param privateNode the private ROS node handle
@@ -87,8 +87,6 @@ class TransformMaintenance {
 
   ros::Subscriber _subLaserOdometry;    ///< (high frequency) laser odometry subscriber
   ros::Subscriber _subOdomAftMapped;    ///< (low frequency) mapping odometry subscriber
-
-  bool _activeMode;   ///< active or passive mode (in active mode, results are published via ros topics, in passive mode not)
 };
 
 } // end namespace loam