reduced verbosity; disabled feature

source/RobotComponents/libraries/cartographer/ApproximateTimeQueue.cpp

@@ -25,6 +25,11 @@ namespace armarx::cartographer
         messageCallee(messageCallee),
         useOdometry(useOdometry)
     {
+        if(dt > 0)
+        {
+            ARMARX_ERROR << "Requested to throttle processing of data. This is currently not available.";
+        }
+
         const std::size_t laserMaxQueueSize = 10;
 
         ARMARX_TRACE;
@@ -47,7 +52,7 @@ namespace armarx::cartographer
         auto entry = laserQueues.find(message.frame);
         if (entry == laserQueues.end())
         {
-            ARMARX_WARNING << "Received laser data for unknown frame " << message.frame;
+            ARMARX_INFO << "Received laser data for unknown frame `" << message.frame << "`.";
             return;
         }
         LaserTimeQueue& queue = entry->second;
@@ -59,7 +64,7 @@ namespace armarx::cartographer
         }
         catch (const armarx::LocalException& e)
         {
-            ARMARX_WARNING << "Unable to insert message into queue. Reason: " << e.what();
+            ARMARX_INFO << "Unable to insert message into queue. Reason: " << e.what();
             queue.clear();
         }
     }
@@ -83,7 +88,6 @@ namespace armarx::cartographer
 
         if (useOdometry and not hasTimestamp(odomQueue))
         {
-            // ARMARX_INFO << "odom data missing";
             return false;
         }
 
@@ -92,8 +96,6 @@ namespace armarx::cartographer
         {
             if (not hasTimestamp(sensorQueue))
             {
-                // ARMARX_INFO << sensorName << " data missing";
-
                 return false;
             }
         }
@@ -126,7 +128,6 @@ namespace armarx::cartographer
             std::copy(ts.begin(), ts.end(), std::inserter(timestamps, timestamps.end()));
         };
 
-        // insertTimestamps(odomQueue.timestamps());
 
         for (const auto& [_, q] : laserQueues)
         {
@@ -152,16 +153,17 @@ namespace armarx::cartographer
 
         if (dt < 0)
         {
-            ARMARX_DEBUG << "Trying to process a timestamp that is " << dt
-                         << " older than last processed timestamp.";
+            ARMARX_VERBOSE << "Trying to process a timestamp that is " << dt
+                           << " older than last processed timestamp.";
             return;
         }
 
         // only process data at a certain frequency
-        if (dt <= dtEps)
-        {
-            return;
-        }
+        // WARNING: this feature is broken. 
+        // if (dt <= dtEps)
+        // {
+        //     return;
+        // }
 
         // process no messages that are too old
         // trimUntil(timestamp - dtHistoryLength);
@@ -181,7 +183,7 @@ namespace armarx::cartographer
             queueSizes["laser_" + sensorName] = sensorQueue.size();
         }
 
-        ARMARX_INFO << deactivateSpam(1) << "queue sizes: " << queueSizes;
+        ARMARX_VERBOSE << deactivateSpam(1) << "queue sizes: " << queueSizes;
 
         lastProcessedTimestamp = timestamp;
 

source/RobotComponents/libraries/cartographer/ArVizDrawer.cpp

@@ -242,7 +242,8 @@ namespace armarx::cartographer
 
         if (laserData.points.points.empty())
         {
-            ARMARX_WARNING << "Empty point cloud received";
+            ARMARX_INFO << "Empty point cloud received";
+            return;
         }
 
         auto laserScannerData = laserData; // copy

source/RobotComponents/libraries/cartographer/CartographerAdapter.cpp

@@ -266,7 +266,7 @@ namespace armarx::cartographer
             ARMARX_TRACE;
             if (odomPose.timestamp <= lastOdomTimestamp.value())
             {
-                ARMARX_WARNING
+                ARMARX_INFO
                     << "Requested to add old data into odom queue. Not willing to do this. It is " 
                     << (odomPose.timestamp - lastOdomTimestamp.value()) << "ms in the past.";
                 return;
@@ -343,7 +343,7 @@ namespace armarx::cartographer
 
                     if (data.timestamp <= lastTimestamp)
                     {
-                        ARMARX_WARNING << "Requested to add old data with timestamp "
+                        ARMARX_INFO << "Requested to add old data with timestamp "
                                        << data.timestamp << " into laser queue for sensor "
                                        << data.frame << ". Not willing to do this. It is " 
                                        << (data.timestamp - lastTimestamp) << "ms in the past.";
@@ -364,7 +364,7 @@ namespace armarx::cartographer
                     robot_T_sensor = sensor.pose;
                 } catch (const armarx::LocalException&)
                 {
-                    ARMARX_WARNING << "Sensor " << data.frame << " not found in sensor set";
+                    ARMARX_INFO << "Sensor `" << data.frame << "` not found in sensor set.";
                     return;
                 }
 
@@ -418,7 +418,7 @@ namespace armarx::cartographer
             auto* trajBuilder = mapBuilder->GetTrajectoryBuilder(trajectoryId);
             if (trajBuilder == nullptr)
             {
-                ARMARX_WARNING << "no traj builder";
+                ARMARX_WARNING << "No trajectory builder.";
                 return;
             }
 
@@ -432,7 +432,7 @@ namespace armarx::cartographer
             {
                 if (laserScannerInsertCartoTimestamp <= lastLaserScannerInsertCartoTimestamp)
                 {
-                    ARMARX_WARNING
+                    ARMARX_INFO
                         << "Received laser scanner data out of order. Will not handle it.";
                     return;
                 }

source/RobotComponents/libraries/cartographer/MessageQueue.h

@@ -145,7 +145,7 @@ namespace armarx
                     }
                 }
 
-                ARMARX_INFO << deactivateSpam(1) << "Message queue size is " << queueSize;
+                ARMARX_VERBOSE << deactivateSpam(1) << "Message queue size is " << queueSize;
             }
             cv.notify_all(); // for run() and waitUntilProcessed()
         }

source/RobotComponents/libraries/cartographer/map_registration/LaserScanAggregator.cpp

@@ -35,8 +35,8 @@ namespace armarx
 
         if (not robotPoseQueue.has(timestamp.toMicroSeconds()))
         {
-            ARMARX_WARNING << "Cannot lookup pose for timestamp " << timestamp;
-            return;;
+            ARMARX_INFO << "Cannot lookup pose for timestamp " << timestamp;
+            return;
         }
 
         const Eigen::Affine3f map_T_robot = robotPoseQueue.lookupInterpolate(timestamp.toMicroSeconds())->pose;
@@ -71,4 +71,4 @@ namespace armarx
 
 
 
-} // namespace armarx
\ No newline at end of file
+} // namespace armarx