diff --git a/zed_components/src/zed_camera/src/zed_camera_component.cpp b/zed_components/src/zed_camera/src/zed_camera_component.cpp index a81a749f..0649aa13 100644 --- 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(real_frame_ts.nanoseconds()) / 1e9 << "-" << - static_cast(mGnssTimestamp.nanoseconds()) / 1e9); + static_cast(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; } }