From 4818f9667a4e1224ce059529958f98a9f94c8842 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 5 Apr 2024 16:00:24 +0200 Subject: [PATCH 1/2] Release 4.1 compatibility --- CHANGELOG.rst | 8 ++ README.md | 2 +- .../include/zed_wrapper_nodelet.hpp | 2 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 85 ++++++++----------- zed_wrapper/params/common.yaml | 4 +- 5 files changed, 46 insertions(+), 55 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index b252b495..b3b4fa93 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,14 @@ CHANGELOG ========= +05-04-2024 +---------- +- Fix compatibility with ZED SDK v4.1 +- Add support to Positional Tracking Gen 2 +- The parameter `pos_tracking_mode` can accept the new values `GEN_1` and `GEN_2`, instead of `STANDARD` and `QUALITY` +- Add support to `NEURAL+` depth mode. Value `NEURAL_PLUS` for the parameter `depth_mode` +- The annoying warning `Elaboration takes longer...` is now emitted only in DEBUG mode + v4.0.8 ------ diff --git a/README.md b/README.md index c89700f6..f0e43cf2 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera ### Prerequisites - Ubuntu 20.04 -- [ZED SDK **≥ 4.0.6**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ZED SDK **v4.1**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) ### Build the repository diff --git a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp index 8d1cb5af..cc33cec5 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -636,7 +636,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet // Positional tracking bool mPosTrackingEnabled = false; - sl::POSITIONAL_TRACKING_MODE mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; + sl::POSITIONAL_TRACKING_MODE mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::GEN_2; bool mPosTrackingReady = false; bool mPosTrackingStarted = false; bool mPosTrackingRequired = false; diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index 44da2b62..5492de5e 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -957,19 +957,19 @@ void ZEDWrapperNodelet::readPosTrkParams() std::string pos_trk_mode; mNhNs.getParam("pos_tracking/pos_tracking_mode", pos_trk_mode); - if (pos_trk_mode == "QUALITY") + if (pos_trk_mode == "GEN_1") { - mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::GEN_1; } - else if (pos_trk_mode == "STANDARD") + else if (pos_trk_mode == "GEN_2") { - mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::STANDARD; + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::GEN_2; } else { NODELET_WARN_STREAM("'pos_tracking/pos_tracking_mode' not valid ('" << pos_trk_mode << "'). Using default value."); - mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::GEN_2; } NODELET_INFO_STREAM(" * Positional tracking mode\t-> " << sl::toString(mPosTrkMode).c_str()); @@ -979,8 +979,8 @@ void ZEDWrapperNodelet::readPosTrkParams() mNhNs.getParam("pos_tracking/path_pub_rate", mPathPubRate); NODELET_INFO_STREAM(" * Path rate\t\t\t-> " << mPathPubRate << " Hz"); mNhNs.getParam("pos_tracking/path_max_count", mPathMaxCount); - NODELET_INFO_STREAM(" * Path history size\t\t-> " << (mPathMaxCount == -1) ? std::string("INFINITE") : - std::to_string(mPathMaxCount)); + NODELET_INFO_STREAM(" * Path history size\t\t-> " << ((mPathMaxCount == -1) ? std::string("INFINITE") : + std::to_string(mPathMaxCount))); if (mPathMaxCount < 2 && mPathMaxCount != -1) { @@ -1023,9 +1023,6 @@ void ZEDWrapperNodelet::readPosTrkParams() NODELET_INFO_STREAM(" * Camera is static\t\t-> " << (mIsStatic ? "ENABLED" : "DISABLED")); mNhNs.getParam("pos_tracking/depth_min_range", mPosTrkMinDepth); NODELET_INFO_STREAM(" * Depth minimum range\t\t-> " << mPosTrkMinDepth); - - bool mIsStatic = false; - double mPosTrkMinDepth = 0.0; } } @@ -1490,7 +1487,7 @@ void ZEDWrapperNodelet::checkResolFps() NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 15 FPS."); mCamFrameRate = 15; } - else if (mCamFrameRate >= 23) + else { NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 30 FPS."); mCamFrameRate = 30; @@ -1568,7 +1565,7 @@ void ZEDWrapperNodelet::checkResolFps() NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Forced to 30 FPS."); mCamFrameRate = 30; } - else if (mCamFrameRate >= 45) + else { NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Forced to 60 FPS."); mCamFrameRate = 60; @@ -1634,8 +1631,7 @@ bool ZEDWrapperNodelet::getCamera2BaseTransform() { NODELET_DEBUG("Getting static TF from '%s' to '%s'", mCameraFrameId.c_str(), mBaseFrameId.c_str()); - mCamera2BaseTransfValid = false; - static bool first_error = true; + mCamera2BaseTransfValid = false; // ----> Static transforms // Sensor to Base link @@ -1658,6 +1654,7 @@ bool ZEDWrapperNodelet::getCamera2BaseTransform() } catch (tf2::TransformException& ex) { + static bool first_error = true; if (!first_error) { NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); @@ -1685,9 +1682,7 @@ bool ZEDWrapperNodelet::getSens2CameraTransform() { NODELET_DEBUG("Getting static TF from '%s' to '%s'", mDepthFrameId.c_str(), mCameraFrameId.c_str()); - mSensor2CameraTransfValid = false; - - static bool first_error = true; + mSensor2CameraTransfValid = false; // ----> Static transforms // Sensor to Camera Center @@ -1709,6 +1704,7 @@ bool ZEDWrapperNodelet::getSens2CameraTransform() } catch (tf2::TransformException& ex) { + static bool first_error = true; if (!first_error) { NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); @@ -1737,8 +1733,7 @@ bool ZEDWrapperNodelet::getSens2BaseTransform() { NODELET_DEBUG("Getting static TF from '%s' to '%s'", mDepthFrameId.c_str(), mBaseFrameId.c_str()); - mSensor2BaseTransfValid = false; - static bool first_error = true; + mSensor2BaseTransfValid = false; // ----> Static transforms // Sensor to Base link @@ -1760,6 +1755,7 @@ bool ZEDWrapperNodelet::getSens2BaseTransform() } catch (tf2::TransformException& ex) { + static bool first_error = true; if (!first_error) { NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); @@ -2011,7 +2007,6 @@ bool ZEDWrapperNodelet::start_obj_detect() sl::ObjectDetectionParameters od_p; od_p.enable_segmentation = false; od_p.enable_tracking = mObjDetTracking; - od_p.image_sync = true; od_p.detection_model = mObjDetModel; od_p.filtering_mode = mObjFilterMode; od_p.prediction_timeout_s = mObjDetPredTimeout; @@ -2332,11 +2327,6 @@ void ZEDWrapperNodelet::publishPose() size_t poseSub = mPubPose.getNumSubscribers(); size_t poseCovSub = mPubPoseCov.getNumSubscribers(); - tf2::Transform base_pose; - base_pose.setIdentity(); - - base_pose = mMap2BaseTransf; - std_msgs::Header header; header.stamp = mFrameTimestamp; header.frame_id = mMapFrameId; // frame @@ -2628,11 +2618,10 @@ void ZEDWrapperNodelet::callback_pubFusedPointCloud(const ros::TimerEvent& e) resized = true; } - std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); + //std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); // NODELET_INFO_STREAM("Chunks: " << mFusedPC.chunks.size()); - int index = 0; float* ptCloudPtr = (float*)(&pointcloudFusedMsg->data[0]); int updated = 0; @@ -2655,13 +2644,9 @@ void ZEDWrapperNodelet::callback_pubFusedPointCloud(const ros::TimerEvent& e) pointcloudFusedMsg->header.stamp = sl_tools::slTime2Ros(mFusedPC.chunks[c].timestamp); } } - else - { - index += mFusedPC.chunks[c].vertices.size(); - } } - std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now(); + //std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now(); // NODELET_INFO_STREAM("Updated: " << updated); @@ -2770,7 +2755,6 @@ void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInf zedParam = zed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters; - float baseline = zedParam.getCameraBaseline(); depth_info_msg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; depth_info_msg->D.resize(5); depth_info_msg->D[0] = zedParam.left_cam.disto[0]; // k1 @@ -2922,7 +2906,7 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, if (config.auto_exposure_gain != mCamAutoExposure) { mCamAutoExposure = config.auto_exposure_gain; - NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << mCamAutoExposure ? "ENABLED" : "DISABLED"); + NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << (mCamAutoExposure ? "ENABLED" : "DISABLED")); } mDynParMutex.unlock(); // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); @@ -2960,7 +2944,7 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, if (config.auto_whitebalance != mCamAutoWB) { mCamAutoWB = config.auto_whitebalance; - NODELET_INFO_STREAM("Reconfigure auto white balance: " << mCamAutoWB ? "ENABLED" : "DISABLED"); + NODELET_INFO_STREAM("Reconfigure auto white balance: " << (mCamAutoWB ? "ENABLED" : "DISABLED")); mTriggerAutoWB = true; } else @@ -3033,8 +3017,8 @@ void ZEDWrapperNodelet::pubVideoDepth() sl::Mat mat_right_gray, mat_right_raw_gray; sl::Mat mat_depth, mat_disp, mat_conf; - sl::Timestamp ts_rgb = 0; // used to check RGB/Depth sync - sl::Timestamp ts_depth = 0; // used to check RGB/Depth sync + sl::Timestamp ts_rgb = 0; // used to check RGB/Depth sync + sl::Timestamp ts_depth; // used to check RGB/Depth sync sl::Timestamp grab_ts = 0; // ----> Retrieve all required image data @@ -4054,7 +4038,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { mStopNode = true; - std::lock_guard lock(mCloseZedMutex); + std::lock_guard stop_lock(mCloseZedMutex); NODELET_INFO_STREAM("Closing ZED " << mZedSerialNumber << "..."); if (mRecording) { @@ -4205,20 +4189,20 @@ void ZEDWrapperNodelet::device_poll_thread_func() double elab_usec = std::chrono::duration_cast(end_elab - start_elab).count(); - double mean_elab_sec = mElabPeriodMean_sec->addValue(elab_usec / 1000000.); - - static int count_warn = 0; + double mean_elab_sec = mElabPeriodMean_sec->addValue(elab_usec / 1000000.); if (!loop_rate.sleep()) { + static int count_warn = 0; if (mean_elab_sec > (1. / mPubFrameRate)) { + if (++count_warn > 10) { NODELET_DEBUG_THROTTLE(1.0, "Working thread is not synchronized with the Camera grab rate"); NODELET_DEBUG_STREAM_THROTTLE(1.0, "Expected cycle time: " << loop_rate.expectedCycleTime() << " - Real cycle time: " << mean_elab_sec); - NODELET_WARN_STREAM_THROTTLE(10.0, "Elaboration takes longer (" + NODELET_DEBUG_STREAM_THROTTLE(10.0, "Elaboration takes longer (" << mean_elab_sec << " sec) than requested by the FPS rate (" << loop_rate.expectedCycleTime() << " sec). Please consider to " @@ -4290,7 +4274,7 @@ void ZEDWrapperNodelet::processPointcloud(ros::Time ts) void ZEDWrapperNodelet::processCameraSettings() { - int value; + sl::VIDEO_SETTINGS setting; sl::ERROR_CODE err; @@ -4299,6 +4283,7 @@ void ZEDWrapperNodelet::processCameraSettings() // NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK"); mDynParMutex.lock(); + int value; setting = sl::VIDEO_SETTINGS::BRIGHTNESS; err = mZed.getCameraSettings(setting, value); if (err == sl::ERROR_CODE::SUCCESS && value != mCamBrightness) @@ -4540,8 +4525,8 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic if (mPcPublishing) { - double freq = 1000000. / mPcPeriodMean_usec->getMean(); - double freq_perc = 100. * freq / mPointCloudFreq; + freq = 1000000. / mPcPeriodMean_usec->getMean(); + freq_perc = 100. * freq / mPointCloudFreq; stat.addf("Point Cloud", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); } else @@ -4573,8 +4558,8 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic if (mObjDetRunning) { - double freq = 1000. / mObjDetPeriodMean_msec->getMean(); - double freq_perc = 100. * freq / mPubFrameRate; + freq = 1000. / mObjDetPeriodMean_msec->getMean(); + freq_perc = 100. * freq / mPubFrameRate; stat.addf("Object detection", "Mean Frequency: %.3f Hz (%.1f%%)", freq, freq_perc); } else @@ -4908,7 +4893,8 @@ bool ZEDWrapperNodelet::on_set_roi(zed_interfaces::set_roi::Request& req, zed_in // Create mask NODELET_INFO(" * Setting ROI"); std::vector sl_poly; - std::string log_msg = parseRoiPoly(parsed_poly, sl_poly); + //std::string log_msg = + parseRoiPoly(parsed_poly, sl_poly); // NODELET_INFO_STREAM(" * Parsed ROI: " << log_msg.c_str()); sl::Resolution resol(mCamWidth, mCamHeight); sl::Mat roi_mask(resol, sl::MAT_TYPE::U8_C1, sl::MEM::CPU); @@ -5644,7 +5630,6 @@ void ZEDWrapperNodelet::processPose() tr_2d.setZ(mFixedZValue); mMap2BaseTransf.setOrigin(tr_2d); - double roll, pitch, yaw; tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); tf2::Quaternion quat_2d; @@ -5660,8 +5645,6 @@ void ZEDWrapperNodelet::processPose() mBaseFrameId.c_str(), mMap2BaseTransf.getOrigin().x(), mMap2BaseTransf.getOrigin().y(), mMap2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - bool initOdom = false; - // Transformation from map to odometry frame mOdomMutex.lock(); // mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index f2c752bb..1c206c79 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -40,13 +40,13 @@ general: #video: depth: - depth_mode: 'ULTRA' # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' + depth_mode: 'NEURAL_PLUS' # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', `NEURAL_PLUS` depth_stabilization: 1 # [0-100] - 0: Disabled openni_depth_mode: false # 'false': 32bit float meter units, 'true': 16bit uchar millimeter units pos_tracking: pos_tracking_enabled: true # True to enable positional tracking from start - pos_tracking_mode: 'STANDARD' # Matches the ZED SDK setting: 'QUALITY', 'STANDARD' -> Use 'QUALITY' with 'ULTRA' depth mode for improved outdoors performance + pos_tracking_mode: 'GEN_2' # Matches the ZED SDK setting: 'GEN_1', 'GEN_2' set_gravity_as_origin: true # If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose. imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. publish_tf: true # publish `odom -> base_link` TF From aa913d21c2cf32bf467a9b9007a6d21ce2731b01 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 5 Apr 2024 16:04:03 +0200 Subject: [PATCH 2/2] Fix default params --- zed_wrapper/params/common.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 1c206c79..ad73a711 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -40,7 +40,7 @@ general: #video: depth: - depth_mode: 'NEURAL_PLUS' # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', `NEURAL_PLUS` + depth_mode: 'ULTRA' # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', `NEURAL_PLUS` depth_stabilization: 1 # [0-100] - 0: Disabled openni_depth_mode: false # 'false': 32bit float meter units, 'true': 16bit uchar millimeter units @@ -75,7 +75,7 @@ mapping: sensors: sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame - max_pub_rate: 400. # max frequency of publishing of sensors data. MAX: 400. - MIN: grab rate + max_pub_rate: 200. # max frequency of publishing of sensors data. MAX: 400. - MIN: grab rate publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: