Merge pull request #193 from thandal/gnss_cleanup

minor gnss cleanup
stereolabs · Nov 10, 2023 · d1cfd5b · d1cfd5b
2 parents b02b455 + 2e0c629
commit d1cfd5b
Showing 1 changed file with 13 additions and 9 deletions.
diff --git a/zed_components/src/zed_camera/src/zed_camera_component.cpp b/zed_components/src/zed_camera/src/zed_camera_component.cpp
@@ -3992,7 +3992,7 @@ bool ZedCamera::startCamera()
     mZed.startPublishing(mFusionConfig->communication_parameters);
     DEBUG_GNSS(" Camera publishing OK");
 
-    // Fusion subscrive to camera data
+    // Fusion subscribe to camera data
     fus_err = mFusion.subscribe(
       mCamUuid, mFusionConfig->communication_parameters,
       mFusionConfig->pose);
@@ -5172,7 +5172,8 @@ void ZedCamera::threadFunc_zedGrab()
         // Process Fusion data
         mFusionStatus = mFusion.process();
         // ----> Fusion errors?
-        if (mFusionStatus != sl::FUSION_ERROR_CODE::SUCCESS) {
+        if (mFusionStatus != sl::FUSION_ERROR_CODE::SUCCESS &&
+            mFusionStatus != sl::FUSION_ERROR_CODE::NO_NEW_DATA_AVAILABLE) {
           RCLCPP_ERROR_STREAM(
             get_logger(),
             "Fusion error: " << sl::toString(mFusionStatus).c_str());
@@ -5204,7 +5205,7 @@ void ZedCamera::threadFunc_zedGrab()
             "DeltaT: " << dT_sec << " sec [" <<
               std::fixed << std::setprecision(9) <<
               static_cast<float>(real_frame_ts.nanoseconds()) / 1e9 << "-" <<
-              static_cast<float>(mGnssTimestamp.nanoseconds()) / 1e9);
+              static_cast<float>(mGnssTimestamp.nanoseconds()) / 1e9 << "]");
 
           if (dT_sec < 0.0) {
             RCLCPP_WARN_STREAM(
@@ -6869,7 +6870,7 @@ void ZedCamera::processGeoPose()
   {
     rclcpp::Clock steady_clock(RCL_STEADY_TIME);
     RCLCPP_DEBUG_THROTTLE(
-      get_logger(), steady_clock, 1.0, "Waiting for a valid GNSS fused pose...");
+      get_logger(), steady_clock, 1000.0, "Waiting for a valid GNSS fused pose...");
     return;
   }
 
@@ -8883,7 +8884,6 @@ void ZedCamera::callback_gnssFix(const sensor_msgs::msg::NavSatFix::SharedPtr ms
   gnssData.ts.setNanoseconds(ts_gnss_nsec);
   gnssData.setCoordinates(latit, longit, altit, false);
 
-
   if (msg->position_covariance_type != sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN) {
 
     // TODO(Walter) Handle the new covariance mode in the SDK
@@ -8911,10 +8911,14 @@ void ZedCamera::callback_gnssFix(const sensor_msgs::msg::NavSatFix::SharedPtr ms
   mGnssFixValid = true; // Used to keep track of signal loss
 
   if (mZed.isOpened() && mZed.isPositionalTrackingEnabled() ) {
-    DEBUG_STREAM_GNSS(
-      "Datum ingested - [" << mGnssTimestamp.nanoseconds() << " nsec] " << latit << "°," << longit << "° / " << altit <<
-        " m");
-    mFusion.ingestGNSSData(gnssData);
+    auto ingest_error = mFusion.ingestGNSSData(gnssData);
+    if (ingest_error == sl::FUSION_ERROR_CODE::SUCCESS){
+        DEBUG_STREAM_GNSS("Datum ingested - [" << mGnssTimestamp.nanoseconds() << " nsec] " << latit << "°," << longit << "° / " << altit << " m");
+    } else {
+    	RCLCPP_ERROR_STREAM(
+    	  get_logger(),
+          "Ingest error occurred when ingesting GNSSData: " << ingest_error);
+    }
     mGnssFixNew = true;
   }
 }